Even with the trunk version, the assert still occurs.
Below the source code how to reproduce this situation is attached.
I had to update the code to get it running with the trunk version.
package org.prt.simulator;
import java.util.ArrayList;
import org.jagatoo.input.InputSystem;
import org.jagatoo.input.devices.components.Key;
import org.jagatoo.input.events.KeyPressedEvent;
import org.jagatoo.input.events.KeyReleasedEvent;
import org.openmali.types.twodee.Sized2iRO;
import org.openmali.vecmath2.Colorf;
import org.openmali.vecmath2.Matrix3f;
import org.openmali.vecmath2.Point3f;
import org.openmali.vecmath2.Tuple3f;
import org.openmali.vecmath2.Vector3f;
import org.xith3d.base.Xith3DEnvironment;
import org.xith3d.loop.CanvasFPSListener;
import org.xith3d.loop.InputAdapterRenderLoop;
import org.xith3d.loop.opscheduler.OperationScheduler;
import org.xith3d.physics.PhysicsEngine;
import org.xith3d.physics.PhysicsGFXManager;
import org.xith3d.physics.collision.CollideableGroup;
import org.xith3d.physics.collision.CollisionEngine;
import org.xith3d.physics.collision.CollisionResolver;
import org.xith3d.physics.collision.collideable.BoxCollideable;
import org.xith3d.physics.collision.collideable.PlaneCollideable;
import org.xith3d.physics.joode.JoodePhysicsEngine;
import org.xith3d.physics.simulation.Body;
import org.xith3d.physics.simulation.SimulationEngine;
import org.xith3d.physics.simulation.SimulationWorld;
import org.xith3d.render.Canvas3D;
import org.xith3d.render.Canvas3DFactory;
import org.xith3d.render.config.CanvasConstructionInfo;
import org.xith3d.render.config.DisplayMode.FullscreenMode;
import org.xith3d.scenegraph.BranchGroup;
import org.xith3d.ui.hud.HUD;
import org.xith3d.ui.hud.widgets.Label;
import org.xith3d.ui.text2d.TextAlignment;
/**
* Simulator class. Simple simulator for PRT. Can be extended in the near
* future. Used units are in S.I. units (meters etc.)
*/
public class Simulator extends InputAdapterRenderLoop {
private PhysicsEngine physicsEngine;
private ArrayList<Body> movingObjects = new ArrayList<Body>();
private ArrayList<Point3f> originalPositions = new ArrayList<Point3f>();
private ArrayList<Vector3f> originalVelocities = new ArrayList<Vector3f>();
private RobotBody robotBody;
// Definitions
private final float fieldWidth = 6.0f;
private final float fieldLength = 9.0f;
private final float halfFieldWidth = this.fieldWidth / 2.0f;
private final float halfFieldLength = this.fieldLength / 2.0f;
private final int robotCount = 6; // at least 1
private float linearVelocity = 1.0f;
float angle = 0.0f;
float MAX_LINEAR_VELOCITY = 6.0f;
@Override
public void onKeyPressed(KeyPressedEvent e, Key key) {
switch (key.getKeyID()) {
case NUMPAD_ADD:
linearVelocity = Math.min(this.linearVelocity * 1.05f, MAX_LINEAR_VELOCITY);
System.out.println("key +: linear velocity set to " + linearVelocity);
break;
case NUMPAD_SUBTRACT:
linearVelocity = Math.max(linearVelocity / 1.05f, 0);
System.out.println("key -: linear velocity set to " + linearVelocity);
break;
case _0:
case NUMPAD0:
linearVelocity = 0.0f;
robotBody.getBody().setLinearVelocity(0.0f, 0.0f, 0.0f);
robotBody.getBody().setAngularVelocity(0.0f, 0.0f, 0.0f);
System.out.println("0: everything stopped");
break;
case BACK_SPACE:
robotBody.getBody().setAngularVelocity(0.0f, 0.0f, 0.0f);
System.out.println("BS: rotation stopped");
break;
case UP:
if (this.physicsEngine.isEnabled()) {
Vector3f newLinearVelocity = (Vector3f) this.robotBody.getDirection().mul(this.robotBody.getSpeed());
this.robotBody.getBody().setLinearVelocity(newLinearVelocity);
}
else {
this.physicsEngine.setEnabled(true);
}
System.out.println("Key UP: speed:" + this.robotBody.getBody().getLinearVelocity().length()
+ ", direction: " + this.robotBody.getDirection().toString());
break;
case DOWN:
if (this.physicsEngine.isEnabled()) {
float speed = this.robotBody.getSpeed();
if (speed > 0.0f) {
Vector3f newLinearVelocity = (Vector3f) this.robotBody.getBody().getLinearVelocity().mul(.5f);
this.robotBody.getBody().setLinearVelocity(newLinearVelocity);
}
}
else {
this.physicsEngine.setEnabled(true);
}
System.out.println("Key DOWN: speed:" + this.robotBody.getBody().getLinearVelocity().length()
+ ", direction: " + this.robotBody.getDirection().toString());
break;
case LEFT:
if (this.physicsEngine.isEnabled()) {
float newRotation = this.robotBody.getBody().getRotationY() + 2.0f * (float) Math.PI / 16.0f;
if (Float.isNaN(newRotation)) {
newRotation = 0.0f;
}
this.robotBody.getBody().setRotationY(newRotation);
angle = newRotation;
}
else {
this.physicsEngine.setEnabled(true);
}
System.out.println(String.format("Key left: velocity angle set to %f", this.robotBody.getBody()
.getRotationY()));
break;
case RIGHT:
if (this.physicsEngine.isEnabled()) {
float newRotation = this.robotBody.getBody().getRotationY() - 2.0f * (float) Math.PI / 16.0f;
if (Float.isNaN(newRotation)) {
newRotation = 0.0f;
}
this.robotBody.getBody().setRotationY(newRotation);
angle = newRotation;
}
else {
this.physicsEngine.setEnabled(true);
}
System.out.println(String.format("Key left: velocity angle set to %f", this.robotBody.getBody()
.getRotationY()));
break;
}
}
@Override
public void onKeyReleased(KeyReleasedEvent e, Key key) {
switch (key.getKeyID()) {
case SPACE:
if (this.physicsEngine.isEnabled()) {
for (int i = 0; i < this.movingObjects.size(); i++) {
final Body body = this.movingObjects.get(i);
Tuple3f pos = this.originalPositions.get(i);
body.setPosition(pos);
Vector3f vel = this.originalVelocities.get(i);
vel.scale(1f, 1f, 100f);
body.setLinearVelocity(vel);
// body.resetAngularVelocity();
body.setRotationMatrix(Matrix3f.IDENTITY);
}
}
else {
this.physicsEngine.setEnabled(true);
}
break;
case ESCAPE:
this.end();
break;
}
}
private void addMovingObject(Body body) {
this.movingObjects.add(body);
this.originalPositions.add(new Point3f(body.getPosition()));
this.originalVelocities.add(new Vector3f(body.getLinearVelocity()));
}
private void setupPhysicsAndScene(Xith3DEnvironment sceneGraph) {
// XPAL world creation
final PhysicsEngine physEng = new JoodePhysicsEngine();
final CollisionEngine collEng = physEng.getCollisionEngine();
final SimulationEngine simEng = physEng.getSimulationEngine();
final SimulationWorld simWorld = simEng.newWorld();
simWorld.setStepSize(1000L);
simWorld.setGravity(0f, -9.81f, 0f);
final PhysicsGFXManager gfxManager = physEng.getGFXManager();
physEng.getDefaultSurfaceParameters().setParameter("mu", 0.2f);
final BranchGroup scene = sceneGraph.addPerspectiveBranch().getBranchGroup();
CollideableGroup environment = collEng.newGroup();
// Create field ground
PlaneCollideable ground = collEng.newPlane(0f, 1f, 0f, 0f);
PlaneCollideable.setDefaultAppearance("grass.jpg");
gfxManager.directAdd(ground, scene);
environment.addCollideable(ground);
// Create the falling bricks...
Tuple3f ballPos = new Tuple3f(0.0f, 0.0f, 3.0f + 10.0f);
for (int i = 0; i < robotCount; i++) {
Tuple3f robotPos;
if (i == 0) {
robotPos = new Tuple3f(ballPos.getX() - 0.175f, ballPos.getY() - 0.1f, 0.05f + 20.0f);
}
else {
robotPos = new Tuple3f( /* x */(i / (float) robotCount) * fieldLength - halfFieldLength,
/* y */halfFieldWidth - 0.40f,
/* z */0.05f + 10.0f * (i + 1) / robotCount);
}
Body robot = createRobot(collEng, simWorld, gfxManager, scene, environment, robotPos, "Robot"
+ Integer.toString(i));
// Add collision resolvers for this robot
// i.e. one collision resolver for each previously created moving object (ball, other robots)
for (Body movingObject : this.movingObjects) {
if (movingObject != robot) {
physEng.addCollisionResolver(new CollisionResolver(robot, movingObject));
}
}
if (i == 1) {
robot.setLinearVelocity((i % 2) / 5.0f, (i % 3) / 3.0f, 0.0f);
}
}
String robotName = movingObjects.get(1).getName() + " (selected)";
movingObjects.get(1).setName(robotName);
// 0 is ball; 1 is first robot
robotBody = new RobotBody(movingObjects.get(1), physEng);
physEng.addCollisionResolver(new CollisionResolver(environment));
physEng.setEnabled(false);
sceneGraph.setPhysicsEngine(physEng);
this.physicsEngine = physEng;
}
private Body createRobot(final CollisionEngine collEng, final SimulationWorld simWorld,
final PhysicsGFXManager gfxManager, final BranchGroup scene, CollideableGroup environment,
Tuple3f position, String name) {
BoxCollideable.setDefaultAppearance("crate.png");
BoxCollideable robot = collEng.newBox(0.75f, 0.10f, 0.35f);
Body body = simWorld.newBody();
Tuple3f simPos = convertRc2Sim(position.getX(), position.getY(), position.getZ() + robot.getSize().getZ()
+ 0.01f);
body.setMass(20.0f);
body.setPosition(simPos);
body.addCollideable(robot);
body.setName(name);
simWorld.addBody(body);
scene.addChild(gfxManager.add(robot));
environment.addCollideable(robot);
addMovingObject(body);
System.out.println("Lin. vel. robot=" + body.getLinearVelocity().getX());
System.out.println("Lin. vel. moving object (robot)="
+ movingObjects.get(movingObjects.size() - 1).getLinearVelocity().getX());
return body;
}
private HUD createInfoHUD(Sized2iRO size, OperationScheduler opScheder) {
HUD hud = new HUD(size, 800f);
Label info = new Label(hud.getResX(), 40f,
"Press SPACE to (re-)start the simulation.", Colorf.WHITE,
TextAlignment.CENTER_LEFT);
info.setPadding((int) 10f);
hud.getContentPane().addWidget( info, 0f, hud.getResY() - info.getHeight() );
return (hud);
}
public Simulator(CanvasConstructionInfo canvasInfo) throws Exception {
super(128f);
System.out.println("Hit SPACE to start/reset the simulation, or ESC to exit");
Xith3DEnvironment env = new Xith3DEnvironment(5f, 5f, 5f, 0f, 0f, 0f, 0f, 1f, 0f, this);
setupPhysicsAndScene(env);
env.addHUD(createInfoHUD(canvasInfo.getDisplayMode(), getOperationScheduler()));
Canvas3D canvas = Canvas3DFactory.create(canvasInfo);
env.addCanvas(canvas);
this.addFPSListener(new CanvasFPSListener(canvas)); // get an impression of how fast we are simulating
// enable external inputs - keyboard command
InputSystem.getInstance().registerNewKeyboardAndMouse( canvas.getPeer() );
this.begin(TimingMode.MICROSECONDS);
}
static Tuple3f convertRc2Sim(float x, float y, float z) {
return new Tuple3f(x, z, y);
}
public static void main(String[] args) throws Exception {
new Simulator(new CanvasConstructionInfo(FullscreenMode.WINDOWED, true, "RoboCup Simulator"));
}
public class RobotBody {
private Body body = null;
private float speed = MAX_LINEAR_VELOCITY;
public RobotBody(Body body, PhysicsEngine physEng) {
this.body = body;
}
public float getSpeed() {
return speed;
}
public void setSpeed(float speed) {
this.speed = speed;
}
public Vector3f getDirection() {
Vector3f dir = body.getLinearVelocity();
dir.setY(0.0f); // not interested in vertical speed
if (Float.isNaN(dir.length()) || dir.length() < .001) {
if (Float.isNaN(angle)) {
angle = 0.0f;
}
dir = new Vector3f((float) Math.cos(angle), 0.0f, (float) Math.cos(angle));
}
else {
angle = body.getRotationY();
if (Float.isNaN(angle)) {
angle = 0.0f;
body.setRotation(0, 0, 0);
}
}
dir.normalize();
return dir;
}
public Body getBody() {
return this.body;
}
}
}
General Category
