// Called once a frame for movement for this object.
public void move()
{
vector3d.insertionResult result;
float l,tl; // Length variables
float[] adjust = LavaBall.getSense();
Vector3 mvel = Vector3.alloc();
Vector3 mpos = Vector3.alloc();
Vector3 temp = Vector3.alloc();
// copy position and velocity to local variables.
// FIXME - change to time based rather than frame based movement.
Vector3.copy(mpos,position);
Vector3.copy(mvel,velocity);
// Save the length of the velocity. We use later to restore the velocity.
l = Vector3.length(velocity);
// Determine if any collisions by testing against the bsp. Bsp will return a ratio
// of your movement distance for when the first insertion into the world surfaces takes
// place.
// If there is an intersection, then reflect the move and move the remaining distance.
result = World.bsp.testInsertion(Vector3.to_float(mpos), Vector3.to_float(mvel), radius,null, sequence_number++);
while(result.intersection)
{
// Shorten the movement vector by the ratio of the distance traveled.
Vector3.scale(temp, mvel, result.dist);
Vector3.add(mpos,mpos,temp);
// Subtract it from the overall movement vector. This is the movement
// left to be made this frame.
Vector3.subtract(mvel,mvel,temp);
// Reflect the velocity based on the normal of collision. Reflection is not
// perfectly elastic. The constant at the end slows the outgoing velocity.
// Since we are only operating on the local velocity vector, we need to find
// the ratio of that velocity reduction and apply it to the l variable that will
// be used later to restore the member velocity variable.
tl = Vector3.length(mvel);
Vector3.reflect(mvel,mvel,Vector3.to_Vector3(result.normal),0.3f);
l = l * (Vector3.length(mvel)/tl);
// Test for another insertion on the next bit of our movement.
result = World.bsp.testInsertion(Vector3.to_float(mpos), Vector3.to_float(mvel), radius,null, sequence_number++);
}
// Add in the last bit of non-colliding movement
Vector3.add(mpos,mpos,mvel);
// Restore velocity
Vector3.normalize(mvel,mvel);
Vector3.scale(velocity, mvel, l);
// Restore position
Vector3.copy(position,mpos);
// New acceleration based on sensor orientation
// Start with the normalized orientation acceleration.
acceleration.x=adjust[0];
acceleration.y=adjust[1];
acceleration.z=adjust[2];
// Scale it to something that makes sense for this world.
Vector3.normalize(acceleration,acceleration);
Vector3.scale(acceleration,acceleration, 0.004f);
// Update the velocity with the acceleration
Vector3.add(velocity,velocity,acceleration);
Vector3.free(); // temp
Vector3.free(); // mpos
Vector3.free(); // mvel
}