// 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 }