diff --git a/.classpath b/.classpath deleted file mode 100644 index eece5e5..0000000 --- a/.classpath +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/.project b/.project deleted file mode 100644 index e0b69c7..0000000 --- a/.project +++ /dev/null @@ -1,24 +0,0 @@ - - - atriasoft-ephysics - - - atriasoft-ephysics - - - - org.eclipse.jdt.core.javabuilder - - - - - net.sf.eclipsecs.core.CheckstyleBuilder - - - - - - org.eclipse.jdt.core.javanature - net.sf.eclipsecs.core.CheckstyleNature - - diff --git a/ephysics.iml b/ephysics.iml deleted file mode 100644 index aecc9ab..0000000 --- a/ephysics.iml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/lutin_org-atriasoft-ephysics.py b/lutin_org-atriasoft-ephysics.py index d6c46e4..368f73d 100644 --- a/lutin_org-atriasoft-ephysics.py +++ b/lutin_org-atriasoft-ephysics.py @@ -46,7 +46,7 @@ def configure(target, my_module): 'src/org/atriasoft/ephysics/mathematics/Set.java', 'src/org/atriasoft/ephysics/mathematics/Mathematics.java', 'src/org/atriasoft/ephysics/configuration/Defaults.java', - 'src/org/atriasoft/ephysics/internal/Log.java', + 'src/org/atriasoft/ephysics/internal/LOGGER.java', 'src/org/atriasoft/ephysics/engine/DynamicsWorld.java', 'src/org/atriasoft/ephysics/engine/CollisionWorld.java', 'src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java', diff --git a/out/eclipse/.gitignore b/out/eclipse/.gitignore deleted file mode 100644 index d27d8cb..0000000 --- a/out/eclipse/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -/classes/ -/__pycache__/ diff --git a/src/org/atriasoft/ephysics/body/CollisionBody.java b/src/org/atriasoft/ephysics/body/CollisionBody.java index d0b7f99..663d809 100644 --- a/src/org/atriasoft/ephysics/body/CollisionBody.java +++ b/src/org/atriasoft/ephysics/body/CollisionBody.java @@ -30,16 +30,18 @@ import org.atriasoft.ephysics.collision.ProxyShape; import org.atriasoft.ephysics.collision.shapes.AABB; import org.atriasoft.ephysics.collision.shapes.CollisionShape; import org.atriasoft.ephysics.engine.CollisionWorld; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * This class represents a body that is able to collide with others bodies. This class inherits from the Body class. * */ public class CollisionBody extends Body { + static final Logger LOGGER = LoggerFactory.getLogger(CollisionBody.class); // First element of the linked list of contact manifolds involving this body public ContactManifoldListElement contactManifoldsList; // Number of collision shapes @@ -52,7 +54,7 @@ public class CollisionBody extends Body { protected BodyType type; // Reference to the world the body belongs to protected CollisionWorld world; - + /** * Constructor * @param transform The transform of the body @@ -67,20 +69,22 @@ public class CollisionBody extends Body { this.numberCollisionShapes = 0; this.contactManifoldsList = null; this.world = world; - - //Log.debug(" set transform: " + transform); + + //LOGGER.debug(" set transform: " + transform); if (Float.isNaN(transform.getPosition().x())) { - Log.critical(" set transform: " + transform); + LOGGER.error("[CRITICAL] set transform: " + transform); + System.exit(-1); } if (Float.isInfinite(transform.getOrientation().z())) { - Log.critical(" set transform: " + transform); + LOGGER.error("[CRITICAL] set transform: " + transform); + System.exit(-1); } } - + /** * Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to * when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program. - * + * * This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the * returned proxy shape to get and set information about the corresponding collision shape for that body. * @param collisionShape A pointer to the collision shape you want to add to the body @@ -103,7 +107,7 @@ public class CollisionBody extends Body { this.numberCollisionShapes++; return proxyShape; } - + /** * Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved). */ @@ -112,7 +116,7 @@ public class CollisionBody extends Body { this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape); } } - + /** * Compute and return the AABB of the body by merging all proxy shapes AABBs * @return The axis-aligned bounding box (AABB) of the body in world-space coordinates @@ -122,20 +126,21 @@ public class CollisionBody extends Body { if (this.proxyCollisionShapes == null) { return bodyAABB; } - //Log.info("tmp this.transform : " + this.transform); - //Log.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform()); - //Log.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform())); - this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform())); + //LOGGER.info("tmp this.transform : " + this.transform); + //LOGGER.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform()); + //LOGGER.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform())); + this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, + this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform())); for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) { final AABB aabb = new AABB(); shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform())); bodyAABB.mergeWithAABB(aabb); } - //Log.info("tmp aabb : " + bodyAABB); - //Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin())); + //LOGGER.info("tmp aabb : " + bodyAABB); + //LOGGER.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin())); return bodyAABB; } - + /** * Get the first element of the linked list of contact manifolds involving this body * @return A pointer to the first element of the linked-list with the contact manifolds of this body @@ -143,7 +148,7 @@ public class CollisionBody extends Body { public ContactManifoldListElement getContactManifoldsList() { return this.contactManifoldsList; } - + /** * Get the body local-space coordinates of a point given in the world-space coordinates * @param worldPoint A point in world-space coordinates @@ -152,7 +157,7 @@ public class CollisionBody extends Body { public Vector3f getLocalPoint(final Vector3f worldPoint) { return this.transform.inverseNew().multiply(worldPoint); } - + /** * Get the body local-space coordinates of a vector given in the world-space coordinates * @param worldVector A vector in world-space coordinates @@ -161,7 +166,7 @@ public class CollisionBody extends Body { public Vector3f getLocalVector(final Vector3f worldVector) { return this.transform.getOrientation().inverse().multiply(worldVector); } - + /** * Get the linked list of proxy shapes of that body * @return The pointer of the first proxy shape of the linked-list of all the @@ -170,7 +175,7 @@ public class CollisionBody extends Body { public ProxyShape getProxyShapesList() { return this.proxyCollisionShapes; } - + /** * Return the current position and orientation * @return The current transformation of the body that transforms the local-space of the body int32to world-space @@ -178,7 +183,7 @@ public class CollisionBody extends Body { public Transform3D getTransform() { return this.transform; } - + /** * Return the type of the body * @return the type of the body (STATIC, KINEMATIC, DYNAMIC) @@ -186,11 +191,11 @@ public class CollisionBody extends Body { public BodyType getType() { return this.type; } - + public CollisionWorld getWorld() { return this.world; } - + /** * Get the world-space coordinates of a point given the local-space coordinates of the body * @param localPoint A point in the local-space coordinates of the body @@ -199,7 +204,7 @@ public class CollisionBody extends Body { public Vector3f getWorldPoint(final Vector3f localPoint) { return this.transform.multiply(localPoint); } - + /** * Get the world-space vector of a vector given in local-space coordinates of the body * @param localVector A vector in the local-space coordinates of the body @@ -208,7 +213,7 @@ public class CollisionBody extends Body { public Vector3f getWorldVector(final Vector3f localVector) { return this.transform.getOrientation().multiply(localVector); } - + /** * Raycast method with feedback information * The method returns the closest hit among all the collision shapes of the body @@ -221,7 +226,7 @@ public class CollisionBody extends Body { return false; } boolean isHit = false; - + final Ray rayTemp = new Ray(ray); for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { // Test if the ray hits the collision shape @@ -232,7 +237,7 @@ public class CollisionBody extends Body { } return isHit; } - + /** * Remove all the collision shapes */ @@ -250,7 +255,7 @@ public class CollisionBody extends Body { } this.proxyCollisionShapes = null; } - + /** * Remove a collision shape from the body * To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body @@ -286,7 +291,7 @@ public class CollisionBody extends Body { current = current.next; } } - + /** * Reset the contact manifold lists */ @@ -294,7 +299,7 @@ public class CollisionBody extends Body { // Delete the linked list of contact manifolds of that body this.contactManifoldsList = null; } - + /** * Reset the this.isAlreadyInIsland variable of the body and contact manifolds. * This method also returns the number of contact manifolds of the body. @@ -311,7 +316,7 @@ public class CollisionBody extends Body { } return nbManifolds; } - + /** * Set whether or not the body is active * @param isActive True if you want to activate the body @@ -337,23 +342,25 @@ public class CollisionBody extends Body { resetContactManifoldsList(); } } - + /** * Set the current position and orientation * @param transform The transformation of the body that transforms the local-space of the body int32to world-space */ public void setTransform(final Transform3D transform) { - //Log.info(" set transform: " + this.transform + " ==> " + transform); + //LOGGER.info(" set transform: " + this.transform + " ==> " + transform); if (Float.isNaN(transform.getPosition().x())) { - Log.critical(" set transform: " + this.transform + " ==> " + transform); + LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform); + System.exit(-1); } if (Float.isInfinite(transform.getOrientation().z())) { - Log.critical(" set transform: " + this.transform + " ==> " + transform); + LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform); + System.exit(-1); } this.transform = transform; updateBroadPhaseState(); } - + /** * Set the type of the body * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) @@ -365,7 +372,7 @@ public class CollisionBody extends Body { updateBroadPhaseState(); } } - + /** * Return true if a point is inside the collision body * This method returns true if a point is inside any collision shape of the body @@ -380,7 +387,7 @@ public class CollisionBody extends Body { } return false; } - + /** * Update the broad-phase state for this body (because it has moved for instance) */ @@ -391,13 +398,14 @@ public class CollisionBody extends Body { updateProxyShapeInBroadPhase(shape, false); } } - + /** * Update the broad-phase state of a proxy collision shape of the body */ public void updateProxyShapeInBroadPhase(final ProxyShape proxyShape, final boolean forceReinsert /* false */) { final AABB aabb = new AABB(); proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiply(proxyShape.getLocalToBodyTransform())); - this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0), forceReinsert); + this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0), + forceReinsert); } } diff --git a/src/org/atriasoft/ephysics/body/RigidBody.java b/src/org/atriasoft/ephysics/body/RigidBody.java index 8a98113..4ade211 100644 --- a/src/org/atriasoft/ephysics/body/RigidBody.java +++ b/src/org/atriasoft/ephysics/body/RigidBody.java @@ -32,10 +32,11 @@ import org.atriasoft.ephysics.constraint.JointListElement; import org.atriasoft.ephysics.engine.CollisionWorld; import org.atriasoft.ephysics.engine.DynamicsWorld; import org.atriasoft.ephysics.engine.Material; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.etk.math.Matrix3f; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * This class represents a rigid body of the physics @@ -44,7 +45,8 @@ import org.atriasoft.etk.math.Vector3f; * CollisionBody class. */ public class RigidBody extends CollisionBody { - + static final Logger LOGGER = LoggerFactory.getLogger(RigidBody.class); + protected float angularDamping = 0.0f; //!< Angular velocity damping factor protected boolean angularReactionEnable = true; public Vector3f angularVelocity = Vector3f.ZERO; //!< Angular velocity of the body @@ -61,7 +63,7 @@ public class RigidBody extends CollisionBody { public Vector3f linearVelocity = Vector3f.ZERO; //!< Linear velocity of the body public float massInverse; //!< Inverse of the mass of the body protected Material material = new Material(); //!< Material properties of the rigid body - + /** * Constructor * @param transform The transformation of the body @@ -73,7 +75,7 @@ public class RigidBody extends CollisionBody { this.centerOfMassWorld = transform.getPosition(); this.massInverse = 1.0f / this.initMass; } - + /** * Add a collision shape to the body. * When you add a collision shape to the body, an intternal copy of this collision shape will be created internally. @@ -85,7 +87,10 @@ public class RigidBody extends CollisionBody { * @param mass Mass (in kilograms) of the collision shape you want to add * @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added. */ - public ProxyShape addCollisionShape(final CollisionShape collisionShape, final Transform3D transform, final float mass) { + public ProxyShape addCollisionShape( + final CollisionShape collisionShape, + final Transform3D transform, + final float mass) { assert (mass > 0.0f); // Create a new proxy collision shape to attach the collision shape to the body final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, mass); @@ -105,7 +110,7 @@ public class RigidBody extends CollisionBody { recomputeMassInformation(); return proxyShape; } - + /** * Apply an external force to the body at a given point (in world-space coordinates). * If the point is not at the center of mass of the body, it will also @@ -127,7 +132,7 @@ public class RigidBody extends CollisionBody { this.externalForce = this.externalForce.add(force); this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force)); } - + /** * Apply an external force to the body at its center of mass. * If the body is sleeping, calling this method will wake it up. Note that the @@ -145,7 +150,7 @@ public class RigidBody extends CollisionBody { } this.externalForce = this.externalForce.add(force); } - + /** * Apply an external torque to the body. * If the body is sleeping, calling this method will wake it up. Note that the @@ -163,7 +168,7 @@ public class RigidBody extends CollisionBody { } this.externalTorque = this.externalTorque.add(torque); } - + /** * Set the variable to know if the gravity is applied to this rigid body * @param isEnabled True if you want the gravity to be applied to this body @@ -171,7 +176,7 @@ public class RigidBody extends CollisionBody { public void enableGravity(final boolean isEnabled) { this.isGravityEnabled = isEnabled; } - + /** * Get the angular velocity damping factor * @return The angular damping factor of this body @@ -179,7 +184,7 @@ public class RigidBody extends CollisionBody { public float getAngularDamping() { return this.angularDamping; } - + /** * Get the angular velocity of the body * @return The angular velocity vector of the body @@ -187,7 +192,7 @@ public class RigidBody extends CollisionBody { public Vector3f getAngularVelocity() { return this.angularVelocity; } - + /** * Get the inverse of the inertia tensor in world coordinates. * The inertia tensor Iw in world coordinates is computed with the @@ -201,15 +206,16 @@ public class RigidBody extends CollisionBody { // TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE // INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES // Compute and return the inertia tensor in world coordinates - //Log.error("}}} this.transform=" + this.transform); - //Log.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse); - //Log.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew()); - //Log.error("}}} this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix()); - final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transpose()); - //Log.error("}}} tmp=" + tmp); + //LOGGER.error("}}} this.transform=" + this.transform); + //LOGGER.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse); + //LOGGER.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew()); + //LOGGER.error("}}} this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix()); + final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse) + .multiply(this.transform.getOrientation().getMatrix().transpose()); + //LOGGER.error("}}} tmp=" + tmp); return tmp; } - + /** * Get the local inertia tensor of the body (in local-space coordinates) * @return The 3x3 inertia tensor matrix of the body @@ -217,7 +223,7 @@ public class RigidBody extends CollisionBody { public Matrix3f getInertiaTensorLocal() { return this.inertiaTensorLocal; } - + /** * Get the inertia tensor in world coordinates. * The inertia tensor Iw in world coordinates is computed @@ -229,9 +235,10 @@ public class RigidBody extends CollisionBody { */ public Matrix3f getInertiaTensorWorld() { // Compute and return the inertia tensor in world coordinates - return this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocal).multiply(this.transform.getOrientation().getMatrix().transpose()); + return this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocal) + .multiply(this.transform.getOrientation().getMatrix().transpose()); } - + /** * Get the first element of the linked list of joints involving this body * @return The first element of the linked-list of all the joints involving this body @@ -239,7 +246,7 @@ public class RigidBody extends CollisionBody { public JointListElement getJointsList() { return this.jointsList; } - + /** * Get the linear velocity damping factor * @return The linear damping factor of this body @@ -247,7 +254,7 @@ public class RigidBody extends CollisionBody { public float getLinearDamping() { return this.linearDamping; } - + /** * Get the linear velocity * @return The linear velocity vector of the body @@ -255,7 +262,7 @@ public class RigidBody extends CollisionBody { public Vector3f getLinearVelocity() { return this.linearVelocity; } - + /** * Get the mass of the body * @return The mass (in kilograms) of the body @@ -263,7 +270,7 @@ public class RigidBody extends CollisionBody { public float getMass() { return this.initMass; } - + /** * get a reference to the material properties of the rigid body * @return A reference to the material of the body @@ -271,11 +278,11 @@ public class RigidBody extends CollisionBody { public Material getMaterial() { return this.material; } - + public boolean isAngularReactionEnable() { return this.angularReactionEnable; } - + /** * get the need of gravity appling to this rigid body * @return True if the gravity is applied to the body @@ -283,7 +290,7 @@ public class RigidBody extends CollisionBody { public boolean isGravityEnabled() { return this.isGravityEnabled; } - + /** * Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body. */ @@ -302,7 +309,8 @@ public class RigidBody extends CollisionBody { // Compute the total mass of the body for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { this.initMass += shape.getMass(); - this.centerOfMassLocal = this.centerOfMassLocal.add(shape.getLocalToBodyTransform().getPosition().multiply(shape.getMass())); + this.centerOfMassLocal = this.centerOfMassLocal + .add(shape.getLocalToBodyTransform().getPosition().multiply(shape.getMass())); } if (this.initMass > 0.0f) { this.massInverse = 1.0f / this.initMass; @@ -329,22 +337,24 @@ public class RigidBody extends CollisionBody { final Vector3f off1 = offset.multiply(-offset.x()); final Vector3f off2 = offset.multiply(-offset.y()); final Vector3f off3 = offset.multiply(-offset.z()); - Matrix3f offsetMatrix = new Matrix3f(off1.x() + offsetSquare, off1.y(), off1.z(), off2.x(), off2.y() + offsetSquare, off2.z(), off3.x(), off3.y(), off3.z() + offsetSquare); + Matrix3f offsetMatrix = new Matrix3f(off1.x() + offsetSquare, off1.y(), off1.z(), off2.x(), + off2.y() + offsetSquare, off2.z(), off3.x(), off3.y(), off3.z() + offsetSquare); offsetMatrix = offsetMatrix.multiply(shape.getMass()); this.inertiaTensorLocal = this.inertiaTensorLocal.add(inertiaTensor.add(offsetMatrix)); } // Compute the local inverse inertia tensor this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse(); // Update the linear velocity of the center of mass - this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); + this.linearVelocity = this.linearVelocity + .add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); } - + @Override public void removeCollisionShape(final ProxyShape proxyShape) { super.removeCollisionShape(proxyShape); recomputeMassInformation(); } - + /** * Remove a joint from the joints list */ @@ -369,7 +379,7 @@ public class RigidBody extends CollisionBody { } } } - + /** * Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation. * @param angularDamping The angular damping factor of this body @@ -378,11 +388,11 @@ public class RigidBody extends CollisionBody { assert (angularDamping >= 0.0f); this.angularDamping = angularDamping; } - + public void setAngularReactionEnable(final boolean angularReactionEnable) { this.angularReactionEnable = angularReactionEnable; } - + /** * Set the angular velocity. * @param angularVelocity The angular velocity vector of the body @@ -396,7 +406,7 @@ public class RigidBody extends CollisionBody { setIsSleeping(false); } } - + /** * Set the local center of mass of the body (in local-space coordinates) * @param centerOfMassLocal The center of mass of the body in local-space coordinates @@ -408,9 +418,10 @@ public class RigidBody extends CollisionBody { final Vector3f oldCenterOfMass = this.centerOfMassWorld; this.centerOfMassLocal = centerOfMassLocal; this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal); - this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); + this.linearVelocity = this.linearVelocity + .add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); } - + /** * Set the local inertia tensor of the body (in local-space coordinates) * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates @@ -422,7 +433,7 @@ public class RigidBody extends CollisionBody { this.inertiaTensorLocal = inertiaTensorLocal; this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse(); } - + /** * Set the variable to know whether or not the body is sleeping * @param isSleeping New sleeping state of the body @@ -437,7 +448,7 @@ public class RigidBody extends CollisionBody { } super.setIsSleeping(isSleeping); } - + /** * Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation. * @param linearDamping The linear damping factor of this body @@ -446,7 +457,7 @@ public class RigidBody extends CollisionBody { assert (linearDamping >= 0.0f); this.linearDamping = linearDamping; } - + /** * Set the linear velocity of the rigid body. * @param linearVelocity Linear velocity vector of the body @@ -460,7 +471,7 @@ public class RigidBody extends CollisionBody { setIsSleeping(false); } } - + /** * Set the mass of the rigid body * @param mass The mass (in kilograms) of the body @@ -477,7 +488,7 @@ public class RigidBody extends CollisionBody { this.massInverse = 1.0f; } } - + /** * Set a new material for this rigid body * @param material The material you want to set to the body @@ -485,29 +496,32 @@ public class RigidBody extends CollisionBody { public void setMaterial(final Material material) { this.material = material; } - + /** * Set the current position and orientation * @param transform The transformation of the body that transforms the local-space of the body into world-space */ @Override public void setTransform(final Transform3D transform) { - //Log.info(" set transform: " + this.transform + " ==> " + transform); + //LOGGER.info(" set transform: " + this.transform + " ==> " + transform); if (transform.getPosition().x() == Float.NaN) { - Log.critical(" set transform: " + this.transform + " ==> " + transform); + LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform); + System.exit(-1); } if (Float.isInfinite(transform.getOrientation().z())) { - Log.critical(" set transform: " + this.transform + " ==> " + transform); + LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform); + System.exit(-1); } this.transform = transform; final Vector3f oldCenterOfMass = this.centerOfMassWorld; // Compute the new center of mass in world-space coordinates this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal); // Update the linear velocity of the center of mass - this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); + this.linearVelocity = this.linearVelocity + .add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass))); updateBroadPhaseState(); } - + @Override public void setType(final BodyType type) { if (this.type == type) { @@ -536,38 +550,42 @@ public class RigidBody extends CollisionBody { this.externalForce = Vector3f.ZERO; this.externalTorque = Vector3f.ZERO; } - + @Override public void updateBroadPhaseState() { final DynamicsWorld world = (DynamicsWorld) this.world; final Vector3f displacement = this.linearVelocity.multiply(world.timeStep); - + // For all the proxy collision shapes of the body for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { // Recompute the world-space AABB of the collision shape final AABB aabb = new AABB(); - //Log.info(" : " + aabb.getMin() + " " + aabb.getMax()); - //Log.info(" this.transform: " + this.transform); + //LOGGER.info(" : " + aabb.getMin() + " " + aabb.getMax()); + //LOGGER.info(" this.transform: " + this.transform); shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform())); - //Log.info(" : " + aabb.getMin() + " " + aabb.getMax()); + //LOGGER.info(" : " + aabb.getMin() + " " + aabb.getMax()); // Update the broad-phase state for the proxy collision shape - //Log.warning(" ==> updateProxyCollisionShape"); + //LOGGER.warn(" ==> updateProxyCollisionShape"); world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); } } - + /** * Update the transform of the body after a change of the center of mass */ public void updateTransformWithCenterOfMass() { // Translate the body according to the translation of the center of mass position - this.transform = new Transform3D(this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)), this.transform.getOrientation()); + this.transform = new Transform3D( + this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)), + this.transform.getOrientation()); if (Float.isNaN(this.transform.getPosition().x())) { - Log.critical("updateTransformWithCenterOfMass: " + this.transform); + LOGGER.error("[CRITICAL] updateTransformWithCenterOfMass: " + this.transform); + System.exit(-1); } if (Float.isInfinite(this.transform.getOrientation().z())) { - Log.critical(" set transform: " + this.transform); + LOGGER.error("[CRITICAL] set transform: " + this.transform); + System.exit(-1); } } - + } diff --git a/src/org/atriasoft/ephysics/collision/CollisionDetection.java b/src/org/atriasoft/ephysics/collision/CollisionDetection.java index 9073a4c..09e9c95 100644 --- a/src/org/atriasoft/ephysics/collision/CollisionDetection.java +++ b/src/org/atriasoft/ephysics/collision/CollisionDetection.java @@ -2,6 +2,7 @@ package org.atriasoft.ephysics.collision; import java.util.Iterator; import java.util.Map; +import java.util.Map.Entry; import java.util.TreeMap; import org.atriasoft.ephysics.RaycastCallback; @@ -25,12 +26,13 @@ import org.atriasoft.ephysics.engine.CollisionCallback; import org.atriasoft.ephysics.engine.CollisionWorld; import org.atriasoft.ephysics.engine.EventListener; import org.atriasoft.ephysics.engine.OverlappingPair; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.ephysics.mathematics.PairInt; import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.ephysics.mathematics.Set; import org.atriasoft.ephysics.mathematics.SetMultiple; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /* * It computes the collision detection algorithms. We first @@ -39,6 +41,7 @@ import org.atriasoft.etk.math.Vector3f; * collision contacts between bodies. */ public class CollisionDetection implements NarrowPhaseCallback { + static final Logger LOGGER = LoggerFactory.getLogger(CollisionDetection.class); /// Broad-phase algorithm private final BroadPhaseAlgorithm broadPhaseAlgorithm; /// Collision Detection Dispatch configuration @@ -50,11 +53,11 @@ public class CollisionDetection implements NarrowPhaseCallback { private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously // TODO : Delete this private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm - private final SetMultiple noCollisionPairs = new SetMultiple(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other + private final SetMultiple noCollisionPairs = new SetMultiple<>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other public Map overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs /// Reference on the physics world private final CollisionWorld world; - + /// Constructor public CollisionDetection(final CollisionWorld world) { this.world = world; @@ -66,7 +69,7 @@ public class CollisionDetection implements NarrowPhaseCallback { // Fill-in the collision detection matrix with algorithms fillInCollisionMatrix(); } - + /// Add all the contact manifold of colliding pairs to their bodies private void addAllContactManifoldsToBodies() { // For each overlapping pairs in contact during the narrow-phase @@ -76,7 +79,7 @@ public class CollisionDetection implements NarrowPhaseCallback { addContactManifoldToBody(it); } } - + /// Add a contact manifold to the linked list of contact manifolds of the two bodies /// involed in the corresponding contact. private void addContactManifoldToBody(final OverlappingPair pair) { @@ -91,33 +94,33 @@ public class CollisionDetection implements NarrowPhaseCallback { // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body body1.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList); - + // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList); - + } } - + /// Add a pair of bodies that cannot collide with each other public void addNoCollisionPair(final CollisionBody body1, final CollisionBody body2) { this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(body1, body2)); } - + /// Add a proxy collision shape to the collision detection public void addProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb) { // Add the body to the broad-phase this.broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); this.isCollisionShapesAdded = true; } - + // Ask for a collision shape to be tested again during broad-phase. /// We simply put the shape in the list of collision shape that have moved in the /// previous frame so that it is tested for collision again in the broad-phase. public void askForBroadPhaseCollisionCheck(final ProxyShape shape) { this.broadPhaseAlgorithm.addMovedCollisionShape(shape.broadPhaseID); } - + /// Allow the broadphase to notify the collision detection about an overlapping pair. /// This method is called by the broad-phase collision detection algorithm public void broadPhaseNotifyOverlappingPair(final ProxyShape shape1, final ProxyShape shape2) { @@ -126,10 +129,11 @@ public class CollisionDetection implements NarrowPhaseCallback { if (shape1.getBody().getID() == shape2.getBody().getID()) { return; } - //Log.info(" check collision is allowed: " + shape1.getBody().getID() + " " + shape2.getBody().getID()); + //LOGGER.info(" check collision is allowed: " + shape1.getBody().getID() + " " + shape2.getBody().getID()); // Check if the collision filtering allows collision between the two shapes - if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) { - Log.info(" ==> not permited ..."); + if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 + || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) { + LOGGER.info(" ==> not permited ..."); return; } // Compute the overlapping pair ID @@ -139,7 +143,8 @@ public class CollisionDetection implements NarrowPhaseCallback { return; } // Compute the maximum number of contact manifolds for this pair - final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(shape1.getCollisionShape().getType(), shape2.getCollisionShape().getType()); + final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(shape1.getCollisionShape().getType(), + shape2.getCollisionShape().getType()); // Create the overlapping pair and add it int32to the set of overlapping pairs final OverlappingPair newPair = new OverlappingPair(shape1, shape2, nbMaxManifolds); assert (newPair != null); @@ -148,13 +153,13 @@ public class CollisionDetection implements NarrowPhaseCallback { shape1.getBody().setIsSleeping(false); shape2.getBody().setIsSleeping(false); } - + /// Delete all the contact points in the currently overlapping pairs private void clearContactPoints() { // For each overlapping pair this.overlappingPairs.forEach((key, value) -> value.clearContactPoints()); } - + /// Compute the broad-phase collision detection private void computeBroadPhase() { // If new collision shapes have been added to bodies @@ -165,33 +170,29 @@ public class CollisionDetection implements NarrowPhaseCallback { this.broadPhaseAlgorithm.computeOverlappingPairs(); } } - + /// Compute the collision detection public void computeCollisionDetection() { - //Log.info("computeBroadPhase();"); + //LOGGER.info("computeBroadPhase();"); // Compute the broad-phase collision detection computeBroadPhase(); - //Log.info("computeNarrowPhase();"); + //LOGGER.info("computeNarrowPhase();"); // Compute the narrow-phase collision detection computeNarrowPhase(); } - + /// Compute the narrow-phase collision detection private void computeNarrowPhase() { // Clear the set of overlapping pairs in narrow-phase contact this.contactOverlappingPair.clear(); { - //Log.info("list elements:"); - final Iterator> ittt = this.overlappingPairs.entrySet().iterator(); - while (ittt.hasNext()) { - final Map.Entry entry = ittt.next(); - //Log.info(" " + entry.getKey() + " " + entry.getValue()); - + for (Entry entry : this.overlappingPairs.entrySet()) { + } } // For each possible collision pair of bodies final Iterator> it = this.overlappingPairs.entrySet().iterator(); - + while (it.hasNext()) { final Map.Entry entry = it.next(); final OverlappingPair pair = entry.getValue(); @@ -201,7 +202,8 @@ public class CollisionDetection implements NarrowPhaseCallback { // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) + if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 + || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) || !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair @@ -234,23 +236,28 @@ public class CollisionDetection implements NarrowPhaseCallback { // Notify the narrow-phase algorithm about the overlapping pair we are going to test narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); // Create the CollisionShapeInfo objects - final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); - - final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); - + final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), + shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); + + final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), + shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); + // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this); } - + // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); - + } - + /// Compute the narrow-phase collision detection - public void computeNarrowPhaseBetweenShapes(final CollisionCallback callback, final Set shapes1, final Set shapes2) { + public void computeNarrowPhaseBetweenShapes( + final CollisionCallback callback, + final Set shapes1, + final Set shapes2) { this.contactOverlappingPair.clear(); // For each possible collision pair of bodies final Iterator> it = this.overlappingPairs.entrySet().iterator(); @@ -262,20 +269,24 @@ public class CollisionDetection implements NarrowPhaseCallback { assert (shape1.broadPhaseID != shape2.broadPhaseID); // If both shapes1 and shapes2 sets are non-empty, we check that // shape1 is among on set and shape2 is among the other one - if (!shapes1.isEmpty() && !shapes2.isEmpty() && (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0) + if (!shapes1.isEmpty() && !shapes2.isEmpty() + && (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0) && (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) { continue; } - if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0 && shapes1.count(shape2.broadPhaseID) == 0) { + if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0 + && shapes1.count(shape2.broadPhaseID) == 0) { continue; } - if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0 && shapes2.count(shape2.broadPhaseID) == 0) { + if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0 + && shapes2.count(shape2.broadPhaseID) == 0) { continue; } // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) + if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 + || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) || !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair @@ -309,20 +320,23 @@ public class CollisionDetection implements NarrowPhaseCallback { // Notify the narrow-phase algorithm about the overlapping pair we are going to test narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); // Create the CollisionShapeInfo objects - final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); - - final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); - - final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(callback); + final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), + shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); + + final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), + shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); + + final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback( + callback); // Use the narrow-phase collision detection algorithm to check // if there really is a collision narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback); } - + // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } - + void createContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) { // Create a new contact final ContactPoint contact = new ContactPoint(contactInfo); @@ -332,40 +346,43 @@ public class CollisionDetection implements NarrowPhaseCallback { final PairDTree pairId = OverlappingPair.computeID(overlappingPair.getShape1(), overlappingPair.getShape2()); this.contactOverlappingPair.put(pairId, overlappingPair); } - + /// Fill-in the collision detection matrix private void fillInCollisionMatrix() { // For each possible type of collision shape for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) { for (int j = 0; j < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; j++) { - this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i), CollisionShapeType.getType(j)); + this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i), + CollisionShapeType.getType(j)); } } } - + /// Return the Narrow-phase collision detection algorithm to use between two types of shapes - public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType shape1Type, final CollisionShapeType shape2Type) { + public NarrowPhaseAlgorithm getCollisionAlgorithm( + final CollisionShapeType shape1Type, + final CollisionShapeType shape2Type) { return this.collisionMatrix[shape1Type.value][shape2Type.value]; } - + public GJKAlgorithm getNarrowPhaseGJKAlgorithm() { return this.narrowPhaseGJKAlgorithm; } - + /// Return a pointer to the world public CollisionWorld getWorld() { return this.world; } - + /// Return the world event listener public EventListener getWorldEventListener() { return this.world.eventListener; } - + /// Called by a narrow-phase collision algorithm when a new contact has been found @Override public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) { - Log.error("Notify Contact ... --------------------"); + LOGGER.error("Notify Contact ... --------------------"); // If it is the first contact since the pairs are overlapping if (overlappingPair.getNbContactPoints() == 0) { // Trigger a callback event @@ -380,7 +397,7 @@ public class CollisionDetection implements NarrowPhaseCallback { this.world.eventListener.newContact(contactInfo); } } - + /// Ray casting method public void raycast(final RaycastCallback raycastCallback, final Ray ray, final int raycastWithCategoryMaskBits) { final RaycastTest rayCastTest = new RaycastTest(raycastCallback); @@ -388,12 +405,12 @@ public class CollisionDetection implements NarrowPhaseCallback { // callback method for each proxy shape hit by the ray in the broad-phase this.broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); } - + /// Remove a pair of bodies that cannot collide with each other public void removeNoCollisionPair(final CollisionBody body1, final CollisionBody body2) { this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(body1, body2))); } - + /// Remove a proxy collision shape from the collision detection public void removeProxyCollisionShape(final ProxyShape proxyShape) { // Remove all the overlapping pairs involving this proxy shape @@ -401,7 +418,8 @@ public class CollisionDetection implements NarrowPhaseCallback { while (it.hasNext()) { final Map.Entry entry = it.next(); final OverlappingPair pair = entry.getValue(); - if (pair.getShape1().broadPhaseID == proxyShape.broadPhaseID || pair.getShape2().broadPhaseID == proxyShape.broadPhaseID) { + if (pair.getShape1().broadPhaseID == proxyShape.broadPhaseID + || pair.getShape2().broadPhaseID == proxyShape.broadPhaseID) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair it.remove(); @@ -410,28 +428,31 @@ public class CollisionDetection implements NarrowPhaseCallback { // Remove the body from the broad-phase this.broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); } - + /// Report collision between two sets of shapes - public void reportCollisionBetweenShapes(final CollisionCallback callback, final Set shapes1, final Set shapes2) { - - // For each possible collision pair of bodies - final Iterator> it = this.overlappingPairs.entrySet().iterator(); - while (it.hasNext()) { - final Map.Entry entry = it.next(); + public void reportCollisionBetweenShapes( + final CollisionCallback callback, + final Set shapes1, + final Set shapes2) { + + for (Entry entry : this.overlappingPairs.entrySet()) { final OverlappingPair pair = entry.getValue(); final ProxyShape shape1 = pair.getShape1(); final ProxyShape shape2 = pair.getShape2(); assert (shape1.broadPhaseID != shape2.broadPhaseID); // If both shapes1 and shapes2 sets are non-empty, we check that // shape1 is among on set and shape2 is among the other one - if (!shapes1.isEmpty() && !shapes2.isEmpty() && (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0) + if (!shapes1.isEmpty() && !shapes2.isEmpty() + && (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0) && (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) { continue; } - if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0 && shapes1.count(shape2.broadPhaseID) == 0) { + if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0 + && shapes1.count(shape2.broadPhaseID) == 0) { continue; } - if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0 && shapes2.count(shape2.broadPhaseID) == 0) { + if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0 + && shapes2.count(shape2.broadPhaseID) == 0) { continue; } // For each contact manifold set of the overlapping pair @@ -442,19 +463,21 @@ public class CollisionDetection implements NarrowPhaseCallback { for (int i = 0; i < manifold.getNbContactPoints(); i++) { final ContactPoint contactPoint = manifold.getContactPoint(i); // Create the contact info object for the contact - final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(), manifold.getShape2(), manifold.getShape1().getCollisionShape(), - manifold.getShape2().getCollisionShape(), contactPoint.getNormal(), contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(), + final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(), + manifold.getShape2(), manifold.getShape1().getCollisionShape(), + manifold.getShape2().getCollisionShape(), contactPoint.getNormal(), + contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(), contactPoint.getLocalPointOnBody2()); // Notify the collision callback about this new contact if (callback != null) { callback.notifyContact(contactInfo); } } - + } } } - + /// Set the collision dispatch configuration public void setCollisionDispatch(final CollisionDispatch collisionDispatch) { this.collisionDispatch = collisionDispatch; @@ -462,7 +485,7 @@ public class CollisionDetection implements NarrowPhaseCallback { // Fill-in the collision matrix with the new algorithms to use fillInCollisionMatrix(); } - + /// Test if the AABBs of two proxy shapes overlap public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) { // If one of the shape's body is not active, we return no overlap @@ -471,9 +494,12 @@ public class CollisionDetection implements NarrowPhaseCallback { } return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); } - + /// Compute the collision detection - public void testCollisionBetweenShapes(final CollisionCallback callback, final Set shapes1, final Set shapes2) { + public void testCollisionBetweenShapes( + final CollisionCallback callback, + final Set shapes1, + final Set shapes2) { // Compute the broad-phase collision detection computeBroadPhase(); // Delete all the contact points in the currently overlapping pairs @@ -481,17 +507,21 @@ public class CollisionDetection implements NarrowPhaseCallback { // Compute the narrow-phase collision detection among given sets of shapes computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); } - + /// Update a proxy collision shape (that has moved for instance) public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb) { updateProxyCollisionShape(shape, aabb, new Vector3f(0, 0, 0), false); } - + public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement) { updateProxyCollisionShape(shape, aabb, displacement, false); } - - public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement, final boolean forceReinsert) { + + public void updateProxyCollisionShape( + final ProxyShape shape, + final AABB aabb, + final Vector3f displacement, + final boolean forceReinsert) { this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } } diff --git a/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java b/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java index bc668a3..748e584 100644 --- a/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java +++ b/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java @@ -102,7 +102,7 @@ public class BroadPhaseAlgorithm { /// Add a collision shape in the array of shapes that have moved in the last simulation step /// and that need to be tested again for broad-phase overlapping. public void addMovedCollisionShape(final DTree broadPhaseID) { - //Log.info(" ** Element that has moved ... = " + broadPhaseID); + //LOGGER.info(" ** Element that has moved ... = " + broadPhaseID); this.movedShapes.add(broadPhaseID); } @@ -122,10 +122,10 @@ public class BroadPhaseAlgorithm { this.potentialPairs.clear(); // For all collision shapes that have moved (or have been created) during the // last simulation step - //Log.info("moved shape = " + this.movedShapes.size()); + //LOGGER.info("moved shape = " + this.movedShapes.size()); /* for (final DTree it : this.movedShapes) { - Log.info(" - " + it); + LOGGER.info(" - " + it); } */ for (final DTree it : this.movedShapes) { @@ -147,7 +147,7 @@ public class BroadPhaseAlgorithm { } }); } - //Log.print("Find potential pair : " + this.potentialPairs.size()); + //LOGGER.print("Find potential pair : " + this.potentialPairs.size()); // Reset the array of collision shapes that have move (or have been created) during the last simulation step this.movedShapes.clear(); // Sort the array of potential overlapping pairs in order to remove duplicate pairs @@ -245,7 +245,7 @@ public class BroadPhaseAlgorithm { // Update the dynamic AABB tree according to the movement of the collision shape final boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); // If the collision shape has moved out of its fat AABB (and therefore has been reinserted - //Log.error(" ==> hasBeenReInserted = " + hasBeenReInserted); + //LOGGER.error(" ==> hasBeenReInserted = " + hasBeenReInserted); // longo the tree). if (hasBeenReInserted) { // Add the collision shape longo the array of shapes that have moved (or have been created) diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java b/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java index 56a75af..607e339 100644 --- a/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java +++ b/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java @@ -5,10 +5,11 @@ import java.util.Stack; import org.atriasoft.ephysics.Configuration; import org.atriasoft.ephysics.collision.shapes.AABB; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.etk.math.FMath; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * It implements a dynamic AABB tree that is used for broad-phase @@ -18,6 +19,7 @@ import org.atriasoft.etk.math.Vector3f; * "Introduction to Game Physics with Box2D" by Ian Parberry. */ public class DynamicAABBTree { + static final Logger LOGGER = LoggerFactory.getLogger(DynamicAABBTree.class); private final float extraAABBGap; //!< Extra AABB Gap used to allow the collision shape to move a little bit without triggering a large modification of the tree which can be costly /// Allocate and return a node to use in the tree @@ -344,7 +346,7 @@ public class DynamicAABBTree { /// Ray casting method public void raycast(final Ray ray, final CallbackRaycast callback) { if (callback == null) { - Log.error("call with null callback"); + LOGGER.error("call with null callback"); return; } float maxFraction = ray.maxFraction; @@ -468,15 +470,15 @@ public class DynamicAABBTree { /// Report all shapes overlapping with the AABB given in parameter. public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) { if (callback == null) { - Log.error("call with null callback"); + LOGGER.error("call with null callback"); return; } - //Log.error("reportAllShapesOverlappingWithAABB"); + //LOGGER.error("reportAllShapesOverlappingWithAABB"); // Create a stack with the nodes to visit final Stack stack = new Stack<>(); // 64 max stack.push(this.rootNode); - //Log.error(" add stack: " + this.rootNode); + //LOGGER.error(" add stack: " + this.rootNode); // While there are still nodes to visit while (stack.size() > 0) { // Get the next node ID to visit @@ -487,14 +489,14 @@ public class DynamicAABBTree { } // Get the corresponding node final DTree nodeToVisit = nodeIDToVisit; - //Log.error(" check colision: " + nodeIDToVisit); + //LOGGER.error(" check colision: " + nodeIDToVisit); // If the AABB in parameter overlaps with the AABB of the node to visit if (aabb.testCollision(nodeToVisit.aabb)) { // If the node is a leaf if (nodeToVisit.isLeaf()) { /* if (aabb != nodeToVisit.aabb) { - Log.error(" ======> Real collision ..."); + LOGGER.error(" ======> Real collision ..."); } */ // Notify the broad-phase about a new potential overlapping pair @@ -505,8 +507,8 @@ public class DynamicAABBTree { // We need to visit its children stack.push(tmp.childrenleft); stack.push(tmp.childrenright); - //Log.error(" add stack: " + tmp.childrenleft); - //Log.error(" add stack: " + tmp.childrenright); + //LOGGER.error(" add stack: " + tmp.childrenleft); + //LOGGER.error(" add stack: " + tmp.childrenright); } } } @@ -529,11 +531,15 @@ public class DynamicAABBTree { return updateObject(node, newAABB, displacement, false); } - public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement, final boolean forceReinsert) { + public boolean updateObject( + final DTree node, + final AABB newAABB, + final Vector3f displacement, + final boolean forceReinsert) { assert (node.isLeaf()); assert (node.height >= 0); - //Log.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); - //Log.verbose(" : " + newAABB.getMin() + " " + newAABB.getMax()); + //LOGGER.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); + //LOGGER.verbose(" : " + newAABB.getMin() + " " + newAABB.getMax()); // If the new AABB is still inside the fat AABB of the node if (!forceReinsert && node.aabb.contains(newAABB)) { return false; @@ -570,10 +576,10 @@ public class DynamicAABBTree { } node.aabb.setMin(new Vector3f(xmin, ymin, zmin)); node.aabb.setMax(new Vector3f(xmax, ymax, zmax)); - //Log.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); - //Log.error(" : " + newAABB.getMin() + " " + newAABB.getMax()); + //LOGGER.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); + //LOGGER.error(" : " + newAABB.getMin() + " " + newAABB.getMax()); if (!node.aabb.contains(newAABB)) { - //Log.critical("ERROR"); + //LOGGER.critical("ERROR"); } assert (node.aabb.contains(newAABB)); // Reinsert the node into the tree diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java index 98e55cf..31a7d6a 100644 --- a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java @@ -34,20 +34,20 @@ import org.atriasoft.etk.math.Vector3f; public class EPAAlgorithm { /// Add a triangle face in the candidate triangle heap private void addFaceCandidate(final TriangleEPA triangle, final SortedSet heap, final float upperBoundSquarePenDepth) { - ////Log.info("addFaceCandidate: " + triangle.get(0) + ", " + triangle.get(1) + ", " + triangle.get(2) + " " + upperBoundSquarePenDepth); + ////LOGGER.info("addFaceCandidate: " + triangle.get(0) + ", " + triangle.get(1) + ", " + triangle.get(2) + " " + upperBoundSquarePenDepth); // If the closest point of the affine hull of triangle // points is internal to the triangle and if the distance // of the closest point from the origin is at most the // penetration depth upper bound - ////Log.info(" triangle.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle()); - ////Log.info(" triangle.getDistSquare(): " + triangle.getDistSquare()); + ////LOGGER.info(" triangle.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle()); + ////LOGGER.info(" triangle.getDistSquare(): " + triangle.getDistSquare()); if (triangle.isClosestPointInternalToTriangle() && triangle.getDistSquare() <= upperBoundSquarePenDepth) { // Add the triangle face to the list of candidates heap.add(triangle); - ////Log.info("add in heap:"); + ////LOGGER.info("add in heap:"); //int iii = 0; //for (final TriangleEPA elem : heap) { - // ////Log.info(" [" + iii + "] " + elem.getDistSquare()); + // ////LOGGER.info(" [" + iii + "] " + elem.getDistSquare()); // ++iii; //} } @@ -61,7 +61,7 @@ public class EPAAlgorithm { /// the correct penetration depth public Vector3f computePenetrationDepthAndContactPoints(final Simplex simplex, final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2, Vector3f vector, final NarrowPhaseCallback narrowPhaseCallback) { - ////Log.info("computePenetrationDepthAndContactPoints()"); + ////LOGGER.info("computePenetrationDepthAndContactPoints()"); assert (shape1Info.collisionShape().isConvex()); assert (shape2Info.collisionShape().isConvex()); final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape()); @@ -104,7 +104,7 @@ public class EPAAlgorithm { // Select an action according to the number of points in the simplex // computed with GJK algorithm in order to obtain an initial polytope for // The EPA algorithm. - ////Log.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices); + ////LOGGER.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices); switch (nbVertices) { case 1: // Only one point in the simplex (which should be the origin). @@ -179,13 +179,13 @@ public class EPAAlgorithm { // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we ruct the tetrahedron. // Comstruct the 4 triangle faces of the tetrahedron - ////Log.error("befor call: (points, 0, 1, 2)"); + ////LOGGER.error("befor call: (points, 0, 1, 2)"); final TriangleEPA face0 = triangleStore.newTriangle(points, 0, 1, 2); - ////Log.error("befor call: (points, 0, 3, 1)"); + ////LOGGER.error("befor call: (points, 0, 3, 1)"); final TriangleEPA face1 = triangleStore.newTriangle(points, 0, 3, 1); - ////Log.error("befor call: (points, 0, 2, 3)"); + ////LOGGER.error("befor call: (points, 0, 2, 3)"); final TriangleEPA face2 = triangleStore.newTriangle(points, 0, 2, 3); - ////Log.error("befor call: (points, 1, 3, 2)"); + ////LOGGER.error("befor call: (points, 1, 3, 2)"); final TriangleEPA face3 = triangleStore.newTriangle(points, 1, 3, 2); // If the ructed tetrahedron is not correct if (!((face0 != null) && (face1 != null) && (face2 != null) && (face3 != null) && face0.getDistSquare() > 0.0f && face1.getDistSquare() > 0.0 && face2.getDistSquare() > 0.0f @@ -227,23 +227,23 @@ public class EPAAlgorithm { final Vector3f v1 = points[1].less(points[0]); final Vector3f v2 = points[2].less(points[0]); final Vector3f n = v1.cross(v2); - ////Log.info(">>>>>>>>>>>>>>>>>>"); - ////Log.info(" v1 = " + v1); - ////Log.info(" v2 = " + v2); - ////Log.info(" n = " + n); + ////LOGGER.info(">>>>>>>>>>>>>>>>>>"); + ////LOGGER.info(" v1 = " + v1); + ////LOGGER.info(" v2 = " + v2); + ////LOGGER.info(" n = " + n); // Compute the two new vertices to obtain a hexahedron suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiply(-1)), shape2CachedCollisionData)); points[3] = suppPointsA[3].less(suppPointsB[3]); - ////Log.info(" suppPointsA[3]= " + suppPointsA[3]); - ////Log.info(" suppPointsB[3]= " + suppPointsB[3]); - ////Log.info(" points[3] = " + points[3]); + ////LOGGER.info(" suppPointsA[3]= " + suppPointsA[3]); + ////LOGGER.info(" suppPointsB[3]= " + suppPointsB[3]); + ////LOGGER.info(" points[3] = " + points[3]); suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiply(-1), shape1CachedCollisionData); suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData)); points[4] = suppPointsA[4].less(suppPointsB[4]); - ////Log.info(" suppPointsA[4]= " + suppPointsA[4]); - ////Log.info(" suppPointsB[4]= " + suppPointsB[4]); - ////Log.info(" points[4]= " + points[4]); + ////LOGGER.info(" suppPointsA[4]= " + suppPointsA[4]); + ////LOGGER.info(" suppPointsB[4]= " + suppPointsB[4]); + ////LOGGER.info(" points[4]= " + points[4]); TriangleEPA face0 = null; TriangleEPA face1 = null; TriangleEPA face2 = null; @@ -253,25 +253,25 @@ public class EPAAlgorithm { // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we ruct the tetrahedron. // Comstruct the 4 triangle faces of the tetrahedron - ////Log.error("befor call: (points, 0, 1, 2)"); + ////LOGGER.error("befor call: (points, 0, 1, 2)"); face0 = triangleStore.newTriangle(points, 0, 1, 2); - ////Log.error("befor call: (points, 0, 2, 1)"); + ////LOGGER.error("befor call: (points, 0, 2, 1)"); face1 = triangleStore.newTriangle(points, 0, 3, 1); - ////Log.error("befor call: (points, 0, 2, 3)"); + ////LOGGER.error("befor call: (points, 0, 2, 3)"); face2 = triangleStore.newTriangle(points, 0, 2, 3); - ////Log.error("befor call: (points, 1, 3, 2)"); + ////LOGGER.error("befor call: (points, 1, 3, 2)"); face3 = triangleStore.newTriangle(points, 1, 3, 2); } else if (isOriginInTetrahedron(points[0], points[1], points[2], points[4]) == 0) { // The tetrahedron is a correct initial polytope for the EPA algorithm. // Therefore, we ruct the tetrahedron. // Comstruct the 4 triangle faces of the tetrahedron - ////Log.error("befor call: (points, 0, 1, 2)"); + ////LOGGER.error("befor call: (points, 0, 1, 2)"); face0 = triangleStore.newTriangle(points, 0, 1, 2); - ////Log.error("befor call: (points, 0, 4, 1)"); + ////LOGGER.error("befor call: (points, 0, 4, 1)"); face1 = triangleStore.newTriangle(points, 0, 4, 1); - ////Log.error("befor call: (points, 0, 2, 4)"); + ////LOGGER.error("befor call: (points, 0, 2, 4)"); face2 = triangleStore.newTriangle(points, 0, 2, 4); - ////Log.error("befor call: (points, 1, 4, 2)"); + ////LOGGER.error("befor call: (points, 1, 4, 2)"); face3 = triangleStore.newTriangle(points, 1, 4, 2); } else { return vector; @@ -309,10 +309,10 @@ public class EPAAlgorithm { do { triangle = triangleHeap.first(); triangleHeap.remove(triangle); - ////Log.info("rm from heap:"); + ////LOGGER.info("rm from heap:"); int iii = 0; for (final TriangleEPA elem : triangleHeap) { - ////Log.info(" [" + iii + "] " + elem.getDistSquare()); + ////LOGGER.info(" [" + iii + "] " + elem.getDistSquare()); ++iii; } // If the candidate face in the heap is not obsolete @@ -331,11 +331,11 @@ public class EPAAlgorithm { nbVertices++; // Update the upper bound of the penetration depth final float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint()); - ////Log.info(" point=" + points[indexNewVertex]); - ////Log.info("close point=" + triangle.getClosestPoint()); - ////Log.info(" ==>" + wDotv); + ////LOGGER.info(" point=" + points[indexNewVertex]); + ////LOGGER.info("close point=" + triangle.getClosestPoint()); + ////LOGGER.info(" ==>" + wDotv); if (wDotv < 0.0f) { - ////Log.error("depth penetration error " + wDotv); + ////LOGGER.error("depth penetration error " + wDotv); continue; } assert (wDotv >= 0.0f); @@ -392,32 +392,32 @@ public class EPAAlgorithm { /// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of /// the vertex that is wrong if the origin is not in the tetrahedron private int isOriginInTetrahedron(final Vector3f p1, final Vector3f p2, final Vector3f p3, final Vector3f p4) { - ////Log.error("isOriginInTetrahedron(" + p1 + ", " + p2 + ", " + p3 + ", " + p4 + ")"); + ////LOGGER.error("isOriginInTetrahedron(" + p1 + ", " + p2 + ", " + p3 + ", " + p4 + ")"); // Check vertex 1 final Vector3f normal1 = p2.less(p1).cross(p3.less(p1)); if ((normal1.dot(p1) > 0.0f) == (normal1.dot(p4) > 0.0)) { - ////Log.error(" ==> 4"); + ////LOGGER.error(" ==> 4"); return 4; } // Check vertex 2 final Vector3f normal2 = p4.less(p2).cross(p3.less(p2)); if ((normal2.dot(p2) > 0.0f) == (normal2.dot(p1) > 0.0)) { - ////Log.error(" ==> 1"); + ////LOGGER.error(" ==> 1"); return 1; } // Check vertex 3 final Vector3f normal3 = p4.less(p3).cross(p1.less(p3)); if ((normal3.dot(p3) > 0.0f) == (normal3.dot(p2) > 0.0)) { - ////Log.error(" ==> 2"); + ////LOGGER.error(" ==> 2"); return 2; } // Check vertex 4 final Vector3f normal4 = p2.less(p4).cross(p1.less(p4)); if ((normal4.dot(p4) > 0.0f) == (normal4.dot(p3) > 0.0)) { - ////Log.error(" ==> 3"); + ////LOGGER.error(" ==> 3"); return 3; } - ////Log.error(" ==> 0"); + ////LOGGER.error(" ==> 0"); // The origin is in the tetrahedron, we return 0 return 0; } diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java index 1d73d3a..6dde4f9 100644 --- a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java @@ -63,12 +63,12 @@ public class EdgeEPA implements Cloneable { /// Execute the recursive silhouette algorithm from this edge public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) { - //Log.error("EdgeEPA computeSilhouette ..."); + //LOGGER.error("EdgeEPA computeSilhouette ..."); // If the edge has not already been visited if (!this.ownerTriangle.getIsObsolete()) { // If the triangle of this edge is not visible from the given point if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) { - //Log.error("EdgeEPA 1 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); + //LOGGER.error("EdgeEPA 1 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); // If the triangle has been created if (triangle != null) { @@ -82,7 +82,7 @@ public class EdgeEPA implements Cloneable { final int backup = triangleStore.getNbTriangles(); if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { this.ownerTriangle.setIsObsolete(false); - //Log.error("EdgeEPA 2 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); + //LOGGER.error("EdgeEPA 2 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); // If the triangle has been created if (triangle != null) { @@ -93,7 +93,7 @@ public class EdgeEPA implements Cloneable { } else if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { this.ownerTriangle.setIsObsolete(false); triangleStore.resize(backup); - //Log.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); + //LOGGER.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); if (triangle != null) { TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this); diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java index 22d1c25..487d196 100644 --- a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java @@ -3,12 +3,14 @@ package org.atriasoft.ephysics.collision.narrowphase.EPA; import java.util.ArrayList; import java.util.List; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class TrianglesStore { + static final Logger LOGGER = LoggerFactory.getLogger(TrianglesStore.class); private static final int MAX_TRIANGLES = 200; - + public static void shrinkTo(final List list, final int newSize) { final int size = list.size(); if (newSize >= size) { @@ -18,52 +20,52 @@ public class TrianglesStore { list.remove(list.size() - 1); } } - + private final List triangles = new ArrayList<>(); - + /// Clear all the storage public void clear() { this.triangles.clear(); } - + /// Access operator public TriangleEPA get(final int id) { return this.triangles.get(id); } - + /// Return the number of triangles public int getNbTriangles() { return this.triangles.size(); } - + /// Return the last triangle public TriangleEPA last() { return this.triangles.get(this.triangles.size() - 1); } - + /// Create a new triangle public TriangleEPA newTriangle(final Vector3f[] vertices, final int v0, final int v1, final int v2) { - //Log.info("newTriangle: " + v0 + ", " + v1 + ", " + v2); + //LOGGER.info("newTriangle: " + v0 + ", " + v1 + ", " + v2); // If we have not reached the maximum number of triangles if (this.triangles.size() < TrianglesStore.MAX_TRIANGLES) { - + final TriangleEPA tmp = new TriangleEPA(v0, v1, v2); if (!tmp.computeClosestPoint(vertices)) { return null; } this.triangles.add(tmp); - //Log.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2)); + //LOGGER.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2)); return tmp; } // We are at the limit (internal) return null; - + } - + /// Set the number of triangles public void resize(final int backup) { if (backup > this.triangles.size()) { - Log.error("RESIZE BIGGER : " + backup + " > " + this.triangles.size()); + LOGGER.error("RESIZE BIGGER : " + backup + " > " + this.triangles.size()); } TrianglesStore.shrinkTo(this.triangles, backup); } diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java index c2d3570..4c5b6bd 100644 --- a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java +++ b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java @@ -52,7 +52,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { private Vector3f computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2, final NarrowPhaseCallback narrowPhaseCallback, Vector3f v) { - //Log.info("computePenetrationDepthForEnlargedObjects..."); + //LOGGER.info("computePenetrationDepthForEnlargedObjects..."); final Simplex simplex = new Simplex(); float distSquare = Float.MAX_VALUE; float prevDistSquare; @@ -62,59 +62,59 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape()); final Object shape1CachedCollisionData = shape1Info.cachedCollisionData(); final Object shape2CachedCollisionData = shape2Info.cachedCollisionData(); - //Log.info(" transform1=" + transform1); - //Log.info(" transform2=" + transform2); + //LOGGER.info(" transform1=" + transform1); + //LOGGER.info(" transform2=" + transform2); // Transform3D a point from local space of body 2 to local space // of body 1 (the GJK algorithm is done in local space of body 1) final Transform3D body2ToBody1 = transform1.inverseNew().multiply(transform2); // Matrix that transform a direction from local space of body 1 into local space of body 2 final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transpose().multiply(transform1.getOrientation().getMatrix()); - //Log.info(" body2ToBody1=" + body2ToBody1); - //Log.info(" rotateToBody2=" + rotateToBody2); - //Log.info(" v=" + v); + //LOGGER.info(" body2ToBody1=" + body2ToBody1); + //LOGGER.info(" rotateToBody2=" + rotateToBody2); + //LOGGER.info(" v=" + v); do { // Compute the support points for the enlarged object A and B final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiply(-1), (CacheData) shape1CachedCollisionData); - //Log.info(" suppA=" + suppA); + //LOGGER.info(" suppA=" + suppA); final Vector3f suppB = body2ToBody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v), (CacheData) shape2CachedCollisionData)); - //Log.info(" suppB=" + suppB); + //LOGGER.info(" suppB=" + suppB); // Compute the support point for the Minkowski difference A-B final Vector3f w = suppA.less(suppB); final float vDotw = v.dot(w); - //Log.info(" vDotw=" + vDotw); + //LOGGER.info(" vDotw=" + vDotw); // If the enlarge objects do not intersect if (vDotw > 0.0f) { - //Log.info(" ==> ret 1"); + //LOGGER.info(" ==> ret 1"); // No intersection, we return return v; } // Add the new support point to the simplex simplex.addPoint(w, suppA, suppB); if (simplex.isAffinelyDependent()) { - //Log.info(" ==> ret 2"); + //LOGGER.info(" ==> ret 2"); return v; } Vector3f v2 = simplex.computeClosestPoint(v); if (v2 == null) { - //Log.info(" ==> ret 3"); + //LOGGER.info(" ==> ret 3"); return v; } v = v2; // Store and update the square distance prevDistSquare = distSquare; - //Log.info(" distSquare=" + distSquare); + //LOGGER.info(" distSquare=" + distSquare); distSquare = v.length2(); - //Log.info(" distSquare=" + distSquare); + //LOGGER.info(" distSquare=" + distSquare); if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { - //Log.info(" ==> ret 4"); + //LOGGER.info(" ==> ret 4"); return v; } } while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); // Give the simplex computed with GJK algorithm to the EPA algorithm // which will compute the correct penetration depth and contact points // between the two enlarged objects - //Log.info(" ==> ret 5"); + //LOGGER.info(" ==> ret 5"); v = this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback); return v; } @@ -216,9 +216,9 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { @Override public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) { - //Log.error("================================================="); - //Log.error(" shape1Info=" + shape1Info.shapeToWorldTransform); - //Log.error(" shape2Info=" + shape2Info.shapeToWorldTransform); + //LOGGER.error("================================================="); + //LOGGER.error(" shape1Info=" + shape1Info.shapeToWorldTransform); + //LOGGER.error(" shape2Info=" + shape2Info.shapeToWorldTransform); Vector3f suppA = Vector3f.ZERO; // Support point of object A Vector3f suppB = Vector3f.ZERO; // Support point of object B Vector3f w = Vector3f.ZERO; // Support point of Minkowski difference A-B @@ -260,22 +260,22 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { // Initialize the upper bound for the square distance float distSquare = Float.MAX_VALUE; - //Log.error(" T1=" + transform1 + " T2=" + transform2); - //Log.error(" BT1=" + body2Tobody1 + " RT2=" + rotateToBody2); - //Log.error(" M=" + FMath.floatToString(margin) + " M2=" + FMath.floatToString(marginSquare)); - //Log.error(" v=" + cacheSeparatingAxis); + //LOGGER.error(" T1=" + transform1 + " T2=" + transform2); + //LOGGER.error(" BT1=" + body2Tobody1 + " RT2=" + rotateToBody2); + //LOGGER.error(" M=" + FMath.floatToString(margin) + " M2=" + FMath.floatToString(marginSquare)); + //LOGGER.error(" v=" + cacheSeparatingAxis); do { - //Log.error("------------------"); + //LOGGER.error("------------------"); // Compute the support points for original objects (without margins) A and B suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.value.multiply(-1.0f), shape1CachedCollisionData); suppB = body2Tobody1.multiply(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiply(cacheSeparatingAxis.value), shape2CachedCollisionData)); // Compute the support point for the Minkowski difference A-B w = suppA.less(suppB); vDotw = cacheSeparatingAxis.value.dot(w); - //Log.error(" suppA=" + suppA); - //Log.error(" suppB=" + suppB); - //Log.error(" w=" + w); + //LOGGER.error(" suppA=" + suppA); + //LOGGER.error(" suppB=" + suppB); + //LOGGER.error(" w=" + w); // If the enlarge objects (with margins) do not intersect if (vDotw > 0.0f && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence @@ -285,7 +285,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { } // If the objects intersect only in the margins if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * GJKAlgorithm.REL_ERROR_SQUARE) { - //Log.error("11111111 "); + //LOGGER.error("11111111 "); // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); // Project those two points on the margins to have the closest points of both @@ -312,7 +312,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { simplex.addPoint(w, suppA, suppB); // If the simplex is affinely dependent if (simplex.isAffinelyDependent()) { - //Log.error("222222 "); + //LOGGER.error("222222 "); // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); // Project those two points on the margins to have the closest points of both @@ -327,7 +327,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0f) { - //Log.info("penetration depth " + penetrationDepth); + //LOGGER.info("penetration depth " + penetrationDepth); return; } @@ -342,7 +342,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { // If the computation of the closest point fail Vector3f tmp = simplex.computeClosestPoint(cacheSeparatingAxis.value); if (tmp == null) { - //Log.error("3333333333 "); + //LOGGER.error("3333333333 "); // Compute the closet points of both objects (without the margins) simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); // Project those two points on the margins to have the closest points of both @@ -373,7 +373,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm { distSquare = cacheSeparatingAxis.value.length2(); // If the distance to the closest point doesn't improve a lot if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { - //Log.error("444444444 "); + //LOGGER.error("444444444 "); cacheSeparatingAxis.value = simplex.backupClosestPointInSimplex(); // Get the new squared distance diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java index 7a099e0..7bbc669 100644 --- a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java +++ b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java @@ -120,7 +120,7 @@ public class Simplex { /// point : support point of object (A-B) => point = suppPointA - suppPointB public void addPoint(final Vector3f point, final Vector3f suppPointA, final Vector3f suppPointB) { assert (!isFull()); - //Log.warning("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")"); + //LOGGER.warn("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")"); this.lastFound = 0; this.lastFoundBit = 0x1; @@ -130,8 +130,8 @@ public class Simplex { this.lastFound++; this.lastFoundBit <<= 1; } - //Log.warning(" this.lastFound " + this.lastFound); - //Log.warning(" this.lastFoundBit " + this.lastFoundBit); + //LOGGER.warn(" this.lastFound " + this.lastFound); + //LOGGER.warn(" this.lastFoundBit " + this.lastFoundBit); assert (this.lastFound < 4); @@ -139,7 +139,7 @@ public class Simplex { this.points[this.lastFound] = point; this.pointsLengthSquare[this.lastFound] = point.dot(point); this.allBits = this.bitsCurrentSimplex | this.lastFoundBit; - //Log.warning(" this.allBits " + this.allBits); + //LOGGER.warn(" this.allBits " + this.allBits); // Update the cached values updateCache(); @@ -249,7 +249,7 @@ public class Simplex { /// Compute the cached determinant values private void computeDeterminants() { this.det.set(this.lastFoundBit, this.lastFound, 1.0f); - //Log.warning("simplex: computeDeterminants() " + this.det.get(this.lastFoundBit, this.lastFound)); + //LOGGER.warn("simplex: computeDeterminants() " + this.det.get(this.lastFoundBit, this.lastFound)); // If the current simplex is not empty if (!isEmpty()) { // For each possible four points in the simplex set @@ -422,15 +422,15 @@ public class Simplex { /// Update the cached values used during the GJK algorithm private void updateCache() { - //Log.warning("simplex: update Cache()"); + //LOGGER.warn("simplex: update Cache()"); // For each of the four possible points of the simplex for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { - //Log.warning("simplex: iii=" + iii); - //Log.warning("simplex: bit=" + bit); + //LOGGER.warn("simplex: iii=" + iii); + //LOGGER.warn("simplex: bit=" + bit); // If the current points is in the simplex if (overlap(this.bitsCurrentSimplex, bit)) { - //Log.warning("simplex: ==> overlap"); + //LOGGER.warn("simplex: ==> overlap"); // Compute the distance between two points in the possible simplex set final Vector3f tmp = this.points[iii].less(this.points[this.lastFound]); this.diffLength.set(iii, this.lastFound, tmp); diff --git a/src/org/atriasoft/ephysics/collision/shapes/AABB.java b/src/org/atriasoft/ephysics/collision/shapes/AABB.java index 331187f..1f5fb12 100644 --- a/src/org/atriasoft/ephysics/collision/shapes/AABB.java +++ b/src/org/atriasoft/ephysics/collision/shapes/AABB.java @@ -170,10 +170,10 @@ public class AABB { */ public boolean testCollision(final AABB aabb) { if (this == aabb) { - ///Log.info("test : AABB ==> same object "); + ///LOGGER.info("test : AABB ==> same object "); return true; } - //Log.info("test : " + this + " && " + aabb); + //LOGGER.info("test : " + this + " && " + aabb); if (this.maxCoordinates.x() < aabb.minCoordinates.x() || aabb.maxCoordinates.x() < this.minCoordinates.x()) { return false; } @@ -183,7 +183,7 @@ public class AABB { if (this.maxCoordinates.y() < aabb.minCoordinates.y() || aabb.maxCoordinates.y() < this.minCoordinates.y()) { return false; } - //Log.info("detect collision "); + //LOGGER.info("detect collision "); return true; } diff --git a/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java b/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java index c06960b..61a8adc 100644 --- a/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java +++ b/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java @@ -76,8 +76,8 @@ public class BoxShape extends ConvexShape { */ @Override public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) { - //Log.error("getLocalSupportPointWithoutMargin(" + direction); - //Log.error(" extends = " + this.extent); + //LOGGER.error("getLocalSupportPointWithoutMargin(" + direction); + //LOGGER.error(" extends = " + this.extent); return new Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(), direction.y() < 0.0 ? -this.extent.y() : this.extent.y(), direction.z() < 0.0 ? -this.extent.z() : this.extent.z()); } diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java index 909aa1c..3ab79ce 100644 --- a/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java +++ b/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java @@ -13,10 +13,11 @@ import org.atriasoft.ephysics.collision.broadphase.CallbackRaycast; import org.atriasoft.ephysics.collision.broadphase.DTree; import org.atriasoft.ephysics.collision.broadphase.DynamicAABBTree; import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.etk.math.Matrix3f; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * Represents a static concave mesh shape. Note that collision detection @@ -24,6 +25,8 @@ import org.atriasoft.etk.math.Vector3f; * this shape for a static mesh. */ public class ConcaveMeshShape extends ConcaveShape { + static final Logger LOGGER = LoggerFactory.getLogger(ConcaveMeshShape.class); + class ConcaveMeshRaycastCallback { private final ConcaveMeshShape concaveMeshShape; private final DynamicAABBTree dynamicAABBTree; @@ -32,30 +35,31 @@ public class ConcaveMeshShape extends ConcaveShape { private final ProxyShape proxyShape; private final Ray ray; private final RaycastInfo raycastInfo; - + // Constructor - ConcaveMeshRaycastCallback(final DynamicAABBTree dynamicAABBTree, final ConcaveMeshShape concaveMeshShape, final ProxyShape proxyShape, final RaycastInfo raycastInfo, final Ray ray) { + ConcaveMeshRaycastCallback(final DynamicAABBTree dynamicAABBTree, final ConcaveMeshShape concaveMeshShape, + final ProxyShape proxyShape, final RaycastInfo raycastInfo, final Ray ray) { this.dynamicAABBTree = dynamicAABBTree; this.concaveMeshShape = concaveMeshShape; this.proxyShape = proxyShape; this.raycastInfo = raycastInfo; this.ray = ray; this.isHit = false; - + } - + /// Return true if a raycast hit has been found public boolean getIsHit() { return this.isHit; } - + /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree public float operatorparenthese(final DTree node, final Ray ray) { // Add the id of the hit AABB node longo this.hitAABBNodes.add(node); return ray.maxFraction; } - + /// Raycast all collision shapes that have been collected public void raycastTriangles() { float smallestHitFraction = this.ray.maxFraction; @@ -68,8 +72,9 @@ public class ConcaveMeshShape extends ConcaveShape { this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data0, data1, trianglePoints); // Create a triangle collision shape final float margin = this.concaveMeshShape.getTriangleMargin(); - - final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); + + final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], + trianglePoints[2], margin); triangleShape.setRaycastTestType(this.concaveMeshShape.getRaycastTestType()); // Ray casting test against the collision shape final RaycastInfo raycastInfo = new RaycastInfo(); @@ -89,20 +94,20 @@ public class ConcaveMeshShape extends ConcaveShape { } } } - + } - + protected DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles - + protected TriangleMesh triangleMesh; //!< Triangle mesh - + public ConcaveMeshShape(final TriangleMesh triangleMesh) { super(CollisionShapeType.CONCAVE_MESH); this.triangleMesh = triangleMesh; this.raycastTestType = TriangleRaycastSide.FRONT; initBVHTree(); } - + @Override public Matrix3f computeLocalInertiaTensor(final float mass) { // Default inertia tensor @@ -111,29 +116,32 @@ public class ConcaveMeshShape extends ConcaveShape { // the inertia tensor is not used. return new Matrix3f(mass, 0.0f, 0.0f, 0.0f, mass, 0.0f, 0.0f, 0.0f, mass); } - + @Override public Bounds getLocalBounds() { // Get the AABB of the whole tree final AABB treeAABB = this.dynamicAABBTree.getRootAABB(); return new Bounds(treeAABB.getMin(), treeAABB.getMax()); } - + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// given the start vertex index pointer of the triangle. - protected void getTriangleVerticesWithIndexPointer(final int subPart, final int triangleIndex, final Vector3f[] outTriangleVertices) { + protected void getTriangleVerticesWithIndexPointer( + final int subPart, + final int triangleIndex, + final Vector3f[] outTriangleVertices) { assert (outTriangleVertices != null); // Get the triangle vertex array of the current sub-part final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart); if (triangleVertexArray == null) { - Log.error("get null ..."); + LOGGER.error("get null ..."); } final Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex); outTriangleVertices[0] = trianglePoints.get(0).multiply(this.scaling); outTriangleVertices[1] = trianglePoints.get(1).multiply(this.scaling); outTriangleVertices[2] = trianglePoints.get(2).multiply(this.scaling); } - + /// Insert all the triangles longo the dynamic AABB tree protected void initBVHTree() { // TODO : Try to randomly add the triangles into the tree to obtain a better tree @@ -156,21 +164,22 @@ public class ConcaveMeshShape extends ConcaveShape { } } } - + @Override public boolean isConvex() { return false; } - + @Override public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) { // Create the callback object that will compute ray casting against triangles - final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(this.dynamicAABBTree, this, proxyShape, raycastInfo, ray); + final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(this.dynamicAABBTree, this, + proxyShape, raycastInfo, ray); // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. // The raycastCallback object will then compute ray casting against the triangles // in the hit AABBs. this.dynamicAABBTree.raycast(ray, new CallbackRaycast() { - + @Override public float callback(final DTree node, final Ray ray) { return raycastCallback.operatorparenthese(node, ray); @@ -179,14 +188,14 @@ public class ConcaveMeshShape extends ConcaveShape { raycastCallback.raycastTriangles(); return raycastCallback.getIsHit(); } - + @Override public void setLocalScaling(final Vector3f scaling) { super.setLocalScaling(scaling); this.dynamicAABBTree.reset(); initBVHTree(); } - + @Override public void testAllTriangles(final ConcaveShape.TriangleCallback callback, final AABB localAABB) { // Ask the Dynamic AABB Tree to report all the triangles that are overlapping diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java index b94ca81..f504d79 100644 --- a/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java +++ b/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java @@ -15,23 +15,23 @@ public abstract class ConvexShape extends CollisionShape { // Return a local support point in a given direction with the object margin public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) { - ////Log.error(" -> getLocalSupportPointWithMargin(" + direction); + ////LOGGER.error(" -> getLocalSupportPointWithMargin(" + direction); // Get the support point without margin Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); - ////Log.error(" -> supportPoint = " + supportPoint); - ////Log.error(" -> margin = " + FMath.floatToString(this.margin)); + ////LOGGER.error(" -> supportPoint = " + supportPoint); + ////LOGGER.error(" -> margin = " + FMath.floatToString(this.margin)); if (this.margin != 0.0f) { // Add the margin to the support point Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f); - ////Log.error(" -> direction.length2()=" + FMath.floatToString(direction.length2())); - ////Log.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON)); + ////LOGGER.error(" -> direction.length2()=" + FMath.floatToString(direction.length2())); + ////LOGGER.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON)); if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) { unitVec = direction.safeNormalize(); - ////Log.error(" -> unitVec= " + unitVec); + ////LOGGER.error(" -> unitVec= " + unitVec); } supportPoint = supportPoint.add(unitVec.multiply(this.margin)); } - ////Log.error(" -> ==> supportPoint = " + supportPoint); + ////LOGGER.error(" -> ==> supportPoint = " + supportPoint); return supportPoint; } diff --git a/src/org/atriasoft/ephysics/engine/ContactSolver.java b/src/org/atriasoft/ephysics/engine/ContactSolver.java index 0d53e31..15ffb51 100644 --- a/src/org/atriasoft/ephysics/engine/ContactSolver.java +++ b/src/org/atriasoft/ephysics/engine/ContactSolver.java @@ -119,21 +119,21 @@ public class ContactSolver { * @param manifold Constraint to apply the impulse */ private void applyImpulse(final Impulse impulse, final ContactManifoldSolver manifold) { - //Log.info(" -- manifold.massInverseBody1=" + manifold.massInverseBody1); - //Log.info(" -- manifold.inverseInertiaTensorBody1=" + manifold.inverseInertiaTensorBody1); - //Log.info(" -- manifold.massInverseBody2=" + manifold.massInverseBody2); - //Log.info(" -- manifold.inverseInertiaTensorBody2=" + manifold.inverseInertiaTensorBody2); - //Log.info(" -- impulse.angularImpulseBody2=" + impulse.angularImpulseBody2); + //LOGGER.info(" -- manifold.massInverseBody1=" + manifold.massInverseBody1); + //LOGGER.info(" -- manifold.inverseInertiaTensorBody1=" + manifold.inverseInertiaTensorBody1); + //LOGGER.info(" -- manifold.massInverseBody2=" + manifold.massInverseBody2); + //LOGGER.info(" -- manifold.inverseInertiaTensorBody2=" + manifold.inverseInertiaTensorBody2); + //LOGGER.info(" -- impulse.angularImpulseBody2=" + impulse.angularImpulseBody2); // Update the velocities of the body 1 by applying the impulse P this.linearVelocities[manifold.indexBody1] = this.linearVelocities[manifold.indexBody1].add(impulse.linearImpulseBody1().multiply(manifold.massInverseBody1)); - //Log.info(" -- this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]); + //LOGGER.info(" -- this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]); this.angularVelocities[manifold.indexBody1] = this.angularVelocities[manifold.indexBody1].add(manifold.inverseInertiaTensorBody1.multiply(impulse.angularImpulseBody1())); - //Log.info(" -- this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]); + //LOGGER.info(" -- this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]); // Update the velocities of the body 1 by applying the impulse P this.linearVelocities[manifold.indexBody2] = this.linearVelocities[manifold.indexBody2].add(impulse.linearImpulseBody2().multiply(manifold.massInverseBody2)); - //Log.info(" -- this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]); + //LOGGER.info(" -- this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]); this.angularVelocities[manifold.indexBody2] = this.angularVelocities[manifold.indexBody2].add(manifold.inverseInertiaTensorBody2.multiply(impulse.angularImpulseBody2())); - //Log.info(" -- this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]); + //LOGGER.info(" -- this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]); } /** @@ -164,8 +164,8 @@ public class ContactSolver { * @return Impulse of the friction result */ private Impulse computeFriction1Impulse(final float deltaLambda, final ContactPointSolver contactPoint) { - //Log.error("=== r1CrossT1=" + contactPoint.r1CrossT1); - //Log.error("=== r2CrossT1=" + contactPoint.r2CrossT1); + //LOGGER.error("=== r1CrossT1=" + contactPoint.r1CrossT1); + //LOGGER.error("=== r2CrossT1=" + contactPoint.r2CrossT1); return new Impulse(contactPoint.frictionVector1.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT1.multiply(-1).multiply(deltaLambda), contactPoint.frictionVector1.multiply(deltaLambda), contactPoint.r2CrossT1.multiply(deltaLambda)); } @@ -177,8 +177,8 @@ public class ContactSolver { * @return Impulse of the friction result */ private Impulse computeFriction2Impulse(final float deltaLambda, final ContactPointSolver contactPoint) { - //Log.error("=== r1CrossT2=" + contactPoint.r1CrossT2); - //Log.error("=== r2CrossT2=" + contactPoint.r2CrossT2); + //LOGGER.error("=== r1CrossT2=" + contactPoint.r1CrossT2); + //LOGGER.error("=== r2CrossT2=" + contactPoint.r2CrossT2); return new Impulse(contactPoint.frictionvec2.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT2.multiply(-1).multiply(deltaLambda), contactPoint.frictionvec2.multiply(deltaLambda), contactPoint.r2CrossT2.multiply(deltaLambda)); } @@ -200,11 +200,11 @@ public class ContactSolver { // Compute the first friction vector in the direction of the tangent // velocity difference contactManifold.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity); - //Log.error("===1==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1); + //LOGGER.error("===1==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1); } else { // Get any orthogonal vector to the normal as the first friction vector contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector(); - //Log.error("===2==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1); + //LOGGER.error("===2==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1); } // The second friction vector is computed by the cross product of the firs @@ -220,25 +220,25 @@ public class ContactSolver { */ private void computeFrictionVectors(final Vector3f deltaVelocity, final ContactPointSolver contactPoint) { assert (contactPoint.normal.length() > 0.0f); - //Log.error("===3==>>>>>> contactPoint.normal=" + contactPoint.normal); - //Log.error("===3==>>>>>> deltaVelocity=" + deltaVelocity); + //LOGGER.error("===3==>>>>>> contactPoint.normal=" + contactPoint.normal); + //LOGGER.error("===3==>>>>>> deltaVelocity=" + deltaVelocity); // Compute the velocity difference vector in the tangential plane final Vector3f normalVelocity = contactPoint.normal.multiply(deltaVelocity.dot(contactPoint.normal)); - //Log.error("===3==>>>>>> normalVelocity=" + normalVelocity); + //LOGGER.error("===3==>>>>>> normalVelocity=" + normalVelocity); final Vector3f tangentVelocity = deltaVelocity.less(normalVelocity); - //Log.error("===3==>>>>>> tangentVelocity=" + tangentVelocity); + //LOGGER.error("===3==>>>>>> tangentVelocity=" + tangentVelocity); // If the velocty difference in the tangential plane is not zero final float lengthTangenVelocity = tangentVelocity.length(); - //Log.error("===3==>>>>>> lengthTangenVelocity=" + FMath.floatToString(lengthTangenVelocity)); + //LOGGER.error("===3==>>>>>> lengthTangenVelocity=" + FMath.floatToString(lengthTangenVelocity)); if (lengthTangenVelocity > Constant.FLOAT_EPSILON) { // Compute the first friction vector in the direction of the tangent // velocity difference contactPoint.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity); - //Log.error("===3==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1); + //LOGGER.error("===3==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1); } else { // Get any orthogonal vector to the normal as the first friction vector contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector(); - //Log.error("===4==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1); + //LOGGER.error("===4==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1); } // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal @@ -265,8 +265,8 @@ public class ContactSolver { private float computeMixedRestitutionFactor(final RigidBody body1, final RigidBody body2) { final float restitution1 = body1.getMaterial().getBounciness(); final float restitution2 = body2.getMaterial().getBounciness(); - //Log.error("######################### restitution1=" + FMath.floatToString(restitution1)); - //Log.error("######################### restitution2=" + FMath.floatToString(restitution2)); + //LOGGER.error("######################### restitution1=" + FMath.floatToString(restitution1)); + //LOGGER.error("######################### restitution2=" + FMath.floatToString(restitution2)); // Return the largest restitution factor return (restitution1 > restitution2) ? restitution1 : restitution2; } @@ -288,8 +288,8 @@ public class ContactSolver { * @return Impulse of the penetration result */ private Impulse computePenetrationImpulse(final float deltaLambda, final ContactPointSolver contactPoint) { - //Log.error("=== r1CrossN=" + contactPoint.r1CrossN); - //Log.error("=== r2CrossN=" + contactPoint.r2CrossN); + //LOGGER.error("=== r1CrossN=" + contactPoint.r1CrossN); + //LOGGER.error("=== r2CrossN=" + contactPoint.r2CrossN); return new Impulse(contactPoint.normal.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossN.multiply(-1).multiply(deltaLambda), contactPoint.normal.multiply(deltaLambda), contactPoint.r2CrossN.multiply(deltaLambda)); } @@ -300,65 +300,65 @@ public class ContactSolver { private void initializeContactConstraints() { // For each contact raint for (int ccc = 0; ccc < this.contactConstraints.size(); ccc++) { - //Log.warning("}}}} ccc=" + ccc); + //LOGGER.warn("}}}} ccc=" + ccc); final ContactManifoldSolver manifold = this.contactConstraints.get(ccc); // Get the inertia tensors of both bodies final Matrix3f i1 = manifold.inverseInertiaTensorBody1; - //Log.warning("}}}} I1=" + I1); + //LOGGER.warn("}}}} I1=" + I1); final Matrix3f i2 = manifold.inverseInertiaTensorBody2; - //Log.warning("}}}} I2=" + I2); + //LOGGER.warn("}}}} I2=" + I2); // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { manifold.normal = Vector3f.ZERO; - //Log.error("]]] manifold.normal=" + manifold.normal); + //LOGGER.error("]]] manifold.normal=" + manifold.normal); } - //Log.warning("}}}} manifold.normal=" + manifold.normal); + //LOGGER.warn("}}}} manifold.normal=" + manifold.normal); // Get the velocities of the bodies final Vector3f v1 = this.linearVelocities[manifold.indexBody1]; final Vector3f w1 = this.angularVelocities[manifold.indexBody1]; final Vector3f v2 = this.linearVelocities[manifold.indexBody2]; final Vector3f w2 = this.angularVelocities[manifold.indexBody2]; - //Log.warning("}}}} v1=" + v1); - //Log.warning("}}}} w1=" + w1); - //Log.warning("}}}} v2=" + v2); - //Log.warning("}}}} w2=" + w2); + //LOGGER.warn("}}}} v1=" + v1); + //LOGGER.warn("}}}} w1=" + w1); + //LOGGER.warn("}}}} v2=" + v2); + //LOGGER.warn("}}}} w2=" + w2); // For each contact point raint for (int iii = 0; iii < manifold.nbContacts; iii++) { - //Log.warning("}}}} iii=" + iii); + //LOGGER.warn("}}}} iii=" + iii); final ContactPointSolver contactPoint = manifold.contacts[iii]; - //Log.warning("}}}} contactPoint.r1=" + contactPoint.r1); - //Log.warning("}}}} contactPoint.r2=" + contactPoint.r2); - //Log.warning("}}}} contactPoint.normal=" + contactPoint.normal); + //LOGGER.warn("}}}} contactPoint.r1=" + contactPoint.r1); + //LOGGER.warn("}}}} contactPoint.r2=" + contactPoint.r2); + //LOGGER.warn("}}}} contactPoint.normal=" + contactPoint.normal); final ContactPoint externalContact = contactPoint.externalContact; // Compute the velocity difference final Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); - //Log.warning("}}}} deltaV=" + deltaV); + //LOGGER.warn("}}}} deltaV=" + deltaV); contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal); contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); - //Log.warning("}}}} contactPoint.r1CrossN=" + contactPoint.r1CrossN); - //Log.warning("}}}} contactPoint.r2CrossN=" + contactPoint.r2CrossN); + //LOGGER.warn("}}}} contactPoint.r1CrossN=" + contactPoint.r1CrossN); + //LOGGER.warn("}}}} contactPoint.r2CrossN=" + contactPoint.r2CrossN); // Compute the inverse mass matrix K for the penetration raint final float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossN)).cross(contactPoint.r1)).dot(contactPoint.normal) + ((i2.multiply(contactPoint.r2CrossN)).cross(contactPoint.r2)).dot(contactPoint.normal); - //Log.warning("}}}} massPenetration=" + FMath.floatToString(massPenetration)); + //LOGGER.warn("}}}} massPenetration=" + FMath.floatToString(massPenetration)); if (massPenetration > 0.0f) { contactPoint.inversePenetrationMass = 1.0f / massPenetration; } - //Log.warning("}}}} contactPoint.inversePenetrationMass=" + FMath.floatToString(contactPoint.inversePenetrationMass)); + //LOGGER.warn("}}}} contactPoint.inversePenetrationMass=" + FMath.floatToString(contactPoint.inversePenetrationMass)); // If we do not solve the friction raints at the center of the contact manifold - //Log.warning("}}}} this.isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive); + //LOGGER.warn("}}}} this.isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive); if (!this.isSolveFrictionAtContactManifoldCenterActive) { // Compute the friction vectors computeFrictionVectors(deltaV, contactPoint); - //Log.warning("}}}} contactPoint.frictionVector1=" + contactPoint.frictionVector1); + //LOGGER.warn("}}}} contactPoint.frictionVector1=" + contactPoint.frictionVector1); contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2); contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2); - //Log.warning("}}}} contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); - //Log.warning("}}}} contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); - //Log.warning("}}}} contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); - //Log.warning("}}}} contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); + //LOGGER.warn("}}}} contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); + //LOGGER.warn("}}}} contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); + //LOGGER.warn("}}}} contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); + //LOGGER.warn("}}}} contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); // Compute the inverse mass matrix K for the friction // raints at each contact point final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossT1)).cross(contactPoint.r1)).dot(contactPoint.frictionVector1) @@ -377,16 +377,16 @@ public class ContactSolver { // at the beginning of the contact. Note that if it is a resting contact (normal // velocity bellow a given threshold), we do not add a restitution velocity bias contactPoint.restitutionBias = 0.0f; - //Log.warning("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + contactPoint.restitutionBias); + //LOGGER.warn("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + contactPoint.restitutionBias); final float deltaVDotN = deltaV.dot(contactPoint.normal); - //Log.warning("}}}}+++++++++++++++++++++ deltaV=" + deltaV); - //Log.warning("}}}}+++++++++++++++++++++ contactPoint.normal=" + contactPoint.normal); - //Log.warning("}}}}+++++++++++++++++++++ deltaVDotN=" + FMath.floatToString(deltaVDotN)); - //Log.warning("}}}}+++++++++++++++++++++ Configuration.RESTITUTIONVELOCITYTHRESHOLD=" + FMath.floatToString(Configuration.RESTITUTIONVELOCITYTHRESHOLD)); - //Log.warning("}}}}+++++++++++++++++++++ manifold.restitutionFactor=" + FMath.floatToString(manifold.restitutionFactor)); + //LOGGER.warn("}}}}+++++++++++++++++++++ deltaV=" + deltaV); + //LOGGER.warn("}}}}+++++++++++++++++++++ contactPoint.normal=" + contactPoint.normal); + //LOGGER.warn("}}}}+++++++++++++++++++++ deltaVDotN=" + FMath.floatToString(deltaVDotN)); + //LOGGER.warn("}}}}+++++++++++++++++++++ Configuration.RESTITUTIONVELOCITYTHRESHOLD=" + FMath.floatToString(Configuration.RESTITUTIONVELOCITYTHRESHOLD)); + //LOGGER.warn("}}}}+++++++++++++++++++++ manifold.restitutionFactor=" + FMath.floatToString(manifold.restitutionFactor)); if (deltaVDotN < -Configuration.RESTITUTION_VELOCITY_THRESHOLD) { contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; - //Log.warning("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); + //LOGGER.warn("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); } // If the warm starting of the contact solver is active if (this.isWarmStartingActive) { @@ -400,9 +400,9 @@ public class ContactSolver { contactPoint.penetrationSplitImpulse = 0.0f; // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { - //Log.error("]]] contactPoint.normal=" + contactPoint.normal); + //LOGGER.error("]]] contactPoint.normal=" + contactPoint.normal); manifold.normal = manifold.normal.add(contactPoint.normal); - //Log.error("]]] manifold.normal=" + manifold.normal); + //LOGGER.error("]]] manifold.normal=" + manifold.normal); } } // Compute the inverse K matrix for the rolling resistance raint @@ -413,11 +413,11 @@ public class ContactSolver { } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { - //Log.error("]]] manifold.normal=" + manifold.normal); + //LOGGER.error("]]] manifold.normal=" + manifold.normal); manifold.normal = manifold.normal.normalize(); - //Log.error("]]] manifold.normal=" + manifold.normal); + //LOGGER.error("]]] manifold.normal=" + manifold.normal); final Vector3f deltaVFrictionPoint = v2.add(w2.cross(manifold.r2Friction)).less(v1).less(w1.cross(manifold.r1Friction)); - //Log.error("]]] deltaVFrictionPoint=" + deltaVFrictionPoint); + //LOGGER.error("]]] deltaVFrictionPoint=" + deltaVFrictionPoint); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, manifold); // Compute the inverse mass matrix K for the friction raints at the center of @@ -426,30 +426,30 @@ public class ContactSolver { manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2); - //Log.warning("}}}} manifold.r1CrossT1=" + manifold.r1CrossT1); - //Log.warning("}}}} manifold.r1CrossT2=" + manifold.r1CrossT2); - //Log.warning("}}}} manifold.r2CrossT1=" + manifold.r2CrossT1); - //Log.warning("}}}} manifold.r2CrossT2=" + manifold.r2CrossT2); + //LOGGER.warn("}}}} manifold.r1CrossT1=" + manifold.r1CrossT1); + //LOGGER.warn("}}}} manifold.r1CrossT2=" + manifold.r1CrossT2); + //LOGGER.warn("}}}} manifold.r2CrossT1=" + manifold.r2CrossT1); + //LOGGER.warn("}}}} manifold.r2CrossT2=" + manifold.r2CrossT2); final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(manifold.r1CrossT1)).cross(manifold.r1Friction)).dot(manifold.frictionVector1) + ((i2.multiply(manifold.r2CrossT1)).cross(manifold.r2Friction)).dot(manifold.frictionVector1); - //Log.error("]]] friction1Mass=" + FMath.floatToString(friction1Mass)); + //LOGGER.error("]]] friction1Mass=" + FMath.floatToString(friction1Mass)); final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(manifold.r1CrossT2)).cross(manifold.r1Friction)).dot(manifold.frictionvec2) + ((i2.multiply(manifold.r2CrossT2)).cross(manifold.r2Friction)).dot(manifold.frictionvec2); - //Log.error("]]] friction2Mass=" + FMath.floatToString(friction2Mass)); + //LOGGER.error("]]] friction2Mass=" + FMath.floatToString(friction2Mass)); final float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1.multiply(manifold.normal)) + manifold.normal.dot(manifold.inverseInertiaTensorBody2.multiply(manifold.normal)); - //Log.error("]]] frictionTwistMass=" + FMath.floatToString(frictionTwistMass)); + //LOGGER.error("]]] frictionTwistMass=" + FMath.floatToString(frictionTwistMass)); if (friction1Mass > 0.0f) { manifold.inverseFriction1Mass = 1.0f / friction1Mass; - //Log.error("]]] manifold.inverseFriction1Mass=" + FMath.floatToString(manifold.inverseFriction1Mass)); + //LOGGER.error("]]] manifold.inverseFriction1Mass=" + FMath.floatToString(manifold.inverseFriction1Mass)); } if (friction2Mass > 0.0f) { manifold.inverseFriction2Mass = 1.0f / friction2Mass; - //Log.error("]]] manifold.inverseFriction2Mass=" + FMath.floatToString(manifold.inverseFriction2Mass)); + //LOGGER.error("]]] manifold.inverseFriction2Mass=" + FMath.floatToString(manifold.inverseFriction2Mass)); } if (frictionTwistMass > 0.0f) { manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass; - //Log.error("]]] manifold.inverseTwistFrictionMass=" + FMath.floatToString(manifold.inverseTwistFrictionMass)); + //LOGGER.error("]]] manifold.inverseTwistFrictionMass=" + FMath.floatToString(manifold.inverseTwistFrictionMass)); } } } @@ -508,8 +508,8 @@ public class ContactSolver { if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 = new Vector3f(0.0f, 0.0f, 0.0f); internalManifold.frictionPointBody2 = new Vector3f(0.0f, 0.0f, 0.0f); - //Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); - //Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); + //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); + //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); } // For each contact point of the contact manifold for (int ccc = 0; ccc < externalManifold.getNbContactPoints(); ++ccc) { @@ -536,33 +536,33 @@ public class ContactSolver { if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.add(p1); internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.add(p2); - //Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); - //Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); + //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); + //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); } } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.divide(internalManifold.nbContacts); internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.divide(internalManifold.nbContacts); - //Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); - //Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); + //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); + //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); internalManifold.r1Friction = internalManifold.frictionPointBody1.less(x1); internalManifold.r2Friction = internalManifold.frictionPointBody2.less(x2); - //Log.error("]]] internalManifold.r1Friction=" + internalManifold.r1Friction); - //Log.error("]]] internalManifold.r2Friction=" + internalManifold.r2Friction); + //LOGGER.error("]]] internalManifold.r1Friction=" + internalManifold.r1Friction); + //LOGGER.error("]]] internalManifold.r2Friction=" + internalManifold.r2Friction); internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1(); internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2(); - //Log.error("]]] internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1); - //Log.error("]]] internalManifold.oldFrictionvec2=" + internalManifold.oldFrictionvec2); + //LOGGER.error("]]] internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1); + //LOGGER.error("]]] internalManifold.oldFrictionvec2=" + internalManifold.oldFrictionvec2); // If warm starting is active if (this.isWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses internalManifold.friction1Impulse = externalManifold.getFrictionImpulse1(); internalManifold.friction2Impulse = externalManifold.getFrictionImpulse2(); internalManifold.frictionTwistImpulse = externalManifold.getFrictionTwistImpulse(); - //Log.error("]]] internalManifold.friction1Impulse=" + FMath.floatToString(internalManifold.friction1Impulse)); - //Log.error("]]] internalManifold.friction2Impulse=" + FMath.floatToString(internalManifold.friction2Impulse)); - //Log.error("]]] internalManifold.frictionTwistImpulse=" + FMath.floatToString(internalManifold.frictionTwistImpulse)); + //LOGGER.error("]]] internalManifold.friction1Impulse=" + FMath.floatToString(internalManifold.friction1Impulse)); + //LOGGER.error("]]] internalManifold.friction2Impulse=" + FMath.floatToString(internalManifold.friction2Impulse)); + //LOGGER.error("]]] internalManifold.frictionTwistImpulse=" + FMath.floatToString(internalManifold.frictionTwistImpulse)); } else { // Initialize the accumulated impulses to zero internalManifold.friction1Impulse = 0.0f; @@ -630,12 +630,12 @@ public class ContactSolver { * Solve the contacts */ public void solve() { - //Log.warning("========================================"); - //Log.warning("== Contact SOLVER ... " + this.contactConstraints.size()); - //Log.warning("========================================"); + //LOGGER.warn("========================================"); + //LOGGER.warn("== Contact SOLVER ... " + this.contactConstraints.size()); + //LOGGER.warn("========================================"); // For each contact manifold for (int ccc = 0; ccc < this.contactConstraints.size(); ++ccc) { - //Log.warning("ccc=" + ccc + " / " + this.contactConstraints.size()); + //LOGGER.warn("ccc=" + ccc + " / " + this.contactConstraints.size()); final ContactManifoldSolver contactManifold = this.contactConstraints.get(ccc); float sumPenetrationImpulse = 0.0f; // Get the rained velocities @@ -643,47 +643,47 @@ public class ContactSolver { final Vector3f w1 = this.angularVelocities[contactManifold.indexBody1]; final Vector3f v2 = this.linearVelocities[contactManifold.indexBody2]; final Vector3f w2 = this.angularVelocities[contactManifold.indexBody2]; - //Log.warning(" v1=" + v1); - //Log.warning(" w1=" + w1); - //Log.warning(" v2=" + v2); - //Log.warning(" w2=" + w2); + //LOGGER.warn(" v1=" + v1); + //LOGGER.warn(" w1=" + w1); + //LOGGER.warn(" v2=" + v2); + //LOGGER.warn(" w2=" + w2); for (int iii = 0; iii < contactManifold.nbContacts; ++iii) { - //Log.warning(" iii=" + iii); + //LOGGER.warn(" iii=" + iii); final ContactPointSolver contactPoint = contactManifold.contacts[iii]; // --------- Penetration --------- // // Compute J*v Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); final float deltaVDotN = deltaV.dot(contactPoint.normal); - //Log.warning(" contactPoint.R1=" + contactPoint.r1); - //Log.warning(" contactPoint.R2=" + contactPoint.r2); - //Log.warning(" contactPoint.r1CrossN=" + contactPoint.r1CrossN); - //Log.warning(" contactPoint.r2CrossN=" + contactPoint.r2CrossN); - //Log.warning(" contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); - //Log.warning(" contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); - //Log.warning(" contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); - //Log.warning(" contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); - //Log.warning(" contactPoint.penetrationDepth=" + FMath.floatToString(contactPoint.penetrationDepth)); - //Log.warning(" contactPoint.normal=" + contactPoint.normal); - //Log.warning(" deltaV=" + deltaV); + //LOGGER.warn(" contactPoint.R1=" + contactPoint.r1); + //LOGGER.warn(" contactPoint.R2=" + contactPoint.r2); + //LOGGER.warn(" contactPoint.r1CrossN=" + contactPoint.r1CrossN); + //LOGGER.warn(" contactPoint.r2CrossN=" + contactPoint.r2CrossN); + //LOGGER.warn(" contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); + //LOGGER.warn(" contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); + //LOGGER.warn(" contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); + //LOGGER.warn(" contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); + //LOGGER.warn(" contactPoint.penetrationDepth=" + FMath.floatToString(contactPoint.penetrationDepth)); + //LOGGER.warn(" contactPoint.normal=" + contactPoint.normal); + //LOGGER.warn(" deltaV=" + deltaV); float jv = deltaVDotN; // Compute the bias "b" of the raint final float beta = this.isSplitImpulseActive ? ContactSolver.BETA_SPLIT_IMPULSE : ContactSolver.BETA; - //Log.warning(" beta=" + FMath.floatToString(beta)); - //Log.warning(" BETA=" + FMath.floatToString(BETA)); - //Log.warning(" SLOP=" + FMath.floatToString(SLOP)); - //Log.warning(" this.timeStep=" + FMath.floatToString(this.timeStep)); - //Log.warning(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); + //LOGGER.warn(" beta=" + FMath.floatToString(beta)); + //LOGGER.warn(" BETA=" + FMath.floatToString(BETA)); + //LOGGER.warn(" SLOP=" + FMath.floatToString(SLOP)); + //LOGGER.warn(" this.timeStep=" + FMath.floatToString(this.timeStep)); + //LOGGER.warn(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); float biasPenetrationDepth = 0.0f; if (contactPoint.penetrationDepth > ContactSolver.SLOP) { - //Log.warning(" (beta / this.timeStep)=" + FMath.floatToString((beta / this.timeStep))); - //Log.warning(" FMath.max(0.0f, contactPoint.penetrationDepth - SLOP)=" + FMath.floatToString(FMath.max(0.0f, contactPoint.penetrationDepth - SLOP))); + //LOGGER.warn(" (beta / this.timeStep)=" + FMath.floatToString((beta / this.timeStep))); + //LOGGER.warn(" FMath.max(0.0f, contactPoint.penetrationDepth - SLOP)=" + FMath.floatToString(FMath.max(0.0f, contactPoint.penetrationDepth - SLOP))); biasPenetrationDepth = -(beta / this.timeStep) * FMath.max(0.0f, contactPoint.penetrationDepth - ContactSolver.SLOP); - //Log.warning(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); + //LOGGER.warn(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); } - //Log.warning(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); - //Log.warning(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); + //LOGGER.warn(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); + //LOGGER.warn(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); final float b = biasPenetrationDepth + contactPoint.restitutionBias; - //Log.warning(" b=" + FMath.floatToString(b)); + //LOGGER.warn(" b=" + FMath.floatToString(b)); // Compute the Lagrange multiplier lambda float deltaLambda; if (this.isSplitImpulseActive) { @@ -691,24 +691,24 @@ public class ContactSolver { } else { deltaLambda = -(jv + b) * contactPoint.inversePenetrationMass; } - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); float lambdaTemp = contactPoint.penetrationImpulse; contactPoint.penetrationImpulse = FMath.max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - //Log.warning(" contactPoint.penetrationImpulse=" + FMath.floatToString(contactPoint.penetrationImpulse)); - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" contactPoint.penetrationImpulse=" + FMath.floatToString(contactPoint.penetrationImpulse)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); // Compute the impulse P=J^T * lambda final Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); - //Log.warning(" impulsePenetration.a1=" + impulsePenetration.angularImpulseBody1); - //Log.warning(" impulsePenetration.a2=" + impulsePenetration.angularImpulseBody2); - //Log.warning(" impulsePenetration.i1=" + impulsePenetration.linearImpulseBody1); - //Log.warning(" impulsePenetration.i2=" + impulsePenetration.linearImpulseBody2); + //LOGGER.warn(" impulsePenetration.a1=" + impulsePenetration.angularImpulseBody1); + //LOGGER.warn(" impulsePenetration.a2=" + impulsePenetration.angularImpulseBody2); + //LOGGER.warn(" impulsePenetration.i1=" + impulsePenetration.linearImpulseBody1); + //LOGGER.warn(" impulsePenetration.i2=" + impulsePenetration.linearImpulseBody2); // Apply the impulse to the bodies of the raint applyImpulse(impulsePenetration, contactManifold); sumPenetrationImpulse += contactPoint.penetrationImpulse; - //Log.warning(" sumpenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); - //Log.warning(" isSplitImpulseActive=" + this.isSplitImpulseActive); - //Log.warning(" isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive); + //LOGGER.warn(" sumpenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); + //LOGGER.warn(" isSplitImpulseActive=" + this.isSplitImpulseActive); + //LOGGER.warn(" isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive); // If the split impulse position correction is active if (this.isSplitImpulseActive) { // Split impulse (position correction) @@ -717,7 +717,7 @@ public class ContactSolver { final Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; final Vector3f w2Split = this.splitAngularVelocities[contactManifold.indexBody2]; final Vector3f deltaVSplit = v2Split.add(w2Split.cross(contactPoint.r2)).less(v1Split).less(w1Split.cross(contactPoint.r1)); - //Log.warning(" deltaVSplit=" + deltaVSplit); + //LOGGER.warn(" deltaVSplit=" + deltaVSplit); final float jvSplit = deltaVSplit.dot(contactPoint.normal); final float deltaLambdaSplit = -(jvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; final float lambdaTempSplit = contactPoint.penetrationSplitImpulse; @@ -725,10 +725,10 @@ public class ContactSolver { deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; // Compute the impulse P=J^T * lambda final Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint); - //Log.warning(" splitImpulsePenetration.a1=" + splitImpulsePenetration.angularImpulseBody1); - //Log.warning(" splitImpulsePenetration.a2=" + splitImpulsePenetration.angularImpulseBody2); - //Log.warning(" splitImpulsePenetration.i1=" + splitImpulsePenetration.linearImpulseBody1); - //Log.warning(" splitImpulsePenetration.i2=" + splitImpulsePenetration.linearImpulseBody2); + //LOGGER.warn(" splitImpulsePenetration.a1=" + splitImpulsePenetration.angularImpulseBody1); + //LOGGER.warn(" splitImpulsePenetration.a2=" + splitImpulsePenetration.angularImpulseBody2); + //LOGGER.warn(" splitImpulsePenetration.i1=" + splitImpulsePenetration.linearImpulseBody1); + //LOGGER.warn(" splitImpulsePenetration.i2=" + splitImpulsePenetration.linearImpulseBody2); applySplitImpulse(splitImpulsePenetration, contactManifold); } // If we do not solve the friction raints at the center of the contact manifold @@ -740,19 +740,19 @@ public class ContactSolver { // Compute the Lagrange multiplier lambda deltaLambda = -jv; deltaLambda *= contactPoint.inverseFriction1Mass; - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction1Impulse; contactPoint.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); // Compute the impulse P=J^T * lambda final Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); - //Log.warning(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); - //Log.warning(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); - //Log.warning(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); - //Log.warning(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); + //LOGGER.warn(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); + //LOGGER.warn(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); + //LOGGER.warn(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); + //LOGGER.warn(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); @@ -763,18 +763,18 @@ public class ContactSolver { // Compute the Lagrange multiplier lambda deltaLambda = -jv; deltaLambda *= contactPoint.inverseFriction2Mass; - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction2Impulse; contactPoint.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); // Compute the impulse P=J^T * lambda final Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); - //Log.warning(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); - //Log.warning(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); - //Log.warning(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); - //Log.warning(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); + //LOGGER.warn(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); + //LOGGER.warn(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); + //LOGGER.warn(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); + //LOGGER.warn(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); // --------- Rolling resistance raint --------- // @@ -789,82 +789,82 @@ public class ContactSolver { deltaLambdaRolling = contactPoint.rollingResistanceImpulse.less(lambdaTempRolling); // Compute the impulse P=J^T * lambda final Impulse impulseRolling = new Impulse(Vector3f.ZERO, deltaLambdaRolling.multiply(-1), Vector3f.ZERO, deltaLambdaRolling); - //Log.warning(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); - //Log.warning(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); - //Log.warning(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); - //Log.warning(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); + //LOGGER.warn(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); + //LOGGER.warn(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); + //LOGGER.warn(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); + //LOGGER.warn(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); } } } - //Log.info(" w1=" + w1); - //Log.info(" w2=" + w2); + //LOGGER.info(" w1=" + w1); + //LOGGER.info(" w2=" + w2); // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { - //Log.warning(" HHH isSolveFrictionAtContactManifoldCenterActive"); - //Log.info(" v2=" + v2); + //LOGGER.warn(" HHH isSolveFrictionAtContactManifoldCenterActive"); + //LOGGER.info(" v2=" + v2); // ------ First friction raint at the center of the contact manifol ------ // // Compute J*v Vector3f deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction)); float jv = deltaV.dot(contactManifold.frictionVector1); - //Log.info(" v2=" + v2); - //Log.warning(" Jv=" + FMath.floatToString(Jv)); - //Log.warning(" contactManifold.frictionVector1=" + contactManifold.frictionVector1); - //Log.warning(" contactManifold.inverseFriction1Mass=" + FMath.floatToString(contactManifold.inverseFriction1Mass)); + //LOGGER.info(" v2=" + v2); + //LOGGER.warn(" Jv=" + FMath.floatToString(Jv)); + //LOGGER.warn(" contactManifold.frictionVector1=" + contactManifold.frictionVector1); + //LOGGER.warn(" contactManifold.inverseFriction1Mass=" + FMath.floatToString(contactManifold.inverseFriction1Mass)); // Compute the Lagrange multiplier lambda float deltaLambda = -jv * contactManifold.inverseFriction1Mass; - //Log.warning(" contactManifold.frictionCoefficient=" + FMath.floatToString(contactManifold.frictionCoefficient)); - //Log.warning(" sumPenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); - //Log.info(" v2=" + v2); + //LOGGER.warn(" contactManifold.frictionCoefficient=" + FMath.floatToString(contactManifold.frictionCoefficient)); + //LOGGER.warn(" sumPenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); + //LOGGER.info(" v2=" + v2); float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; float lambdaTemp = contactManifold.friction1Impulse; - //Log.warning(" frictionLimit=" + FMath.floatToString(frictionLimit)); - //Log.warning(" lambdaTemp=" + FMath.floatToString(lambdaTemp)); - //Log.info(" v2=" + v2); + //LOGGER.warn(" frictionLimit=" + FMath.floatToString(frictionLimit)); + //LOGGER.warn(" lambdaTemp=" + FMath.floatToString(lambdaTemp)); + //LOGGER.info(" v2=" + v2); contactManifold.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction1Impulse - lambdaTemp; - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); // Compute the impulse P=J^T * lambda Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(deltaLambda); Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiply(-1).multiply(deltaLambda); Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(deltaLambda); Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(deltaLambda); - //Log.info(" v2=" + v2); + //LOGGER.info(" v2=" + v2); final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); - //Log.warning(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); - //Log.warning(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); - //Log.warning(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); - //Log.warning(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); + //LOGGER.warn(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); + //LOGGER.warn(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); + //LOGGER.warn(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); + //LOGGER.warn(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); - //Log.info(" v2=" + v2); - //Log.info(" w2=" + w2); - //Log.info(" v1=" + v1); - //Log.info(" w1=" + w1); - //Log.info(" contactManifold.r2Friction=" + contactManifold.r2Friction); - //Log.info(" contactManifold.r1Friction=" + contactManifold.r1Friction); + //LOGGER.info(" v2=" + v2); + //LOGGER.info(" w2=" + w2); + //LOGGER.info(" v1=" + v1); + //LOGGER.info(" w1=" + w1); + //LOGGER.info(" contactManifold.r2Friction=" + contactManifold.r2Friction); + //LOGGER.info(" contactManifold.r1Friction=" + contactManifold.r1Friction); // ------ Second friction raint at the center of the contact manifol ----- // // Compute J*v deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction)); - //Log.warning(">>>> deltaV=" + deltaV); + //LOGGER.warn(">>>> deltaV=" + deltaV); jv = deltaV.dot(contactManifold.frictionvec2); - //Log.warning(">>>> Jv=" + FMath.floatToString(Jv)); - //Log.warning(">>>> contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass)); + //LOGGER.warn(">>>> Jv=" + FMath.floatToString(Jv)); + //LOGGER.warn(">>>> contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass)); // Compute the Lagrange multiplier lambda deltaLambda = -jv * contactManifold.inverseFriction2Mass; - //Log.warning(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; lambdaTemp = contactManifold.friction2Impulse; - //Log.warning(">>>> lambdaTemp=" + FMath.floatToString(lambdaTemp)); - //Log.warning(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); - //Log.warning(">>>>** frictionLimit=" + FMath.floatToString(frictionLimit)); + //LOGGER.warn(">>>> lambdaTemp=" + FMath.floatToString(lambdaTemp)); + //LOGGER.warn(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); + //LOGGER.warn(">>>>** frictionLimit=" + FMath.floatToString(frictionLimit)); contactManifold.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); - //Log.warning(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); + //LOGGER.warn(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); deltaLambda = contactManifold.friction2Impulse - lambdaTemp; - //Log.warning(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); // Compute the impulse P=J^T * lambda linearImpulseBody1 = contactManifold.frictionvec2.multiply(-deltaLambda); angularImpulseBody1 = contactManifold.r1CrossT2.multiply(-deltaLambda); @@ -872,36 +872,36 @@ public class ContactSolver { angularImpulseBody2 = contactManifold.r2CrossT2.multiply(deltaLambda); final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); - //Log.warning(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); - //Log.warning(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); - //Log.warning(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); - //Log.warning(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); + //LOGGER.warn(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); + //LOGGER.warn(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); + //LOGGER.warn(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); + //LOGGER.warn(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); - //Log.info(" v2=" + v2); - //Log.info(" w2=" + w2); - //Log.info(" v1=" + v1); - //Log.info(" w1=" + w1); + //LOGGER.info(" v2=" + v2); + //LOGGER.info(" w2=" + w2); + //LOGGER.info(" v1=" + v1); + //LOGGER.info(" w1=" + w1); // ------ Twist friction raint at the center of the contact manifol ------ // // Compute J*v deltaV = w2.less(w1); - //Log.warning(" deltaV=" + deltaV); - //Log.warning(" contactManifold.normal=" + contactManifold.normal); + //LOGGER.warn(" deltaV=" + deltaV); + //LOGGER.warn(" contactManifold.normal=" + contactManifold.normal); jv = deltaV.dot(contactManifold.normal); - //Log.warning(" Jv=" + FMath.floatToString(Jv)); - //Log.warning(" contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass); + //LOGGER.warn(" Jv=" + FMath.floatToString(Jv)); + //LOGGER.warn(" contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass); deltaLambda = -jv * (contactManifold.inverseTwistFrictionMass); - //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda)); frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; - //Log.warning(" contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient); - //Log.warning(" sumPenetrationImpulse=" + sumPenetrationImpulse); - //Log.warning(" frictionLimit=" + FMath.floatToString(frictionLimit)); + //LOGGER.warn(" contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient); + //LOGGER.warn(" sumPenetrationImpulse=" + sumPenetrationImpulse); + //LOGGER.warn(" frictionLimit=" + FMath.floatToString(frictionLimit)); lambdaTemp = contactManifold.frictionTwistImpulse; - //Log.warning(" lambdaTemp=" + lambdaTemp); + //LOGGER.warn(" lambdaTemp=" + lambdaTemp); contactManifold.frictionTwistImpulse = FMath.max(-frictionLimit, FMath.min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); - //Log.warning(" contactManifold.frictionTwistImpulse=" + contactManifold.frictionTwistImpulse); + //LOGGER.warn(" contactManifold.frictionTwistImpulse=" + contactManifold.frictionTwistImpulse); deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - //Log.warning(" deltaLambda=" + deltaLambda); + //LOGGER.warn(" deltaLambda=" + deltaLambda); // Compute the impulse P=J^T * lambda linearImpulseBody1 = Vector3f.ZERO; angularImpulseBody1 = contactManifold.normal.multiply(-deltaLambda); @@ -909,10 +909,10 @@ public class ContactSolver { angularImpulseBody2 = contactManifold.normal.multiply(deltaLambda); final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); - //Log.warning(" impulseTwistFriction.a1=" + impulseTwistFriction.angularImpulseBody1); - //Log.warning(" impulseTwistFriction.a2=" + impulseTwistFriction.angularImpulseBody2); - //Log.warning(" impulseTwistFriction.i1=" + impulseTwistFriction.linearImpulseBody1); - //Log.warning(" impulseTwistFriction.i2=" + impulseTwistFriction.linearImpulseBody2); + //LOGGER.warn(" impulseTwistFriction.a1=" + impulseTwistFriction.angularImpulseBody1); + //LOGGER.warn(" impulseTwistFriction.a2=" + impulseTwistFriction.angularImpulseBody2); + //LOGGER.warn(" impulseTwistFriction.i1=" + impulseTwistFriction.linearImpulseBody1); + //LOGGER.warn(" impulseTwistFriction.i2=" + impulseTwistFriction.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseTwistFriction, contactManifold); // --------- Rolling resistance raint at the center of the contact manifold --------- // @@ -930,10 +930,10 @@ public class ContactSolver { angularImpulseBody2 = deltaLambdaRolling; final Impulse impulseRolling = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2); - //Log.warning(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); - //Log.warning(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); - //Log.warning(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); - //Log.warning(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); + //LOGGER.warn(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); + //LOGGER.warn(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); + //LOGGER.warn(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); + //LOGGER.warn(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); } @@ -1036,11 +1036,11 @@ public class ContactSolver { final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiply(contactManifold.friction1Impulse) .add(contactManifold.oldFrictionvec2.multiply(contactManifold.friction2Impulse)); - //Log.error("]]] oldFrictionImpulse=" + oldFrictionImpulse); + //LOGGER.error("]]] oldFrictionImpulse=" + oldFrictionImpulse); contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2); - //Log.error("]]] contactManifold.friction1Impulse=" + contactManifold.friction1Impulse); - //Log.error("]]] contactManifold.friction2Impulse=" + contactManifold.friction2Impulse); + //LOGGER.error("]]] contactManifold.friction1Impulse=" + contactManifold.friction1Impulse); + //LOGGER.error("]]] contactManifold.friction2Impulse=" + contactManifold.friction2Impulse); // ------ First friction raint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(contactManifold.friction1Impulse); @@ -1048,10 +1048,10 @@ public class ContactSolver { Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(contactManifold.friction1Impulse); Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(contactManifold.friction1Impulse); - //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); - //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); - //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); - //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2); final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction1, contactManifold); @@ -1062,10 +1062,10 @@ public class ContactSolver { linearImpulseBody2 = contactManifold.frictionvec2.multiply(contactManifold.friction2Impulse); angularImpulseBody2 = contactManifold.r2CrossT2.multiply(contactManifold.friction2Impulse); - //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); - //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); - //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); - //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2); final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseFriction2, contactManifold); @@ -1076,10 +1076,10 @@ public class ContactSolver { linearImpulseBody2 = Vector3f.ZERO; angularImpulseBody2 = contactManifold.normal.multiply(contactManifold.frictionTwistImpulse); - //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); - //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); - //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); - //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2); final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseTwistFriction, contactManifold); @@ -1088,8 +1088,8 @@ public class ContactSolver { angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiply(-1); angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); - //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2); final Impulse impulseRollingResistance = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRollingResistance, contactManifold); diff --git a/src/org/atriasoft/ephysics/engine/DynamicsWorld.java b/src/org/atriasoft/ephysics/engine/DynamicsWorld.java index 0535b07..8512e23 100644 --- a/src/org/atriasoft/ephysics/engine/DynamicsWorld.java +++ b/src/org/atriasoft/ephysics/engine/DynamicsWorld.java @@ -27,12 +27,13 @@ import org.atriasoft.ephysics.constraint.JointListElement; import org.atriasoft.ephysics.constraint.JointsPositionCorrectionTechnique; import org.atriasoft.ephysics.constraint.SliderJoint; import org.atriasoft.ephysics.constraint.SliderJointInfo; -import org.atriasoft.ephysics.internal.Log; import org.atriasoft.ephysics.mathematics.Set; import org.atriasoft.etk.math.FMath; import org.atriasoft.etk.math.Quaternion; import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Vector3f; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * This class represents a dynamics world. This class inherits from @@ -40,6 +41,7 @@ import org.atriasoft.etk.math.Vector3f; * and their movements are simulated using the laws of physics. */ public class DynamicsWorld extends CollisionWorld { + static final Logger LOGGER = LoggerFactory.getLogger(DynamicsWorld.class); private static int kkk = 0; protected ContactSolver contactSolver; //!< Contact solver protected Vector3f gravity = Vector3f.ZERO; //!< Gravity vector of the world @@ -60,11 +62,11 @@ public class DynamicsWorld extends CollisionWorld { protected float sleepAngularVelocity; //!< Sleep angular velocity threshold protected float sleepLinearVelocity; //!< Sleep linear velocity threshold protected Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) - + protected Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) protected float timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity. public float timeStep; //!< Current frame time step (in seconds) - + /** * Constructor * @param gravity Gravity vector in the world (in meters per second squared) @@ -82,14 +84,14 @@ public class DynamicsWorld extends CollisionWorld { this.sleepAngularVelocity = Configuration.DEFAULT_SLEEP_ANGULAR_VELOCITY; this.timeBeforeSleep = Configuration.DEFAULT_TIME_BEFORE_SLEEP; } - + /** * Add the joint to the list of joints of the two bodies involved in the joint * @param joint Joint to add at the body. */ protected void addJointToBody(final Joint joint) { if (joint == null) { - //Log.warning("Request add null joint"); + //LOGGER.warn("Request add null joint"); return; } // Add the joint at the beginning of the linked list of joints of the first body @@ -97,7 +99,7 @@ public class DynamicsWorld extends CollisionWorld { // Add the joint at the beginning of the linked list of joints of the second body joint.getBody2().jointsList = new JointListElement(joint, joint.getBody2().jointsList); } - + /** * Compute the islands of awake bodies. * An island is an isolated group of rigid bodies that have raints (joints or contacts) @@ -125,7 +127,7 @@ public class DynamicsWorld extends CollisionWorld { for (int iii = 0; iii < nbBodies; iii++) { stackBodiesToVisit.add(null); } - + // For each rigid body of the world for (final RigidBody body : this.rigidBodies) { // If the body has already been added to an island, we go to the next body @@ -164,7 +166,8 @@ public class DynamicsWorld extends CollisionWorld { } // For each contact manifold in which the current body is involded ContactManifoldListElement contactElement; - for (contactElement = bodyToVisit.contactManifoldsList; contactElement != null; contactElement = contactElement.next) { + for (contactElement = bodyToVisit.contactManifoldsList; contactElement != null; + contactElement = contactElement.next) { final ContactManifold contactManifold = contactElement.contactManifold; assert (contactManifold.getNbContactPoints() > 0); // Check if the current contact manifold has already been added into an island @@ -215,7 +218,7 @@ public class DynamicsWorld extends CollisionWorld { this.islands.get(this.islands.size() - 1).resetStaticBobyNotInIsland(); } } - + /** * Create a joint between two bodies in the world and return a pointer to the new joint * @param jointInfo The information that is necessary to create the joint @@ -257,7 +260,7 @@ public class DynamicsWorld extends CollisionWorld { // Return the pointer to the created joint return newJoint; } - + /** * Create a rigid body into the physics world * @param transform Transform3Dation from body local-space to world-space @@ -277,14 +280,14 @@ public class DynamicsWorld extends CollisionWorld { // Return the pointer to the rigid body return rigidBody; } - + /** * Destroy a joint * @param joint Pointer to the joint you want to destroy */ public void destroyJoint(Joint joint) { if (joint == null) { - //Log.warning("Request destroy null joint"); + //LOGGER.warn("Request destroy null joint"); return; } // If the collision between the two bodies of the raint was disabled @@ -302,7 +305,7 @@ public class DynamicsWorld extends CollisionWorld { joint.getBody2().removeJointFromjointsList(joint); joint = null; } - + /** * Destroy a rigid body and all the joints which it beints * @param rigidBody Pointer to the body you want to destroy @@ -323,7 +326,7 @@ public class DynamicsWorld extends CollisionWorld { this.rigidBodies.remove(rigidBody); rigidBody = null; } - + /** * Enable/Disable the sleeping technique. * The sleeping technique is used to put bodies that are not moving into sleep @@ -340,7 +343,7 @@ public class DynamicsWorld extends CollisionWorld { } } } - + /** * Get list of all contacts. * @return The list of all contacts of the world @@ -360,7 +363,7 @@ public class DynamicsWorld extends CollisionWorld { // Return all the contact manifold return contactManifolds; } - + /** * Get the gravity vector of the world * @return The current gravity vector (in meter per seconds squared) @@ -368,18 +371,18 @@ public class DynamicsWorld extends CollisionWorld { public Vector3f getGravity() { return this.gravity; } - + public List getIslands() { return this.islands; } - + /** * Get the number of iterations for the position raint solver */ public int getNbIterationsPositionSolver() { return this.nbPositionSolverIterations; } - + /** * Get the number of iterations for the velocity raint solver * @return Number if iteration. @@ -387,7 +390,7 @@ public class DynamicsWorld extends CollisionWorld { public int getNbIterationsVelocitySolver() { return this.nbVelocitySolverIterations; } - + /** * Get the number of all joints * @return Number of joints in the world @@ -395,7 +398,7 @@ public class DynamicsWorld extends CollisionWorld { public int getNbJoints() { return this.joints.size(); } - + /** * Get the number of rigid bodies in the world * @return Number of rigid bodies in the world @@ -403,7 +406,7 @@ public class DynamicsWorld extends CollisionWorld { public int getNbRigidBodies() { return this.rigidBodies.size(); } - + /** * Get an iterator to the beginning of the bodies of the physics world * @return Starting iterator of the set of rigid bodies @@ -411,7 +414,7 @@ public class DynamicsWorld extends CollisionWorld { public List getRigidBodies() { return this.rigidBodies; } - + /** * Return the current sleep angular velocity * @return The sleep angular velocity (in radian per second) @@ -419,7 +422,7 @@ public class DynamicsWorld extends CollisionWorld { public float getSleepAngularVelocity() { return this.sleepAngularVelocity; } - + /** * Get the sleep linear velocity * @return The current sleep linear velocity (in meters per second) @@ -427,7 +430,7 @@ public class DynamicsWorld extends CollisionWorld { public float getSleepLinearVelocity() { return this.sleepLinearVelocity; } - + /** * Get the time a body is required to stay still before sleeping * @return Time a body is required to stay still before sleeping (in seconds) @@ -435,7 +438,7 @@ public class DynamicsWorld extends CollisionWorld { public float getTimeBeforeSleep() { return this.timeBeforeSleep; } - + /** * Initialize the bodies velocities arrays for the next simulation step. */ @@ -479,25 +482,25 @@ public class DynamicsWorld extends CollisionWorld { indexBody++; } } - + /** * Integrate position and orientation of the rigid bodies. * The positions and orientations of the bodies are integrated using * the sympletic Euler time stepping scheme. */ protected void integrateRigidBodiesPositions() { - //Log.error("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++integrateRigidBodiesPositions"); + //LOGGER.error("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++integrateRigidBodiesPositions"); // For each island of the world - for (int i = 0; i < this.islands.size(); i++) { - final List bodies = this.islands.get(i).getBodies(); + for (final Island element : this.islands) { + final List bodies = element.getBodies(); // For each body of the island - for (int b = 0; b < bodies.size(); b++) { - //Log.error(" [" + b + "/" + bodies.size() + "]"); + for (final RigidBody element2 : bodies) { + //LOGGER.error(" [" + b + "/" + bodies.size() + "]"); // Get the rained velocity - final int indexArray = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b)); + final int indexArray = this.mapBodyToConstrainedVelocityIndex.get(element2); Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray]; Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray]; - //Log.error(" newAngVelocity = " + newAngVelocity); + //LOGGER.error(" newAngVelocity = " + newAngVelocity); // Add the split impulse velocity from Contact Solver (only used // to update the position) if (this.contactSolver.isSplitImpulseActive()) { @@ -505,32 +508,35 @@ public class DynamicsWorld extends CollisionWorld { newAngVelocity = newAngVelocity.add(this.splitAngularVelocities[indexArray]); } // Get current position and orientation of the body - final Vector3f currentPosition = bodies.get(b).centerOfMassWorld; - //Log.error(" bodies.get(b).getTransform() = " + bodies.get(b).getTransform()); - final Quaternion currentOrientation = bodies.get(b).getTransform().getOrientation(); + final Vector3f currentPosition = element2.centerOfMassWorld; + //LOGGER.error(" bodies.get(b).getTransform() = " + bodies.get(b).getTransform()); + final Quaternion currentOrientation = element2.getTransform().getOrientation(); // Update the new rained position and orientation of the body this.rainedPositions[indexArray] = newLinVelocity.multiply(this.timeStep).add(currentPosition); - //Log.error(" currentOrientation = " + currentOrientation); - //Log.error(" newAngVelocity = " + newAngVelocity); - //Log.error(" this.timeStep = " + FMath.floatToString(this.timeStep)); - this.rainedOrientations[indexArray] = currentOrientation.add(new Quaternion(0, newAngVelocity).multiply(currentOrientation).multiply(0.5f * this.timeStep)); - //Log.error(" this.rainedPositions[indexArray] = " + this.rainedPositions[indexArray]); - //Log.error(" this.rainedOrientations[indexArray] = " + this.rainedOrientations[indexArray]); + //LOGGER.error(" currentOrientation = " + currentOrientation); + //LOGGER.error(" newAngVelocity = " + newAngVelocity); + //LOGGER.error(" this.timeStep = " + FMath.floatToString(this.timeStep)); + this.rainedOrientations[indexArray] = currentOrientation.add( + new Quaternion(0, newAngVelocity).multiply(currentOrientation).multiply(0.5f * this.timeStep)); + //LOGGER.error(" this.rainedPositions[indexArray] = " + this.rainedPositions[indexArray]); + //LOGGER.error(" this.rainedOrientations[indexArray] = " + this.rainedOrientations[indexArray]); } } for (int iii = 1; iii < this.rainedPositions.length; iii++) { - Log.error(DynamicsWorld.kkk + " this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]); - Log.error(DynamicsWorld.kkk + " this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]); + LOGGER.error( + DynamicsWorld.kkk + " this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]); + LOGGER.error( + DynamicsWorld.kkk + " this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]); } - //Log.error("------------------------------------------------------------------------------------------------"); - //Log.error("------------------------------------------------------------------------------------------------"); - //Log.error("------------------------------------------------------------------------------------------------"); - //Log.error("------------------------------------------------------------------------------------------------"); + //LOGGER.error("------------------------------------------------------------------------------------------------"); + //LOGGER.error("------------------------------------------------------------------------------------------------"); + //LOGGER.error("------------------------------------------------------------------------------------------------"); + //LOGGER.error("------------------------------------------------------------------------------------------------"); if (DynamicsWorld.kkk++ == 98) { //System.exit(-1); } } - + /** * Integrate the velocities of rigid bodies. * This method only set the temporary velocities but does not update @@ -541,33 +547,32 @@ public class DynamicsWorld extends CollisionWorld { protected void integrateRigidBodiesVelocities() { // Initialize the bodies velocity arrays initVelocityArrays(); - //Log.info("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"); + //LOGGER.info("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"); // For each island of the world - for (int iii = 0; iii < this.islands.size(); iii++) { - //Log.info("Manage island : " + iii + "/" + this.islands.size()); - final List bodies = this.islands.get(iii).getBodies(); + for (final Island element : this.islands) { + //LOGGER.info("Manage island : " + iii + "/" + this.islands.size()); + final List bodies = element.getBodies(); // For each body of the island - for (int bbb = 0; bbb < bodies.size(); bbb++) { - //Log.info(" body : " + bbb + "/" + bodies.size()); - // Insert the body into the map of rained velocities - final RigidBody tmpval = bodies.get(bbb); + for (final RigidBody tmpval : bodies) { final int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval); - //Log.info(" indexBody=" + indexBody); - + //LOGGER.info(" indexBody=" + indexBody); + assert (this.splitLinearVelocities[indexBody].isZero()); assert (this.splitAngularVelocities[indexBody].isZero()); // Integrate the external force to get the new velocity of the body - this.rainedLinearVelocities[indexBody] = bodies.get(bbb).getLinearVelocity().add(bodies.get(bbb).externalForce.multiply(this.timeStep * bodies.get(bbb).massInverse)); - //Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); - this.rainedAngularVelocities[indexBody] = bodies.get(bbb).getAngularVelocity() - .add(bodies.get(bbb).getInertiaTensorInverseWorld().multiply(bodies.get(bbb).externalTorque.multiply(this.timeStep))); - //Log.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); + this.rainedLinearVelocities[indexBody] = tmpval.getLinearVelocity() + .add(tmpval.externalForce.multiply(this.timeStep * tmpval.massInverse)); + //LOGGER.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + this.rainedAngularVelocities[indexBody] = tmpval.getAngularVelocity().add( + tmpval.getInertiaTensorInverseWorld().multiply(tmpval.externalTorque.multiply(this.timeStep))); + //LOGGER.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); // If the gravity has to be applied to this rigid body - if (bodies.get(bbb).isGravityEnabled() && this.isGravityEnabled) { + if (tmpval.isGravityEnabled() && this.isGravityEnabled) { // Integrate the gravity force - this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody].add(this.gravity.multiply(this.timeStep * bodies.get(bbb).massInverse * bodies.get(bbb).getMass())); + this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody] + .add(this.gravity.multiply(this.timeStep * tmpval.massInverse * tmpval.getMass())); } - //Log.info(" G this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + //LOGGER.info(" G this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); // Apply the velocity damping // Damping force : Fc = -c' * v (c=damping factor) // Equation : m * dv/dt = -c' * v @@ -581,22 +586,23 @@ public class DynamicsWorld extends CollisionWorld { // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // => e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) - final float linDampingFactor = bodies.get(bbb).getLinearDamping(); - //Log.info(" linDampingFactor=" + FMath.floatToString(linDampingFactor)); - final float angDampingFactor = bodies.get(bbb).getAngularDamping(); - //Log.info(" angDampingFactor=" + FMath.floatToString(angDampingFactor)); + final float linDampingFactor = tmpval.getLinearDamping(); + //LOGGER.info(" linDampingFactor=" + FMath.floatToString(linDampingFactor)); + final float angDampingFactor = tmpval.getAngularDamping(); + //LOGGER.info(" angDampingFactor=" + FMath.floatToString(angDampingFactor)); final float linearDamping = FMath.pow(1.0f - linDampingFactor, this.timeStep); - //Log.info(" linearDamping=" + FMath.floatToString(linearDamping)); + //LOGGER.info(" linearDamping=" + FMath.floatToString(linearDamping)); final float angularDamping = FMath.pow(1.0f - angDampingFactor, this.timeStep); - //Log.info(" angularDamping=" + FMath.floatToString(angularDamping)); + //LOGGER.info(" angularDamping=" + FMath.floatToString(angularDamping)); this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody].multiply(linearDamping); - //Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); - this.rainedAngularVelocities[indexBody] = this.rainedAngularVelocities[indexBody].multiply(angularDamping); - //Log.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); + //LOGGER.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + this.rainedAngularVelocities[indexBody] = this.rainedAngularVelocities[indexBody] + .multiply(angularDamping); + //LOGGER.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); } } } - + /** * Get if the gravity is enaled * @return True if the gravity is enabled in the world @@ -604,7 +610,7 @@ public class DynamicsWorld extends CollisionWorld { public boolean isGravityEnabled() { return this.isGravityEnabled; } - + /** * Get if the sleeping technique is enabled * @return True if the sleeping technique is enabled and false otherwise @@ -612,7 +618,7 @@ public class DynamicsWorld extends CollisionWorld { public boolean isSleepingEnabled() { return this.isSleepingEnabled; } - + /** * Reset the external force and torque applied to the bodies */ @@ -623,7 +629,7 @@ public class DynamicsWorld extends CollisionWorld { obj.externalTorque = Vector3f.ZERO; }); } - + /** * Set the position correction technique used for contacts * @param technique Technique used for the position correction (Baumgarte or Split Impulses) @@ -635,7 +641,7 @@ public class DynamicsWorld extends CollisionWorld { this.contactSolver.setIsSplitImpulseActive(true); } } - + /** * Set an event listener object to receive events callbacks. * @note If you use null as an argument, the events callbacks will be disabled. @@ -645,7 +651,7 @@ public class DynamicsWorld extends CollisionWorld { public void setEventListener(final EventListener eventListener) { this.eventListener = eventListener; } - + /** * Set the gravity vector of the world * @param gravity The gravity vector (in meter per seconds squared) @@ -653,7 +659,7 @@ public class DynamicsWorld extends CollisionWorld { public void setGravity(final Vector3f gravity) { this.gravity = gravity; } - + /** * Enable/Disable the gravity * @param isGravityEnabled True if you want to enable the gravity in the world @@ -662,7 +668,7 @@ public class DynamicsWorld extends CollisionWorld { public void setIsGratityEnabled(final boolean isGravityEnabled) { this.isGravityEnabled = isGravityEnabled; } - + /** * Activate or deactivate the solving of friction raints at the center of the contact * manifold instead of solving them at each contact point @@ -672,7 +678,7 @@ public class DynamicsWorld extends CollisionWorld { public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean isActive) { this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } - + /** * Set the position correction technique used for joints * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) @@ -684,7 +690,7 @@ public class DynamicsWorld extends CollisionWorld { this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); } } - + /** * Set the number of iterations for the position raint solver * @param nbIterations Number of iterations for the position solver @@ -692,7 +698,7 @@ public class DynamicsWorld extends CollisionWorld { public void setNbIterationsPositionSolver(final int nbIterations) { this.nbPositionSolverIterations = nbIterations; } - + /** * Set the number of iterations for the velocity raint solver * @param nbIterations Number of iterations for the velocity solver @@ -700,7 +706,7 @@ public class DynamicsWorld extends CollisionWorld { public void setNbIterationsVelocitySolver(final int nbIterations) { this.nbVelocitySolverIterations = nbIterations; } - + /** * Set the sleep angular velocity. * When the velocity of a body becomes smaller than the sleep linear/angular @@ -710,36 +716,36 @@ public class DynamicsWorld extends CollisionWorld { */ public void setSleepAngularVelocity(final float sleepAngularVelocity) { if (sleepAngularVelocity < 0.0f) { - //Log.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 "); + //LOGGER.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 "); return; } this.sleepAngularVelocity = sleepAngularVelocity; } - + /** * Set the sleep linear velocity * @param sleepLinearVelocity The new sleep linear velocity (in meters per second) */ public void setSleepLinearVelocity(final float sleepLinearVelocity) { if (sleepLinearVelocity < 0.0f) { - //Log.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 "); + //LOGGER.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 "); return; } this.sleepLinearVelocity = sleepLinearVelocity; } - + /** * Set the time a body is required to stay still before sleeping * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) */ public void setTimeBeforeSleep(final float timeBeforeSleep) { if (timeBeforeSleep < 0.0f) { - //Log.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 "); + //LOGGER.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 "); return; } this.timeBeforeSleep = timeBeforeSleep; } - + /** * Solve the contacts and raints */ @@ -749,10 +755,10 @@ public class DynamicsWorld extends CollisionWorld { this.contactSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); this.raintSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); this.raintSolver.setConstrainedPositionsArrays(this.rainedPositions, this.rainedOrientations); - - //Log.info(")))))))))))))))"); - for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { - //Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); + + //LOGGER.info(")))))))))))))))"); + for (final Vector3f element : this.rainedAngularVelocities) { + //LOGGER.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); } // ---------- Solve velocity raints for joints and contacts ---------- // final int idIsland = 0; @@ -761,7 +767,7 @@ public class DynamicsWorld extends CollisionWorld { // Check if there are contacts and raints to solve final boolean isConstraintsToSolve = island.getNbJoints() > 0; final boolean isContactsToSolve = island.getNbContactManifolds() > 0; - //Log.info("solveContactsAndConstraints : " + idIsland + " " + isConstraintsToSolve + " " + isContactsToSolve); + //LOGGER.info("solveContactsAndConstraints : " + idIsland + " " + isConstraintsToSolve + " " + isContactsToSolve); if (!isConstraintsToSolve && !isContactsToSolve) { continue; } @@ -795,12 +801,12 @@ public class DynamicsWorld extends CollisionWorld { this.contactSolver.cleanup(); } } - //Log.info("(((((((((((((("); - for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { - //Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); + //LOGGER.info("(((((((((((((("); + for (final Vector3f element : this.rainedAngularVelocities) { + //LOGGER.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); } } - + /** * Solve the position error correction of the raints */ @@ -810,16 +816,16 @@ public class DynamicsWorld extends CollisionWorld { return; } // For each island of the world - for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { + for (final Island element : this.islands) { // ---------- Solve the position error correction for the raints ---------- // // For each iteration of the position (error correction) solver for (int i = 0; i < this.nbPositionSolverIterations; i++) { // Solve the position raints - this.raintSolver.solvePositionConstraints(this.islands.get(islandIndex)); + this.raintSolver.solvePositionConstraints(element); } } } - + @Override public void testCollision(final CollisionBody body1, final CollisionBody body2, final CollisionCallback callback) { // Create the sets of shapes @@ -834,7 +840,7 @@ public class DynamicsWorld extends CollisionWorld { // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } - + @Override public void testCollision(final CollisionBody body, final CollisionCallback callback) { // Create the sets of shapes @@ -847,7 +853,7 @@ public class DynamicsWorld extends CollisionWorld { // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); } - + /// Test and report collisions between all shapes of the world @Override public void testCollision(final CollisionCallback callback) { @@ -855,7 +861,7 @@ public class DynamicsWorld extends CollisionWorld { // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); } - + @Override public void testCollision(final ProxyShape shape, final CollisionCallback callback) { // Create the sets of shapes @@ -865,7 +871,7 @@ public class DynamicsWorld extends CollisionWorld { // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); } - + @Override public void testCollision(final ProxyShape shape1, final ProxyShape shape2, final CollisionCallback callback) { // Create the sets of shapes @@ -876,18 +882,18 @@ public class DynamicsWorld extends CollisionWorld { // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } - + /** * Update the physics simulation * @param timeStep The amount of time to step the simulation by (in seconds) */ public void update(final float timeStep) { - //Log.error("========> start compute " + this.rigidBodies.size()); + //LOGGER.error("========> start compute " + this.rigidBodies.size()); for (final CollisionBody elem : this.bodies) { - //Log.info(" " + elem.getID() + " / " + elem.getAABB()); - //Log.info(" " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info(" " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info(" " + elem.getID() + " / " + elem.getTransform()); } - + this.timeStep = timeStep; // Notify the event listener about the beginning of an internal tick if (this.eventListener != null) { @@ -904,70 +910,70 @@ public class DynamicsWorld extends CollisionWorld { // Compute the islands (separate groups of bodies with raints between each others) computeIslands(); for (final CollisionBody elem : this.bodies) { - //Log.info("111 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("111 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("111 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("111 " + elem.getID() + " / " + elem.getTransform()); } // Integrate the velocities integrateRigidBodiesVelocities(); for (final CollisionBody elem : this.bodies) { - //Log.info("222 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("222 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("222 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("222 " + elem.getID() + " / " + elem.getTransform()); } // Solve the contacts and raints solveContactsAndConstraints(); for (final CollisionBody elem : this.bodies) { - //Log.info("333 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("333 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("333 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("333 " + elem.getID() + " / " + elem.getTransform()); } // Integrate the position and orientation of each body integrateRigidBodiesPositions(); for (final CollisionBody elem : this.bodies) { - //Log.info("444 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("444 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("444 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("444 " + elem.getID() + " / " + elem.getTransform()); } // Solve the position correction for raints solvePositionCorrection(); for (final CollisionBody elem : this.bodies) { - //Log.info("555 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("555 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("555 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("555 " + elem.getID() + " / " + elem.getTransform()); } // Update the state (positions and velocities) of the bodies - //Log.info(" ==> update state"); + //LOGGER.info(" ==> update state"); updateBodiesState(); // here to update each aaBB skeleton in the Broadphase ... not systematic but why ? - //Log.info(" ===> bug a partir d'ici sur le quaternion"); + //LOGGER.info(" ===> bug a partir d'ici sur le quaternion"); for (final CollisionBody elem : this.bodies) { - //Log.info("--- " + elem.getID() + " / " + elem.getAABB()); - //Log.info("--- " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("--- " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("--- " + elem.getID() + " / " + elem.getTransform()); } if (this.isSleepingEnabled) { updateSleepingBodies(); } for (final CollisionBody elem : this.bodies) { - //Log.info("666 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("666 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("666 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("666 " + elem.getID() + " / " + elem.getTransform()); } // Notify the event listener about the end of an internal tick if (this.eventListener != null) { this.eventListener.endInternalTick(); } for (final CollisionBody elem : this.bodies) { - //Log.info("777 " + elem.getID() + " / " + elem.getAABB()); - //Log.info("777 " + elem.getID() + " / " + elem.getTransform()); + //LOGGER.info("777 " + elem.getID() + " / " + elem.getAABB()); + //LOGGER.info("777 " + elem.getID() + " / " + elem.getTransform()); } // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); - //Log.error("<< ============= end compute "); + //LOGGER.error("<< ============= end compute "); } - + /** * Update the postion/orientation of the bodies */ protected void updateBodiesState() { // For each island of the world - for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { + for (final Island element : this.islands) { // For each body of the island - final List bodies = this.islands.get(islandIndex).getBodies(); - for (int b = 0; b < this.islands.get(islandIndex).getNbBodies(); b++) { + final List bodies = element.getBodies(); + for (int b = 0; b < element.getNbBodies(); b++) { final int index = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b)); // Update the linear and angular velocity of the body bodies.get(b).linearVelocity = this.rainedLinearVelocities[index]; @@ -977,20 +983,21 @@ public class DynamicsWorld extends CollisionWorld { // Update the position of the center of mass of the body bodies.get(b).centerOfMassWorld = this.rainedPositions[index]; // Update the orientation of the body - //Log.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]); - //Log.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew()); + //LOGGER.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]); + //LOGGER.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew()); if (bodies.get(b).isAngularReactionEnable()) { - bodies.get(b).setTransform(bodies.get(b).getTransform().withOrientation(this.rainedOrientations[index].safeNormalize())); + bodies.get(b).setTransform(bodies.get(b).getTransform() + .withOrientation(this.rainedOrientations[index].safeNormalize())); } // Update the transform of the body (using the new center of mass and new orientation) bodies.get(b).updateTransformWithCenterOfMass(); // Update the broad-phase state of the body - //Log.info(" " + b + " ==> updateBroadPhaseState"); + //LOGGER.info(" " + b + " ==> updateBroadPhaseState"); bodies.get(b).updateBroadPhaseState(); } } } - + /** * Put bodies to sleep if needed. * For each island, if all the bodies have been almost still for a int enough period of @@ -1000,28 +1007,31 @@ public class DynamicsWorld extends CollisionWorld { final float sleepLinearVelocitySquare = this.sleepLinearVelocity * this.sleepLinearVelocity; final float sleepAngularVelocitySquare = this.sleepAngularVelocity * this.sleepAngularVelocity; // For each island of the world - for (int i = 0; i < this.islands.size(); i++) { + for (final Island element : this.islands) { float minSleepTime = Float.MAX_VALUE; // For each body of the island - final List bodies = this.islands.get(i).getBodies(); - for (int b = 0; b < bodies.size(); b++) { + final List bodies = element.getBodies(); + for (final RigidBody element2 : bodies) { // Skip static bodies - if (bodies.get(b).getType() == BodyType.STATIC) { + if (element2.getType() == BodyType.STATIC) { continue; } // If the body is velocity is large enough to stay awake - Log.error(" check if ready to sleep: linear = " + bodies.get(b).getLinearVelocity().length2() + " > " + sleepLinearVelocitySquare); - Log.error(" check if ready to sleep: angular = " + bodies.get(b).getAngularVelocity().length2() + " > " + sleepAngularVelocitySquare); - if (bodies.get(b).getLinearVelocity().length2() > sleepLinearVelocitySquare || bodies.get(b).getAngularVelocity().length2() > sleepAngularVelocitySquare - || !bodies.get(b).isAllowedToSleep()) { + LOGGER.error(" check if ready to sleep: linear = " + element2.getLinearVelocity().length2() + " > " + + sleepLinearVelocitySquare); + LOGGER.error(" check if ready to sleep: angular = " + element2.getAngularVelocity().length2() + " > " + + sleepAngularVelocitySquare); + if (element2.getLinearVelocity().length2() > sleepLinearVelocitySquare + || element2.getAngularVelocity().length2() > sleepAngularVelocitySquare + || !element2.isAllowedToSleep()) { // Reset the sleep time of the body - bodies.get(b).sleepTime = 0.0f; + element2.sleepTime = 0.0f; minSleepTime = 0.0f; } else { // If the body velocity is bellow the sleeping velocity threshold // Increase the sleep time - bodies.get(b).sleepTime += this.timeStep; - if (bodies.get(b).sleepTime < minSleepTime) { - minSleepTime = bodies.get(b).sleepTime; + element2.sleepTime += this.timeStep; + if (element2.sleepTime < minSleepTime) { + minSleepTime = element2.sleepTime; } } } @@ -1030,11 +1040,11 @@ public class DynamicsWorld extends CollisionWorld { // the time required to become a sleeping body if (minSleepTime >= this.timeBeforeSleep) { // Put all the bodies of the island to sleep - for (int b = 0; b < this.islands.get(i).getNbBodies(); b++) { + for (int b = 0; b < element.getNbBodies(); b++) { bodies.get(b).setIsSleeping(true); } } } } - + } diff --git a/src/org/atriasoft/ephysics/engine/Island.java b/src/org/atriasoft/ephysics/engine/Island.java index 40a4396..cf1c0bf 100644 --- a/src/org/atriasoft/ephysics/engine/Island.java +++ b/src/org/atriasoft/ephysics/engine/Island.java @@ -7,17 +7,19 @@ import org.atriasoft.ephysics.body.BodyType; import org.atriasoft.ephysics.body.RigidBody; import org.atriasoft.ephysics.collision.ContactManifold; import org.atriasoft.ephysics.constraint.Joint; -import org.atriasoft.ephysics.internal.Log; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; /** * An island represent an isolated group of awake bodies that are connected with each other by * some contraints (contacts or joints). */ public class Island { + static final Logger LOGGER = LoggerFactory.getLogger(Island.class); private final List bodies = new ArrayList<>(); //!< Array with all the bodies of the island private final List contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island private final List joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island - + /** * Constructor */ @@ -27,76 +29,76 @@ public class Island { //this.contactManifolds.reserve(nbMaxContactManifolds); //this.joints.reserve(nbMaxJoints); } - - /** + + /** * Add a body. */ public void addBody(final RigidBody body) { if (body.isSleeping()) { - Log.error("Try to add a body that is sleeping ..."); + LOGGER.error("Try to add a body that is sleeping ..."); return; } this.bodies.add(body); } - - /** + + /** * Add a contact manifold. */ public void addContactManifold(final ContactManifold contactManifold) { this.contactManifolds.add(contactManifold); } - - /** + + /** * Add a joint. */ public void addJoint(final Joint joint) { this.joints.add(joint); } - - /** + + /** * Return a pointer to the array of bodies */ public List getBodies() { return this.bodies; } - - /** + + /** * Return a pointer to the array of contact manifolds */ public List getContactManifold() { return this.contactManifolds; } - - /** + + /** * Return a pointer to the array of joints */ public List getJoints() { return this.joints; } - - /** + + /** * Get the number of body * @return Number of bodies. */ public int getNbBodies() { return this.bodies.size(); } - - /** + + /** * @ Get the number of contact manifolds * Return the number of contact manifolds in the island */ public int getNbContactManifolds() { return this.contactManifolds.size(); } - - /** + + /** * Return the number of joints in the island */ public int getNbJoints() { return this.joints.size(); } - + /** * Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands */ @@ -107,5 +109,5 @@ public class Island { } } } - + } diff --git a/src/org/atriasoft/ephysics/internal/Log.java b/src/org/atriasoft/ephysics/internal/Log.java deleted file mode 100644 index c605afc..0000000 --- a/src/org/atriasoft/ephysics/internal/Log.java +++ /dev/null @@ -1,68 +0,0 @@ -package org.atriasoft.ephysics.internal; - -import org.atriasoft.reggol.LogLevel; -import org.atriasoft.reggol.Logger; - -public class Log { - private static final String LIB_NAME = "ephysics"; - private static final String LIB_NAME_DRAW = Logger.getDrawableName(LIB_NAME); - private static final boolean PRINT_CRITICAL = Logger.getNeedPrint(LIB_NAME, LogLevel.CRITICAL); - private static final boolean PRINT_ERROR = Logger.getNeedPrint(LIB_NAME, LogLevel.ERROR); - private static final boolean PRINT_WARNING = Logger.getNeedPrint(LIB_NAME, LogLevel.WARNING); - private static final boolean PRINT_INFO = Logger.getNeedPrint(LIB_NAME, LogLevel.INFO); - private static final boolean PRINT_DEBUG = Logger.getNeedPrint(LIB_NAME, LogLevel.DEBUG); - private static final boolean PRINT_VERBOSE = Logger.getNeedPrint(LIB_NAME, LogLevel.VERBOSE); - private static final boolean PRINT_TODO = Logger.getNeedPrint(LIB_NAME, LogLevel.TODO); - private static final boolean PRINT_PRINT = Logger.getNeedPrint(LIB_NAME, LogLevel.PRINT); - - public static void critical(final String data) { - if (PRINT_CRITICAL) { - Logger.critical(LIB_NAME_DRAW, data); - } - } - - public static void debug(final String data) { - if (PRINT_DEBUG) { - Logger.debug(LIB_NAME_DRAW, data); - } - } - - public static void error(final String data) { - if (PRINT_ERROR) { - Logger.error(LIB_NAME_DRAW, data); - } - } - - public static void info(final String data) { - if (PRINT_INFO) { - Logger.info(LIB_NAME_DRAW, data); - } - } - - public static void print(final String data) { - if (PRINT_PRINT) { - Logger.print(LIB_NAME_DRAW, data); - } - } - - public static void todo(final String data) { - if (PRINT_TODO) { - Logger.todo(LIB_NAME_DRAW, data); - } - } - - public static void verbose(final String data) { - if (PRINT_VERBOSE) { - Logger.verbose(LIB_NAME_DRAW, data); - } - } - - public static void warning(final String data) { - if (PRINT_WARNING) { - Logger.warning(LIB_NAME_DRAW, data); - } - } - - private Log() {} - -}