jatria-soft/ephysics
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

[FEAT] continue maven integration

Browse Source
This commit is contained in:
Edouard DUPIN 2024-06-09 09:57:21 +02:00
parent e4969408ad
commit 58932c43f4
23 changed files with 835 additions and 881 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
.classpath
View File

@ -1,24 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<classpath>
<classpathentry excluding="**/*.java__|**/__*.java" kind="src" path="src">
<attributes>
<attribute name="optional" value="true"/>
</attributes>
</classpathentry>
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-15">
<attributes>
<attribute name="module" value="true"/>
</attributes>
</classpathentry>
<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/5">
<attributes>
<attribute name="test" value="true"/>
</attributes>
</classpathentry>
<classpathentry combineaccessrules="false" kind="src" path="/atriasoft-etk">
<attributes>
<attribute name="module" value="true"/>
</attributes>
</classpathentry>
<classpathentry kind="output" path="out/eclipse/classes"/>
</classpath>

24
.project
View File

@ -1,24 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>atriasoft-ephysics</name>
<comment></comment>
<projects>
<project>atriasoft-ephysics</project>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.jdt.core.javabuilder</name>
<arguments>
</arguments>
</buildCommand>
<buildCommand>
<name>net.sf.eclipsecs.core.CheckstyleBuilder</name>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.jdt.core.javanature</nature>
<nature>net.sf.eclipsecs.core.CheckstyleNature</nature>
</natures>
</projectDescription>

13
ephysics.iml
View File

@ -1,13 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="JAVA_MODULE" version="4">
<component name="NewModuleRootManager" inherit-compiler-output="true">
<exclude-output />
<content url="file://$MODULE_DIR$">
<sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
</content>
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
<orderEntry type="library" scope="TEST" name="org.junit.jupiter:junit-jupiter-api:5.7.1" level="project" />
<orderEntry type="module" module-name="etk" />
</component>
</module>

2
lutin_org-atriasoft-ephysics.py
View File

@ -46,7 +46,7 @@ def configure(target, my_module):
'src/org/atriasoft/ephysics/mathematics/Set.java', 'src/org/atriasoft/ephysics/mathematics/Set.java',
'src/org/atriasoft/ephysics/mathematics/Mathematics.java', 'src/org/atriasoft/ephysics/mathematics/Mathematics.java',
'src/org/atriasoft/ephysics/configuration/Defaults.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/DynamicsWorld.java',
'src/org/atriasoft/ephysics/engine/CollisionWorld.java', 'src/org/atriasoft/ephysics/engine/CollisionWorld.java',
'src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java', 'src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java',

2
out/eclipse/.gitignore vendored
View File

@ -1,2 +0,0 @@
/classes/
/__pycache__/

90
src/org/atriasoft/ephysics/body/CollisionBody.java
View File

@ -30,16 +30,18 @@ import org.atriasoft.ephysics.collision.ProxyShape;
import org.atriasoft.ephysics.collision.shapes.AABB; import org.atriasoft.ephysics.collision.shapes.AABB;
import org.atriasoft.ephysics.collision.shapes.CollisionShape; import org.atriasoft.ephysics.collision.shapes.CollisionShape;
import org.atriasoft.ephysics.engine.CollisionWorld; import org.atriasoft.ephysics.engine.CollisionWorld;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.ephysics.mathematics.Ray;
import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Transform3D;
import org.atriasoft.etk.math.Vector3f; 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. * 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 { 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 // First element of the linked list of contact manifolds involving this body
public ContactManifoldListElement contactManifoldsList; public ContactManifoldListElement contactManifoldsList;
// Number of collision shapes // Number of collision shapes
@ -52,7 +54,7 @@ public class CollisionBody extends Body {
protected BodyType type; protected BodyType type;
// Reference to the world the body belongs to // Reference to the world the body belongs to
protected CollisionWorld world; protected CollisionWorld world;
/** /**
* Constructor * Constructor
* @param transform The transform of the body * @param transform The transform of the body
@ -67,20 +69,22 @@ public class CollisionBody extends Body {
this.numberCollisionShapes = 0; this.numberCollisionShapes = 0;
this.contactManifoldsList = null; this.contactManifoldsList = null;
this.world = world; this.world = world;
//Log.debug(" set transform: " + transform); //LOGGER.debug(" set transform: " + transform);
if (Float.isNaN(transform.getPosition().x())) { 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())) { 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 * 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. * 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 * 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. * 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 * @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++; this.numberCollisionShapes++;
return proxyShape; return proxyShape;
} }
/** /**
* Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved). * 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); this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape);
} }
} }
/** /**
* Compute and return the AABB of the body by merging all proxy shapes AABBs * 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 * @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) { if (this.proxyCollisionShapes == null) {
return bodyAABB; return bodyAABB;
} }
//Log.info("tmp this.transform : " + this.transform); //LOGGER.info("tmp this.transform : " + this.transform);
//Log.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform()); //LOGGER.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
//Log.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(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())); this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB,
this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform()));
for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) { for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) {
final AABB aabb = new AABB(); final AABB aabb = new AABB();
shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform())); shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
bodyAABB.mergeWithAABB(aabb); bodyAABB.mergeWithAABB(aabb);
} }
//Log.info("tmp aabb : " + bodyAABB); //LOGGER.info("tmp aabb : " + bodyAABB);
//Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin())); //LOGGER.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
return bodyAABB; return bodyAABB;
} }
/** /**
* Get the first element of the linked list of contact manifolds involving this body * 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 * @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() { public ContactManifoldListElement getContactManifoldsList() {
return this.contactManifoldsList; return this.contactManifoldsList;
} }
/** /**
* Get the body local-space coordinates of a point given in the world-space coordinates * Get the body local-space coordinates of a point given in the world-space coordinates
* @param worldPoint A point in 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) { public Vector3f getLocalPoint(final Vector3f worldPoint) {
return this.transform.inverseNew().multiply(worldPoint); return this.transform.inverseNew().multiply(worldPoint);
} }
/** /**
* Get the body local-space coordinates of a vector given in the world-space coordinates * Get the body local-space coordinates of a vector given in the world-space coordinates
* @param worldVector A vector in 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) { public Vector3f getLocalVector(final Vector3f worldVector) {
return this.transform.getOrientation().inverse().multiply(worldVector); return this.transform.getOrientation().inverse().multiply(worldVector);
} }
/** /**
* Get the linked list of proxy shapes of that body * 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 * @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() { public ProxyShape getProxyShapesList() {
return this.proxyCollisionShapes; return this.proxyCollisionShapes;
} }
/** /**
* Return the current position and orientation * Return the current position and orientation
* @return The current transformation of the body that transforms the local-space of the body int32to world-space * @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() { public Transform3D getTransform() {
return this.transform; return this.transform;
} }
/** /**
* Return the type of the body * Return the type of the body
* @return the type of the body (STATIC, KINEMATIC, DYNAMIC) * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
@ -186,11 +191,11 @@ public class CollisionBody extends Body {
public BodyType getType() { public BodyType getType() {
return this.type; return this.type;
} }
public CollisionWorld getWorld() { public CollisionWorld getWorld() {
return this.world; return this.world;
} }
/** /**
* Get the world-space coordinates of a point given the local-space coordinates of the body * 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 * @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) { public Vector3f getWorldPoint(final Vector3f localPoint) {
return this.transform.multiply(localPoint); return this.transform.multiply(localPoint);
} }
/** /**
* Get the world-space vector of a vector given in local-space coordinates of the body * 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 * @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) { public Vector3f getWorldVector(final Vector3f localVector) {
return this.transform.getOrientation().multiply(localVector); return this.transform.getOrientation().multiply(localVector);
} }
/** /**
* Raycast method with feedback information * Raycast method with feedback information
* The method returns the closest hit among all the collision shapes of the body * 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; return false;
} }
boolean isHit = false; boolean isHit = false;
final Ray rayTemp = new Ray(ray); final Ray rayTemp = new Ray(ray);
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
// Test if the ray hits the collision shape // Test if the ray hits the collision shape
@ -232,7 +237,7 @@ public class CollisionBody extends Body {
} }
return isHit; return isHit;
} }
/** /**
* Remove all the collision shapes * Remove all the collision shapes
*/ */
@ -250,7 +255,7 @@ public class CollisionBody extends Body {
} }
this.proxyCollisionShapes = null; this.proxyCollisionShapes = null;
} }
/** /**
* Remove a collision shape from the body * 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 * 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; current = current.next;
} }
} }
/** /**
* Reset the contact manifold lists * Reset the contact manifold lists
*/ */
@ -294,7 +299,7 @@ public class CollisionBody extends Body {
// Delete the linked list of contact manifolds of that body // Delete the linked list of contact manifolds of that body
this.contactManifoldsList = null; this.contactManifoldsList = null;
} }
/** /**
* Reset the this.isAlreadyInIsland variable of the body and contact manifolds. * Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
* This method also returns the number of contact manifolds of the body. * This method also returns the number of contact manifolds of the body.
@ -311,7 +316,7 @@ public class CollisionBody extends Body {
} }
return nbManifolds; return nbManifolds;
} }
/** /**
* Set whether or not the body is active * Set whether or not the body is active
* @param isActive True if you want to activate the body * @param isActive True if you want to activate the body
@ -337,23 +342,25 @@ public class CollisionBody extends Body {
resetContactManifoldsList(); resetContactManifoldsList();
} }
} }
/** /**
* Set the current position and orientation * Set the current position and orientation
* @param transform The transformation of the body that transforms the local-space of the body int32to world-space * @param transform The transformation of the body that transforms the local-space of the body int32to world-space
*/ */
public void setTransform(final Transform3D transform) { 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())) { 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())) { 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; this.transform = transform;
updateBroadPhaseState(); updateBroadPhaseState();
} }
/** /**
* Set the type of the body * Set the type of the body
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC) * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
@ -365,7 +372,7 @@ public class CollisionBody extends Body {
updateBroadPhaseState(); updateBroadPhaseState();
} }
} }
/** /**
* Return true if a point is inside the collision body * 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 * 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; return false;
} }
/** /**
* Update the broad-phase state for this body (because it has moved for instance) * 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); updateProxyShapeInBroadPhase(shape, false);
} }
} }
/** /**
* Update the broad-phase state of a proxy collision shape of the body * Update the broad-phase state of a proxy collision shape of the body
*/ */
public void updateProxyShapeInBroadPhase(final ProxyShape proxyShape, final boolean forceReinsert /* false */) { public void updateProxyShapeInBroadPhase(final ProxyShape proxyShape, final boolean forceReinsert /* false */) {
final AABB aabb = new AABB(); final AABB aabb = new AABB();
proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiply(proxyShape.getLocalToBodyTransform())); 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);
} }
} }

142
src/org/atriasoft/ephysics/body/RigidBody.java
View File

@ -32,10 +32,11 @@ import org.atriasoft.ephysics.constraint.JointListElement;
import org.atriasoft.ephysics.engine.CollisionWorld; import org.atriasoft.ephysics.engine.CollisionWorld;
import org.atriasoft.ephysics.engine.DynamicsWorld; import org.atriasoft.ephysics.engine.DynamicsWorld;
import org.atriasoft.ephysics.engine.Material; import org.atriasoft.ephysics.engine.Material;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.etk.math.Matrix3f; import org.atriasoft.etk.math.Matrix3f;
import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Transform3D;
import org.atriasoft.etk.math.Vector3f; import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/** /**
* This class represents a rigid body of the physics * This class represents a rigid body of the physics
@ -44,7 +45,8 @@ import org.atriasoft.etk.math.Vector3f;
* CollisionBody class. * CollisionBody class.
*/ */
public class RigidBody extends CollisionBody { public class RigidBody extends CollisionBody {
static final Logger LOGGER = LoggerFactory.getLogger(RigidBody.class);
protected float angularDamping = 0.0f; //!< Angular velocity damping factor protected float angularDamping = 0.0f; //!< Angular velocity damping factor
protected boolean angularReactionEnable = true; protected boolean angularReactionEnable = true;
public Vector3f angularVelocity = Vector3f.ZERO; //!< Angular velocity of the body 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 Vector3f linearVelocity = Vector3f.ZERO; //!< Linear velocity of the body
public float massInverse; //!< Inverse of the mass of the body public float massInverse; //!< Inverse of the mass of the body
protected Material material = new Material(); //!< Material properties of the rigid body protected Material material = new Material(); //!< Material properties of the rigid body
/** /**
* Constructor * Constructor
* @param transform The transformation of the body * @param transform The transformation of the body
@ -73,7 +75,7 @@ public class RigidBody extends CollisionBody {
this.centerOfMassWorld = transform.getPosition(); this.centerOfMassWorld = transform.getPosition();
this.massInverse = 1.0f / this.initMass; this.massInverse = 1.0f / this.initMass;
} }
/** /**
* Add a collision shape to the body. * 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. * 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 * @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. * @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); assert (mass > 0.0f);
// Create a new proxy collision shape to attach the collision shape to the body // Create a new proxy collision shape to attach the collision shape to the body
final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, mass); final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, mass);
@ -105,7 +110,7 @@ public class RigidBody extends CollisionBody {
recomputeMassInformation(); recomputeMassInformation();
return proxyShape; return proxyShape;
} }
/** /**
* Apply an external force to the body at a given point (in world-space coordinates). * 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 * 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.externalForce = this.externalForce.add(force);
this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force)); this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force));
} }
/** /**
* Apply an external force to the body at its center of mass. * 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 * 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); this.externalForce = this.externalForce.add(force);
} }
/** /**
* Apply an external torque to the body. * Apply an external torque to the body.
* If the body is sleeping, calling this method will wake it up. Note that the * 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); this.externalTorque = this.externalTorque.add(torque);
} }
/** /**
* Set the variable to know if the gravity is applied to this rigid body * 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 * @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) { public void enableGravity(final boolean isEnabled) {
this.isGravityEnabled = isEnabled; this.isGravityEnabled = isEnabled;
} }
/** /**
* Get the angular velocity damping factor * Get the angular velocity damping factor
* @return The angular damping factor of this body * @return The angular damping factor of this body
@ -179,7 +184,7 @@ public class RigidBody extends CollisionBody {
public float getAngularDamping() { public float getAngularDamping() {
return this.angularDamping; return this.angularDamping;
} }
/** /**
* Get the angular velocity of the body * Get the angular velocity of the body
* @return The angular velocity vector of the body * @return The angular velocity vector of the body
@ -187,7 +192,7 @@ public class RigidBody extends CollisionBody {
public Vector3f getAngularVelocity() { public Vector3f getAngularVelocity() {
return this.angularVelocity; return this.angularVelocity;
} }
/** /**
* Get the inverse of the inertia tensor in world coordinates. * Get the inverse of the inertia tensor in world coordinates.
* The inertia tensor Iw in world coordinates is computed with the * 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 // 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 // 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 // Compute and return the inertia tensor in world coordinates
//Log.error("}}} this.transform=" + this.transform); //LOGGER.error("}}} this.transform=" + this.transform);
//Log.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse); //LOGGER.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
//Log.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew()); //LOGGER.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
//Log.error("}}} this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix()); //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()); final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse)
//Log.error("}}} tmp=" + tmp); .multiply(this.transform.getOrientation().getMatrix().transpose());
//LOGGER.error("}}} tmp=" + tmp);
return tmp; return tmp;
} }
/** /**
* Get the local inertia tensor of the body (in local-space coordinates) * Get the local inertia tensor of the body (in local-space coordinates)
* @return The 3x3 inertia tensor matrix of the body * @return The 3x3 inertia tensor matrix of the body
@ -217,7 +223,7 @@ public class RigidBody extends CollisionBody {
public Matrix3f getInertiaTensorLocal() { public Matrix3f getInertiaTensorLocal() {
return this.inertiaTensorLocal; return this.inertiaTensorLocal;
} }
/** /**
* Get the inertia tensor in world coordinates. * Get the inertia tensor in world coordinates.
* The inertia tensor Iw in world coordinates is computed * The inertia tensor Iw in world coordinates is computed
@ -229,9 +235,10 @@ public class RigidBody extends CollisionBody {
*/ */
public Matrix3f getInertiaTensorWorld() { public Matrix3f getInertiaTensorWorld() {
// Compute and return the inertia tensor in world coordinates // 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 * 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 * @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() { public JointListElement getJointsList() {
return this.jointsList; return this.jointsList;
} }
/** /**
* Get the linear velocity damping factor * Get the linear velocity damping factor
* @return The linear damping factor of this body * @return The linear damping factor of this body
@ -247,7 +254,7 @@ public class RigidBody extends CollisionBody {
public float getLinearDamping() { public float getLinearDamping() {
return this.linearDamping; return this.linearDamping;
} }
/** /**
* Get the linear velocity * Get the linear velocity
* @return The linear velocity vector of the body * @return The linear velocity vector of the body
@ -255,7 +262,7 @@ public class RigidBody extends CollisionBody {
public Vector3f getLinearVelocity() { public Vector3f getLinearVelocity() {
return this.linearVelocity; return this.linearVelocity;
} }
/** /**
* Get the mass of the body * Get the mass of the body
* @return The mass (in kilograms) of the body * @return The mass (in kilograms) of the body
@ -263,7 +270,7 @@ public class RigidBody extends CollisionBody {
public float getMass() { public float getMass() {
return this.initMass; return this.initMass;
} }
/** /**
* get a reference to the material properties of the rigid body * get a reference to the material properties of the rigid body
* @return A reference to the material of the body * @return A reference to the material of the body
@ -271,11 +278,11 @@ public class RigidBody extends CollisionBody {
public Material getMaterial() { public Material getMaterial() {
return this.material; return this.material;
} }
public boolean isAngularReactionEnable() { public boolean isAngularReactionEnable() {
return this.angularReactionEnable; return this.angularReactionEnable;
} }
/** /**
* get the need of gravity appling to this rigid body * get the need of gravity appling to this rigid body
* @return True if the gravity is applied to the body * @return True if the gravity is applied to the body
@ -283,7 +290,7 @@ public class RigidBody extends CollisionBody {
public boolean isGravityEnabled() { public boolean isGravityEnabled() {
return this.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. * 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 // Compute the total mass of the body
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
this.initMass += shape.getMass(); 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) { if (this.initMass > 0.0f) {
this.massInverse = 1.0f / this.initMass; this.massInverse = 1.0f / this.initMass;
@ -329,22 +337,24 @@ public class RigidBody extends CollisionBody {
final Vector3f off1 = offset.multiply(-offset.x()); final Vector3f off1 = offset.multiply(-offset.x());
final Vector3f off2 = offset.multiply(-offset.y()); final Vector3f off2 = offset.multiply(-offset.y());
final Vector3f off3 = offset.multiply(-offset.z()); 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()); offsetMatrix = offsetMatrix.multiply(shape.getMass());
this.inertiaTensorLocal = this.inertiaTensorLocal.add(inertiaTensor.add(offsetMatrix)); this.inertiaTensorLocal = this.inertiaTensorLocal.add(inertiaTensor.add(offsetMatrix));
} }
// Compute the local inverse inertia tensor // Compute the local inverse inertia tensor
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse(); this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
// Update the linear velocity of the center of mass // 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 @Override
public void removeCollisionShape(final ProxyShape proxyShape) { public void removeCollisionShape(final ProxyShape proxyShape) {
super.removeCollisionShape(proxyShape); super.removeCollisionShape(proxyShape);
recomputeMassInformation(); recomputeMassInformation();
} }
/** /**
* Remove a joint from the joints list * 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. * 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 * @param angularDamping The angular damping factor of this body
@ -378,11 +388,11 @@ public class RigidBody extends CollisionBody {
assert (angularDamping >= 0.0f); assert (angularDamping >= 0.0f);
this.angularDamping = angularDamping; this.angularDamping = angularDamping;
} }
public void setAngularReactionEnable(final boolean angularReactionEnable) { public void setAngularReactionEnable(final boolean angularReactionEnable) {
this.angularReactionEnable = angularReactionEnable; this.angularReactionEnable = angularReactionEnable;
} }
/** /**
* Set the angular velocity. * Set the angular velocity.
* @param angularVelocity The angular velocity vector of the body * @param angularVelocity The angular velocity vector of the body
@ -396,7 +406,7 @@ public class RigidBody extends CollisionBody {
setIsSleeping(false); setIsSleeping(false);
} }
} }
/** /**
* Set the local center of mass of the body (in local-space coordinates) * 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 * @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; final Vector3f oldCenterOfMass = this.centerOfMassWorld;
this.centerOfMassLocal = centerOfMassLocal; this.centerOfMassLocal = centerOfMassLocal;
this.centerOfMassWorld = this.transform.multiply(this.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) * 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 * @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.inertiaTensorLocal = inertiaTensorLocal;
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse(); this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
} }
/** /**
* Set the variable to know whether or not the body is sleeping * Set the variable to know whether or not the body is sleeping
* @param isSleeping New sleeping state of the body * @param isSleeping New sleeping state of the body
@ -437,7 +448,7 @@ public class RigidBody extends CollisionBody {
} }
super.setIsSleeping(isSleeping); 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. * 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 * @param linearDamping The linear damping factor of this body
@ -446,7 +457,7 @@ public class RigidBody extends CollisionBody {
assert (linearDamping >= 0.0f); assert (linearDamping >= 0.0f);
this.linearDamping = linearDamping; this.linearDamping = linearDamping;
} }
/** /**
* Set the linear velocity of the rigid body. * Set the linear velocity of the rigid body.
* @param linearVelocity Linear velocity vector of the body * @param linearVelocity Linear velocity vector of the body
@ -460,7 +471,7 @@ public class RigidBody extends CollisionBody {
setIsSleeping(false); setIsSleeping(false);
} }
} }
/** /**
* Set the mass of the rigid body * Set the mass of the rigid body
* @param mass The mass (in kilograms) of the body * @param mass The mass (in kilograms) of the body
@ -477,7 +488,7 @@ public class RigidBody extends CollisionBody {
this.massInverse = 1.0f; this.massInverse = 1.0f;
} }
} }
/** /**
* Set a new material for this rigid body * Set a new material for this rigid body
* @param material The material you want to set to the 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) { public void setMaterial(final Material material) {
this.material = material; this.material = material;
} }
/** /**
* Set the current position and orientation * Set the current position and orientation
* @param transform The transformation of the body that transforms the local-space of the body into world-space * @param transform The transformation of the body that transforms the local-space of the body into world-space
*/ */
@Override @Override
public void setTransform(final Transform3D transform) { 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) { 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())) { 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; this.transform = transform;
final Vector3f oldCenterOfMass = this.centerOfMassWorld; final Vector3f oldCenterOfMass = this.centerOfMassWorld;
// Compute the new center of mass in world-space coordinates // Compute the new center of mass in world-space coordinates
this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal); this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
// Update the linear velocity of the center of mass // 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(); updateBroadPhaseState();
} }
@Override @Override
public void setType(final BodyType type) { public void setType(final BodyType type) {
if (this.type == type) { if (this.type == type) {
@ -536,38 +550,42 @@ public class RigidBody extends CollisionBody {
this.externalForce = Vector3f.ZERO; this.externalForce = Vector3f.ZERO;
this.externalTorque = Vector3f.ZERO; this.externalTorque = Vector3f.ZERO;
} }
@Override @Override
public void updateBroadPhaseState() { public void updateBroadPhaseState() {
final DynamicsWorld world = (DynamicsWorld) this.world; final DynamicsWorld world = (DynamicsWorld) this.world;
final Vector3f displacement = this.linearVelocity.multiply(world.timeStep); final Vector3f displacement = this.linearVelocity.multiply(world.timeStep);
// For all the proxy collision shapes of the body // For all the proxy collision shapes of the body
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
// Recompute the world-space AABB of the collision shape // Recompute the world-space AABB of the collision shape
final AABB aabb = new AABB(); final AABB aabb = new AABB();
//Log.info(" : " + aabb.getMin() + " " + aabb.getMax()); //LOGGER.info(" : " + aabb.getMin() + " " + aabb.getMax());
//Log.info(" this.transform: " + this.transform); //LOGGER.info(" this.transform: " + this.transform);
shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform())); 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 // Update the broad-phase state for the proxy collision shape
//Log.warning(" ==> updateProxyCollisionShape"); //LOGGER.warn(" ==> updateProxyCollisionShape");
world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
} }
} }
/** /**
* Update the transform of the body after a change of the center of mass * Update the transform of the body after a change of the center of mass
*/ */
public void updateTransformWithCenterOfMass() { public void updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position // 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())) { 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())) { if (Float.isInfinite(this.transform.getOrientation().z())) {
Log.critical(" set transform: " + this.transform); LOGGER.error("[CRITICAL] set transform: " + this.transform);
System.exit(-1);
} }
} }
} }

194
src/org/atriasoft/ephysics/collision/CollisionDetection.java
View File

@ -2,6 +2,7 @@ package org.atriasoft.ephysics.collision;
import java.util.Iterator; import java.util.Iterator;
import java.util.Map; import java.util.Map;
import java.util.Map.Entry;
import java.util.TreeMap; import java.util.TreeMap;
import org.atriasoft.ephysics.RaycastCallback; 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.CollisionWorld;
import org.atriasoft.ephysics.engine.EventListener; import org.atriasoft.ephysics.engine.EventListener;
import org.atriasoft.ephysics.engine.OverlappingPair; import org.atriasoft.ephysics.engine.OverlappingPair;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.PairInt; import org.atriasoft.ephysics.mathematics.PairInt;
import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.ephysics.mathematics.Ray;
import org.atriasoft.ephysics.mathematics.Set; import org.atriasoft.ephysics.mathematics.Set;
import org.atriasoft.ephysics.mathematics.SetMultiple; import org.atriasoft.ephysics.mathematics.SetMultiple;
import org.atriasoft.etk.math.Vector3f; import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/* /*
* It computes the collision detection algorithms. We first * It computes the collision detection algorithms. We first
@ -39,6 +41,7 @@ import org.atriasoft.etk.math.Vector3f;
* collision contacts between bodies. * collision contacts between bodies.
*/ */
public class CollisionDetection implements NarrowPhaseCallback { public class CollisionDetection implements NarrowPhaseCallback {
static final Logger LOGGER = LoggerFactory.getLogger(CollisionDetection.class);
/// Broad-phase algorithm /// Broad-phase algorithm
private final BroadPhaseAlgorithm broadPhaseAlgorithm; private final BroadPhaseAlgorithm broadPhaseAlgorithm;
/// Collision Detection Dispatch configuration /// 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 private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
// TODO : Delete this // TODO : Delete this
private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
private final SetMultiple<PairInt> noCollisionPairs = new SetMultiple<PairInt>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other private final SetMultiple<PairInt> noCollisionPairs = new SetMultiple<>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other
public Map<PairDTree, OverlappingPair> overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs public Map<PairDTree, OverlappingPair> overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs
/// Reference on the physics world /// Reference on the physics world
private final CollisionWorld world; private final CollisionWorld world;
/// Constructor /// Constructor
public CollisionDetection(final CollisionWorld world) { public CollisionDetection(final CollisionWorld world) {
this.world = world; this.world = world;
@ -66,7 +69,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
// Fill-in the collision detection matrix with algorithms // Fill-in the collision detection matrix with algorithms
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
/// Add all the contact manifold of colliding pairs to their bodies /// Add all the contact manifold of colliding pairs to their bodies
private void addAllContactManifoldsToBodies() { private void addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase // For each overlapping pairs in contact during the narrow-phase
@ -76,7 +79,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
addContactManifoldToBody(it); addContactManifoldToBody(it);
} }
} }
/// Add a contact manifold to the linked list of contact manifolds of the two bodies /// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact. /// involed in the corresponding contact.
private void addContactManifoldToBody(final OverlappingPair pair) { 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 // Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body // list of contact manifolds of the first body
body1.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList); body1.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList);
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body // list of the contact manifolds of the second body
body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList); body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList);
} }
} }
/// Add a pair of bodies that cannot collide with each other /// Add a pair of bodies that cannot collide with each other
public void addNoCollisionPair(final CollisionBody body1, final CollisionBody body2) { public void addNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(body1, body2)); this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(body1, body2));
} }
/// Add a proxy collision shape to the collision detection /// Add a proxy collision shape to the collision detection
public void addProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb) { public void addProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb) {
// Add the body to the broad-phase // Add the body to the broad-phase
this.broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb); this.broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
this.isCollisionShapesAdded = true; this.isCollisionShapesAdded = true;
} }
// Ask for a collision shape to be tested again during broad-phase. // 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 /// 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. /// previous frame so that it is tested for collision again in the broad-phase.
public void askForBroadPhaseCollisionCheck(final ProxyShape shape) { public void askForBroadPhaseCollisionCheck(final ProxyShape shape) {
this.broadPhaseAlgorithm.addMovedCollisionShape(shape.broadPhaseID); this.broadPhaseAlgorithm.addMovedCollisionShape(shape.broadPhaseID);
} }
/// Allow the broadphase to notify the collision detection about an overlapping pair. /// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm /// This method is called by the broad-phase collision detection algorithm
public void broadPhaseNotifyOverlappingPair(final ProxyShape shape1, final ProxyShape shape2) { 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()) { if (shape1.getBody().getID() == shape2.getBody().getID()) {
return; 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 // Check if the collision filtering allows collision between the two shapes
if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) { if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0
Log.info(" ==> not permited ..."); || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) {
LOGGER.info(" ==> not permited ...");
return; return;
} }
// Compute the overlapping pair ID // Compute the overlapping pair ID
@ -139,7 +143,8 @@ public class CollisionDetection implements NarrowPhaseCallback {
return; return;
} }
// Compute the maximum number of contact manifolds for this pair // 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 // Create the overlapping pair and add it int32to the set of overlapping pairs
final OverlappingPair newPair = new OverlappingPair(shape1, shape2, nbMaxManifolds); final OverlappingPair newPair = new OverlappingPair(shape1, shape2, nbMaxManifolds);
assert (newPair != null); assert (newPair != null);
@ -148,13 +153,13 @@ public class CollisionDetection implements NarrowPhaseCallback {
shape1.getBody().setIsSleeping(false); shape1.getBody().setIsSleeping(false);
shape2.getBody().setIsSleeping(false); shape2.getBody().setIsSleeping(false);
} }
/// Delete all the contact points in the currently overlapping pairs /// Delete all the contact points in the currently overlapping pairs
private void clearContactPoints() { private void clearContactPoints() {
// For each overlapping pair // For each overlapping pair
this.overlappingPairs.forEach((key, value) -> value.clearContactPoints()); this.overlappingPairs.forEach((key, value) -> value.clearContactPoints());
} }
/// Compute the broad-phase collision detection /// Compute the broad-phase collision detection
private void computeBroadPhase() { private void computeBroadPhase() {
// If new collision shapes have been added to bodies // If new collision shapes have been added to bodies
@ -165,33 +170,29 @@ public class CollisionDetection implements NarrowPhaseCallback {
this.broadPhaseAlgorithm.computeOverlappingPairs(); this.broadPhaseAlgorithm.computeOverlappingPairs();
} }
} }
/// Compute the collision detection /// Compute the collision detection
public void computeCollisionDetection() { public void computeCollisionDetection() {
//Log.info("computeBroadPhase();"); //LOGGER.info("computeBroadPhase();");
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
//Log.info("computeNarrowPhase();"); //LOGGER.info("computeNarrowPhase();");
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
computeNarrowPhase(); computeNarrowPhase();
} }
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
private void computeNarrowPhase() { private void computeNarrowPhase() {
// Clear the set of overlapping pairs in narrow-phase contact // Clear the set of overlapping pairs in narrow-phase contact
this.contactOverlappingPair.clear(); this.contactOverlappingPair.clear();
{ {
//Log.info("list elements:"); for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
final Iterator<Map.Entry<PairDTree, OverlappingPair>> ittt = this.overlappingPairs.entrySet().iterator();
while (ittt.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = ittt.next();
//Log.info(" " + entry.getKey() + " " + entry.getValue());
} }
} }
// For each possible collision pair of bodies // For each possible collision pair of bodies
final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator(); final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
while (it.hasNext()) { while (it.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = it.next(); final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
final OverlappingPair pair = entry.getValue(); 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 // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // 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)) { || !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 // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // 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 // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.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());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // notifyContact() callback method will be called.
narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this); narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
public void computeNarrowPhaseBetweenShapes(final CollisionCallback callback, final Set<DTree> shapes1, final Set<DTree> shapes2) { public void computeNarrowPhaseBetweenShapes(
final CollisionCallback callback,
final Set<DTree> shapes1,
final Set<DTree> shapes2) {
this.contactOverlappingPair.clear(); this.contactOverlappingPair.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator(); final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
@ -262,20 +269,24 @@ public class CollisionDetection implements NarrowPhaseCallback {
assert (shape1.broadPhaseID != shape2.broadPhaseID); assert (shape1.broadPhaseID != shape2.broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // 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)) { && (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) {
continue; 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; 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; continue;
} }
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // 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)) { || !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 // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // 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 // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.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());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(callback); shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(
callback);
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision // if there really is a collision
narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback); narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
void createContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) { void createContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
// Create a new contact // Create a new contact
final ContactPoint contact = new ContactPoint(contactInfo); final ContactPoint contact = new ContactPoint(contactInfo);
@ -332,40 +346,43 @@ public class CollisionDetection implements NarrowPhaseCallback {
final PairDTree pairId = OverlappingPair.computeID(overlappingPair.getShape1(), overlappingPair.getShape2()); final PairDTree pairId = OverlappingPair.computeID(overlappingPair.getShape1(), overlappingPair.getShape2());
this.contactOverlappingPair.put(pairId, overlappingPair); this.contactOverlappingPair.put(pairId, overlappingPair);
} }
/// Fill-in the collision detection matrix /// Fill-in the collision detection matrix
private void fillInCollisionMatrix() { private void fillInCollisionMatrix() {
// For each possible type of collision shape // For each possible type of collision shape
for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) { for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) {
for (int j = 0; j < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; j++) { 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 /// 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]; return this.collisionMatrix[shape1Type.value][shape2Type.value];
} }
public GJKAlgorithm getNarrowPhaseGJKAlgorithm() { public GJKAlgorithm getNarrowPhaseGJKAlgorithm() {
return this.narrowPhaseGJKAlgorithm; return this.narrowPhaseGJKAlgorithm;
} }
/// Return a pointer to the world /// Return a pointer to the world
public CollisionWorld getWorld() { public CollisionWorld getWorld() {
return this.world; return this.world;
} }
/// Return the world event listener /// Return the world event listener
public EventListener getWorldEventListener() { public EventListener getWorldEventListener() {
return this.world.eventListener; return this.world.eventListener;
} }
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
@Override @Override
public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) { 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 it is the first contact since the pairs are overlapping
if (overlappingPair.getNbContactPoints() == 0) { if (overlappingPair.getNbContactPoints() == 0) {
// Trigger a callback event // Trigger a callback event
@ -380,7 +397,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
this.world.eventListener.newContact(contactInfo); this.world.eventListener.newContact(contactInfo);
} }
} }
/// Ray casting method /// Ray casting method
public void raycast(final RaycastCallback raycastCallback, final Ray ray, final int raycastWithCategoryMaskBits) { public void raycast(final RaycastCallback raycastCallback, final Ray ray, final int raycastWithCategoryMaskBits) {
final RaycastTest rayCastTest = new RaycastTest(raycastCallback); 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 // callback method for each proxy shape hit by the ray in the broad-phase
this.broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits); this.broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
} }
/// Remove a pair of bodies that cannot collide with each other /// Remove a pair of bodies that cannot collide with each other
public void removeNoCollisionPair(final CollisionBody body1, final CollisionBody body2) { public void removeNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(body1, body2))); this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(body1, body2)));
} }
/// Remove a proxy collision shape from the collision detection /// Remove a proxy collision shape from the collision detection
public void removeProxyCollisionShape(final ProxyShape proxyShape) { public void removeProxyCollisionShape(final ProxyShape proxyShape) {
// Remove all the overlapping pairs involving this proxy shape // Remove all the overlapping pairs involving this proxy shape
@ -401,7 +418,8 @@ public class CollisionDetection implements NarrowPhaseCallback {
while (it.hasNext()) { while (it.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = it.next(); final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
final OverlappingPair pair = entry.getValue(); 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 // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
it.remove(); it.remove();
@ -410,28 +428,31 @@ public class CollisionDetection implements NarrowPhaseCallback {
// Remove the body from the broad-phase // Remove the body from the broad-phase
this.broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape); this.broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
} }
/// Report collision between two sets of shapes /// Report collision between two sets of shapes
public void reportCollisionBetweenShapes(final CollisionCallback callback, final Set<DTree> shapes1, final Set<DTree> shapes2) { public void reportCollisionBetweenShapes(
final CollisionCallback callback,
// For each possible collision pair of bodies final Set<DTree> shapes1,
final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator(); final Set<DTree> shapes2) {
while (it.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = it.next(); for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
final OverlappingPair pair = entry.getValue(); final OverlappingPair pair = entry.getValue();
final ProxyShape shape1 = pair.getShape1(); final ProxyShape shape1 = pair.getShape1();
final ProxyShape shape2 = pair.getShape2(); final ProxyShape shape2 = pair.getShape2();
assert (shape1.broadPhaseID != shape2.broadPhaseID); assert (shape1.broadPhaseID != shape2.broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // 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)) { && (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) {
continue; 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; 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; continue;
} }
// For each contact manifold set of the overlapping pair // 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++) { for (int i = 0; i < manifold.getNbContactPoints(); i++) {
final ContactPoint contactPoint = manifold.getContactPoint(i); final ContactPoint contactPoint = manifold.getContactPoint(i);
// Create the contact info object for the contact // Create the contact info object for the contact
final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(), manifold.getShape2(), manifold.getShape1().getCollisionShape(), final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(),
manifold.getShape2().getCollisionShape(), contactPoint.getNormal(), contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(), manifold.getShape2(), manifold.getShape1().getCollisionShape(),
manifold.getShape2().getCollisionShape(), contactPoint.getNormal(),
contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(),
contactPoint.getLocalPointOnBody2()); contactPoint.getLocalPointOnBody2());
// Notify the collision callback about this new contact // Notify the collision callback about this new contact
if (callback != null) { if (callback != null) {
callback.notifyContact(contactInfo); callback.notifyContact(contactInfo);
} }
} }
} }
} }
} }
/// Set the collision dispatch configuration /// Set the collision dispatch configuration
public void setCollisionDispatch(final CollisionDispatch collisionDispatch) { public void setCollisionDispatch(final CollisionDispatch collisionDispatch) {
this.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 // Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
/// Test if the AABBs of two proxy shapes overlap /// Test if the AABBs of two proxy shapes overlap
public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) { public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) {
// If one of the shape's body is not active, we return no overlap // 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); return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
} }
/// Compute the collision detection /// Compute the collision detection
public void testCollisionBetweenShapes(final CollisionCallback callback, final Set<DTree> shapes1, final Set<DTree> shapes2) { public void testCollisionBetweenShapes(
final CollisionCallback callback,
final Set<DTree> shapes1,
final Set<DTree> shapes2) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Delete all the contact points in the currently overlapping pairs // 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 // Compute the narrow-phase collision detection among given sets of shapes
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
} }
/// Update a proxy collision shape (that has moved for instance) /// Update a proxy collision shape (that has moved for instance)
public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb) { public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb) {
updateProxyCollisionShape(shape, aabb, new Vector3f(0, 0, 0), false); updateProxyCollisionShape(shape, aabb, new Vector3f(0, 0, 0), false);
} }
public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement) { public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement) {
updateProxyCollisionShape(shape, aabb, displacement, false); 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); this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
} }
} }

10
src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java
View File

@ -102,7 +102,7 @@ public class BroadPhaseAlgorithm {
/// Add a collision shape in the array of shapes that have moved in the last simulation step /// 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. /// and that need to be tested again for broad-phase overlapping.
public void addMovedCollisionShape(final DTree broadPhaseID) { public void addMovedCollisionShape(final DTree broadPhaseID) {
//Log.info(" ** Element that has moved ... = " + broadPhaseID); //LOGGER.info(" ** Element that has moved ... = " + broadPhaseID);
this.movedShapes.add(broadPhaseID); this.movedShapes.add(broadPhaseID);
} }
@ -122,10 +122,10 @@ public class BroadPhaseAlgorithm {
this.potentialPairs.clear(); this.potentialPairs.clear();
// For all collision shapes that have moved (or have been created) during the // For all collision shapes that have moved (or have been created) during the
// last simulation step // last simulation step
//Log.info("moved shape = " + this.movedShapes.size()); //LOGGER.info("moved shape = " + this.movedShapes.size());
/* /*
for (final DTree it : this.movedShapes) { for (final DTree it : this.movedShapes) {
Log.info(" - " + it); LOGGER.info(" - " + it);
} }
*/ */
for (final DTree it : this.movedShapes) { 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 // Reset the array of collision shapes that have move (or have been created) during the last simulation step
this.movedShapes.clear(); this.movedShapes.clear();
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // 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 // Update the dynamic AABB tree according to the movement of the collision shape
final boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); 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 // 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). // longo the tree).
if (hasBeenReInserted) { if (hasBeenReInserted) {
// Add the collision shape longo the array of shapes that have moved (or have been created) // Add the collision shape longo the array of shapes that have moved (or have been created)

36
src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java
View File

@ -5,10 +5,11 @@ import java.util.Stack;
import org.atriasoft.ephysics.Configuration; import org.atriasoft.ephysics.Configuration;
import org.atriasoft.ephysics.collision.shapes.AABB; import org.atriasoft.ephysics.collision.shapes.AABB;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.ephysics.mathematics.Ray;
import org.atriasoft.etk.math.FMath; import org.atriasoft.etk.math.FMath;
import org.atriasoft.etk.math.Vector3f; 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 * 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. * "Introduction to Game Physics with Box2D" by Ian Parberry.
*/ */
public class DynamicAABBTree { 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 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 /// Allocate and return a node to use in the tree
@ -344,7 +346,7 @@ public class DynamicAABBTree {
/// Ray casting method /// Ray casting method
public void raycast(final Ray ray, final CallbackRaycast callback) { public void raycast(final Ray ray, final CallbackRaycast callback) {
if (callback == null) { if (callback == null) {
Log.error("call with null callback"); LOGGER.error("call with null callback");
return; return;
} }
float maxFraction = ray.maxFraction; float maxFraction = ray.maxFraction;
@ -468,15 +470,15 @@ public class DynamicAABBTree {
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) { public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) {
if (callback == null) { if (callback == null) {
Log.error("call with null callback"); LOGGER.error("call with null callback");
return; return;
} }
//Log.error("reportAllShapesOverlappingWithAABB"); //LOGGER.error("reportAllShapesOverlappingWithAABB");
// Create a stack with the nodes to visit // Create a stack with the nodes to visit
final Stack<DTree> stack = new Stack<>(); final Stack<DTree> stack = new Stack<>();
// 64 max // 64 max
stack.push(this.rootNode); stack.push(this.rootNode);
//Log.error(" add stack: " + this.rootNode); //LOGGER.error(" add stack: " + this.rootNode);
// While there are still nodes to visit // While there are still nodes to visit
while (stack.size() > 0) { while (stack.size() > 0) {
// Get the next node ID to visit // Get the next node ID to visit
@ -487,14 +489,14 @@ public class DynamicAABBTree {
} }
// Get the corresponding node // Get the corresponding node
final DTree nodeToVisit = nodeIDToVisit; 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 the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit.aabb)) { if (aabb.testCollision(nodeToVisit.aabb)) {
// If the node is a leaf // If the node is a leaf
if (nodeToVisit.isLeaf()) { if (nodeToVisit.isLeaf()) {
/* /*
if (aabb != nodeToVisit.aabb) { if (aabb != nodeToVisit.aabb) {
Log.error(" ======> Real collision ..."); LOGGER.error(" ======> Real collision ...");
} }
*/ */
// Notify the broad-phase about a new potential overlapping pair // Notify the broad-phase about a new potential overlapping pair
@ -505,8 +507,8 @@ public class DynamicAABBTree {
// We need to visit its children // We need to visit its children
stack.push(tmp.childrenleft); stack.push(tmp.childrenleft);
stack.push(tmp.childrenright); stack.push(tmp.childrenright);
//Log.error(" add stack: " + tmp.childrenleft); //LOGGER.error(" add stack: " + tmp.childrenleft);
//Log.error(" add stack: " + tmp.childrenright); //LOGGER.error(" add stack: " + tmp.childrenright);
} }
} }
} }
@ -529,11 +531,15 @@ public class DynamicAABBTree {
return updateObject(node, newAABB, displacement, false); 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.isLeaf());
assert (node.height >= 0); assert (node.height >= 0);
//Log.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); //LOGGER.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
//Log.verbose(" : " + newAABB.getMin() + " " + newAABB.getMax()); //LOGGER.verbose(" : " + newAABB.getMin() + " " + newAABB.getMax());
// If the new AABB is still inside the fat AABB of the node // If the new AABB is still inside the fat AABB of the node
if (!forceReinsert && node.aabb.contains(newAABB)) { if (!forceReinsert && node.aabb.contains(newAABB)) {
return false; return false;
@ -570,10 +576,10 @@ public class DynamicAABBTree {
} }
node.aabb.setMin(new Vector3f(xmin, ymin, zmin)); node.aabb.setMin(new Vector3f(xmin, ymin, zmin));
node.aabb.setMax(new Vector3f(xmax, ymax, zmax)); node.aabb.setMax(new Vector3f(xmax, ymax, zmax));
//Log.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax()); //LOGGER.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
//Log.error(" : " + newAABB.getMin() + " " + newAABB.getMax()); //LOGGER.error(" : " + newAABB.getMin() + " " + newAABB.getMax());
if (!node.aabb.contains(newAABB)) { if (!node.aabb.contains(newAABB)) {
//Log.critical("ERROR"); //LOGGER.critical("ERROR");
} }
assert (node.aabb.contains(newAABB)); assert (node.aabb.contains(newAABB));
// Reinsert the node into the tree // Reinsert the node into the tree

82
src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java
View File

@ -34,20 +34,20 @@ import org.atriasoft.etk.math.Vector3f;
public class EPAAlgorithm { public class EPAAlgorithm {
/// Add a triangle face in the candidate triangle heap /// Add a triangle face in the candidate triangle heap
private void addFaceCandidate(final TriangleEPA triangle, final SortedSet<TriangleEPA> heap, final float upperBoundSquarePenDepth) { private void addFaceCandidate(final TriangleEPA triangle, final SortedSet<TriangleEPA> 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 // If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance // points is internal to the triangle and if the distance
// of the closest point from the origin is at most the // of the closest point from the origin is at most the
// penetration depth upper bound // penetration depth upper bound
////Log.info(" triangle.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle()); ////LOGGER.info(" triangle.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle());
////Log.info(" triangle.getDistSquare(): " + triangle.getDistSquare()); ////LOGGER.info(" triangle.getDistSquare(): " + triangle.getDistSquare());
if (triangle.isClosestPointInternalToTriangle() && triangle.getDistSquare() <= upperBoundSquarePenDepth) { if (triangle.isClosestPointInternalToTriangle() && triangle.getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates // Add the triangle face to the list of candidates
heap.add(triangle); heap.add(triangle);
////Log.info("add in heap:"); ////LOGGER.info("add in heap:");
//int iii = 0; //int iii = 0;
//for (final TriangleEPA elem : heap) { //for (final TriangleEPA elem : heap) {
// ////Log.info(" [" + iii + "] " + elem.getDistSquare()); // ////LOGGER.info(" [" + iii + "] " + elem.getDistSquare());
// ++iii; // ++iii;
//} //}
} }
@ -61,7 +61,7 @@ public class EPAAlgorithm {
/// the correct penetration depth /// the correct penetration depth
public Vector3f computePenetrationDepthAndContactPoints(final Simplex simplex, final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, public Vector3f computePenetrationDepthAndContactPoints(final Simplex simplex, final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info,
final Transform3D transform2, Vector3f vector, final NarrowPhaseCallback narrowPhaseCallback) { final Transform3D transform2, Vector3f vector, final NarrowPhaseCallback narrowPhaseCallback) {
////Log.info("computePenetrationDepthAndContactPoints()"); ////LOGGER.info("computePenetrationDepthAndContactPoints()");
assert (shape1Info.collisionShape().isConvex()); assert (shape1Info.collisionShape().isConvex());
assert (shape2Info.collisionShape().isConvex()); assert (shape2Info.collisionShape().isConvex());
final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape()); 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 // Select an action according to the number of points in the simplex
// computed with GJK algorithm in order to obtain an initial polytope for // computed with GJK algorithm in order to obtain an initial polytope for
// The EPA algorithm. // The EPA algorithm.
////Log.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices); ////LOGGER.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices);
switch (nbVertices) { switch (nbVertices) {
case 1: case 1:
// Only one point in the simplex (which should be the origin). // 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. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we ruct the tetrahedron. // Therefore, we ruct the tetrahedron.
// Comstruct the 4 triangle faces of 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); 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); 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); 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); final TriangleEPA face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the ructed tetrahedron is not correct // 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 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 v1 = points[1].less(points[0]);
final Vector3f v2 = points[2].less(points[0]); final Vector3f v2 = points[2].less(points[0]);
final Vector3f n = v1.cross(v2); final Vector3f n = v1.cross(v2);
////Log.info(">>>>>>>>>>>>>>>>>>"); ////LOGGER.info(">>>>>>>>>>>>>>>>>>");
////Log.info(" v1 = " + v1); ////LOGGER.info(" v1 = " + v1);
////Log.info(" v2 = " + v2); ////LOGGER.info(" v2 = " + v2);
////Log.info(" n = " + n); ////LOGGER.info(" n = " + n);
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiply(-1)), shape2CachedCollisionData)); suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiply(-1)), shape2CachedCollisionData));
points[3] = suppPointsA[3].less(suppPointsB[3]); points[3] = suppPointsA[3].less(suppPointsB[3]);
////Log.info(" suppPointsA[3]= " + suppPointsA[3]); ////LOGGER.info(" suppPointsA[3]= " + suppPointsA[3]);
////Log.info(" suppPointsB[3]= " + suppPointsB[3]); ////LOGGER.info(" suppPointsB[3]= " + suppPointsB[3]);
////Log.info(" points[3] = " + points[3]); ////LOGGER.info(" points[3] = " + points[3]);
suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiply(-1), shape1CachedCollisionData); suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiply(-1), shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData)); suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
points[4] = suppPointsA[4].less(suppPointsB[4]); points[4] = suppPointsA[4].less(suppPointsB[4]);
////Log.info(" suppPointsA[4]= " + suppPointsA[4]); ////LOGGER.info(" suppPointsA[4]= " + suppPointsA[4]);
////Log.info(" suppPointsB[4]= " + suppPointsB[4]); ////LOGGER.info(" suppPointsB[4]= " + suppPointsB[4]);
////Log.info(" points[4]= " + points[4]); ////LOGGER.info(" points[4]= " + points[4]);
TriangleEPA face0 = null; TriangleEPA face0 = null;
TriangleEPA face1 = null; TriangleEPA face1 = null;
TriangleEPA face2 = null; TriangleEPA face2 = null;
@ -253,25 +253,25 @@ public class EPAAlgorithm {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we ruct the tetrahedron. // Therefore, we ruct the tetrahedron.
// Comstruct the 4 triangle faces of 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); 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); 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); 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); face3 = triangleStore.newTriangle(points, 1, 3, 2);
} else if (isOriginInTetrahedron(points[0], points[1], points[2], points[4]) == 0) { } else if (isOriginInTetrahedron(points[0], points[1], points[2], points[4]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm. // The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we ruct the tetrahedron. // Therefore, we ruct the tetrahedron.
// Comstruct the 4 triangle faces of 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); 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); 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); 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); face3 = triangleStore.newTriangle(points, 1, 4, 2);
} else { } else {
return vector; return vector;
@ -309,10 +309,10 @@ public class EPAAlgorithm {
do { do {
triangle = triangleHeap.first(); triangle = triangleHeap.first();
triangleHeap.remove(triangle); triangleHeap.remove(triangle);
////Log.info("rm from heap:"); ////LOGGER.info("rm from heap:");
int iii = 0; int iii = 0;
for (final TriangleEPA elem : triangleHeap) { for (final TriangleEPA elem : triangleHeap) {
////Log.info(" [" + iii + "] " + elem.getDistSquare()); ////LOGGER.info(" [" + iii + "] " + elem.getDistSquare());
++iii; ++iii;
} }
// If the candidate face in the heap is not obsolete // If the candidate face in the heap is not obsolete
@ -331,11 +331,11 @@ public class EPAAlgorithm {
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
final float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint()); final float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
////Log.info(" point=" + points[indexNewVertex]); ////LOGGER.info(" point=" + points[indexNewVertex]);
////Log.info("close point=" + triangle.getClosestPoint()); ////LOGGER.info("close point=" + triangle.getClosestPoint());
////Log.info(" ==>" + wDotv); ////LOGGER.info(" ==>" + wDotv);
if (wDotv < 0.0f) { if (wDotv < 0.0f) {
////Log.error("depth penetration error " + wDotv); ////LOGGER.error("depth penetration error " + wDotv);
continue; continue;
} }
assert (wDotv >= 0.0f); 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 /// 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 /// 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) { 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 // Check vertex 1
final Vector3f normal1 = p2.less(p1).cross(p3.less(p1)); final Vector3f normal1 = p2.less(p1).cross(p3.less(p1));
if ((normal1.dot(p1) > 0.0f) == (normal1.dot(p4) > 0.0)) { if ((normal1.dot(p1) > 0.0f) == (normal1.dot(p4) > 0.0)) {
////Log.error(" ==> 4"); ////LOGGER.error(" ==> 4");
return 4; return 4;
} }
// Check vertex 2 // Check vertex 2
final Vector3f normal2 = p4.less(p2).cross(p3.less(p2)); final Vector3f normal2 = p4.less(p2).cross(p3.less(p2));
if ((normal2.dot(p2) > 0.0f) == (normal2.dot(p1) > 0.0)) { if ((normal2.dot(p2) > 0.0f) == (normal2.dot(p1) > 0.0)) {
////Log.error(" ==> 1"); ////LOGGER.error(" ==> 1");
return 1; return 1;
} }
// Check vertex 3 // Check vertex 3
final Vector3f normal3 = p4.less(p3).cross(p1.less(p3)); final Vector3f normal3 = p4.less(p3).cross(p1.less(p3));
if ((normal3.dot(p3) > 0.0f) == (normal3.dot(p2) > 0.0)) { if ((normal3.dot(p3) > 0.0f) == (normal3.dot(p2) > 0.0)) {
////Log.error(" ==> 2"); ////LOGGER.error(" ==> 2");
return 2; return 2;
} }
// Check vertex 4 // Check vertex 4
final Vector3f normal4 = p2.less(p4).cross(p1.less(p4)); final Vector3f normal4 = p2.less(p4).cross(p1.less(p4));
if ((normal4.dot(p4) > 0.0f) == (normal4.dot(p3) > 0.0)) { if ((normal4.dot(p4) > 0.0f) == (normal4.dot(p3) > 0.0)) {
////Log.error(" ==> 3"); ////LOGGER.error(" ==> 3");
return 3; return 3;
} }
////Log.error(" ==> 0"); ////LOGGER.error(" ==> 0");
// The origin is in the tetrahedron, we return 0 // The origin is in the tetrahedron, we return 0
return 0; return 0;
} }

8
src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java
View File

@ -63,12 +63,12 @@ public class EdgeEPA implements Cloneable {
/// Execute the recursive silhouette algorithm from this edge /// Execute the recursive silhouette algorithm from this edge
public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) { 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 the edge has not already been visited
if (!this.ownerTriangle.getIsObsolete()) { if (!this.ownerTriangle.getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point
if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) { 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()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != null) { if (triangle != null) {
@ -82,7 +82,7 @@ public class EdgeEPA implements Cloneable {
final int backup = triangleStore.getNbTriangles(); final int backup = triangleStore.getNbTriangles();
if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
this.ownerTriangle.setIsObsolete(false); 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()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
// If the triangle has been created // If the triangle has been created
if (triangle != null) { 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)) { } else if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
this.ownerTriangle.setIsObsolete(false); this.ownerTriangle.setIsObsolete(false);
triangleStore.resize(backup); 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()); final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
if (triangle != null) { if (triangle != null) {
TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this); TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);

30
src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java
View File

@ -3,12 +3,14 @@ package org.atriasoft.ephysics.collision.narrowphase.EPA;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.List; import java.util.List;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.etk.math.Vector3f; import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
public class TrianglesStore { public class TrianglesStore {
static final Logger LOGGER = LoggerFactory.getLogger(TrianglesStore.class);
private static final int MAX_TRIANGLES = 200; private static final int MAX_TRIANGLES = 200;
public static void shrinkTo(final List list, final int newSize) { public static void shrinkTo(final List list, final int newSize) {
final int size = list.size(); final int size = list.size();
if (newSize >= size) { if (newSize >= size) {
@ -18,52 +20,52 @@ public class TrianglesStore {
list.remove(list.size() - 1); list.remove(list.size() - 1);
} }
} }
private final List<TriangleEPA> triangles = new ArrayList<>(); private final List<TriangleEPA> triangles = new ArrayList<>();
/// Clear all the storage /// Clear all the storage
public void clear() { public void clear() {
this.triangles.clear(); this.triangles.clear();
} }
/// Access operator /// Access operator
public TriangleEPA get(final int id) { public TriangleEPA get(final int id) {
return this.triangles.get(id); return this.triangles.get(id);
} }
/// Return the number of triangles /// Return the number of triangles
public int getNbTriangles() { public int getNbTriangles() {
return this.triangles.size(); return this.triangles.size();
} }
/// Return the last triangle /// Return the last triangle
public TriangleEPA last() { public TriangleEPA last() {
return this.triangles.get(this.triangles.size() - 1); return this.triangles.get(this.triangles.size() - 1);
} }
/// Create a new triangle /// Create a new triangle
public TriangleEPA newTriangle(final Vector3f[] vertices, final int v0, final int v1, final int v2) { 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 we have not reached the maximum number of triangles
if (this.triangles.size() < TrianglesStore.MAX_TRIANGLES) { if (this.triangles.size() < TrianglesStore.MAX_TRIANGLES) {
final TriangleEPA tmp = new TriangleEPA(v0, v1, v2); final TriangleEPA tmp = new TriangleEPA(v0, v1, v2);
if (!tmp.computeClosestPoint(vertices)) { if (!tmp.computeClosestPoint(vertices)) {
return null; return null;
} }
this.triangles.add(tmp); 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; return tmp;
} }
// We are at the limit (internal) // We are at the limit (internal)
return null; return null;
} }
/// Set the number of triangles /// Set the number of triangles
public void resize(final int backup) { public void resize(final int backup) {
if (backup > this.triangles.size()) { 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); TrianglesStore.shrinkTo(this.triangles, backup);
} }

64
src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java
View File

@ -52,7 +52,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
private Vector3f computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2, private Vector3f computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
final NarrowPhaseCallback narrowPhaseCallback, Vector3f v) { final NarrowPhaseCallback narrowPhaseCallback, Vector3f v) {
//Log.info("computePenetrationDepthForEnlargedObjects..."); //LOGGER.info("computePenetrationDepthForEnlargedObjects...");
final Simplex simplex = new Simplex(); final Simplex simplex = new Simplex();
float distSquare = Float.MAX_VALUE; float distSquare = Float.MAX_VALUE;
float prevDistSquare; float prevDistSquare;
@ -62,59 +62,59 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape()); final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
final Object shape1CachedCollisionData = shape1Info.cachedCollisionData(); final Object shape1CachedCollisionData = shape1Info.cachedCollisionData();
final Object shape2CachedCollisionData = shape2Info.cachedCollisionData(); final Object shape2CachedCollisionData = shape2Info.cachedCollisionData();
//Log.info(" transform1=" + transform1); //LOGGER.info(" transform1=" + transform1);
//Log.info(" transform2=" + transform2); //LOGGER.info(" transform2=" + transform2);
// Transform3D a point from local space of body 2 to local space // 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) // of body 1 (the GJK algorithm is done in local space of body 1)
final Transform3D body2ToBody1 = transform1.inverseNew().multiply(transform2); final Transform3D body2ToBody1 = transform1.inverseNew().multiply(transform2);
// Matrix that transform a direction from local space of body 1 into local space of body 2 // 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()); final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transpose().multiply(transform1.getOrientation().getMatrix());
//Log.info(" body2ToBody1=" + body2ToBody1); //LOGGER.info(" body2ToBody1=" + body2ToBody1);
//Log.info(" rotateToBody2=" + rotateToBody2); //LOGGER.info(" rotateToBody2=" + rotateToBody2);
//Log.info(" v=" + v); //LOGGER.info(" v=" + v);
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiply(-1), (CacheData) shape1CachedCollisionData); 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)); 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 // Compute the support point for the Minkowski difference A-B
final Vector3f w = suppA.less(suppB); final Vector3f w = suppA.less(suppB);
final float vDotw = v.dot(w); final float vDotw = v.dot(w);
//Log.info(" vDotw=" + vDotw); //LOGGER.info(" vDotw=" + vDotw);
// If the enlarge objects do not intersect // If the enlarge objects do not intersect
if (vDotw > 0.0f) { if (vDotw > 0.0f) {
//Log.info(" ==> ret 1"); //LOGGER.info(" ==> ret 1");
// No intersection, we return // No intersection, we return
return v; return v;
} }
// Add the new support point to the simplex // Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB); simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
//Log.info(" ==> ret 2"); //LOGGER.info(" ==> ret 2");
return v; return v;
} }
Vector3f v2 = simplex.computeClosestPoint(v); Vector3f v2 = simplex.computeClosestPoint(v);
if (v2 == null) { if (v2 == null) {
//Log.info(" ==> ret 3"); //LOGGER.info(" ==> ret 3");
return v; return v;
} }
v = v2; v = v2;
// Store and update the square distance // Store and update the square distance
prevDistSquare = distSquare; prevDistSquare = distSquare;
//Log.info(" distSquare=" + distSquare); //LOGGER.info(" distSquare=" + distSquare);
distSquare = v.length2(); distSquare = v.length2();
//Log.info(" distSquare=" + distSquare); //LOGGER.info(" distSquare=" + distSquare);
if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
//Log.info(" ==> ret 4"); //LOGGER.info(" ==> ret 4");
return v; return v;
} }
} while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); } while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
// Give the simplex computed with GJK algorithm to the EPA algorithm // Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points // which will compute the correct penetration depth and contact points
// between the two enlarged objects // between the two enlarged objects
//Log.info(" ==> ret 5"); //LOGGER.info(" ==> ret 5");
v = this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback); v = this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback);
return v; return v;
} }
@ -216,9 +216,9 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
@Override @Override
public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) { public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) {
//Log.error("================================================="); //LOGGER.error("=================================================");
//Log.error(" shape1Info=" + shape1Info.shapeToWorldTransform); //LOGGER.error(" shape1Info=" + shape1Info.shapeToWorldTransform);
//Log.error(" shape2Info=" + shape2Info.shapeToWorldTransform); //LOGGER.error(" shape2Info=" + shape2Info.shapeToWorldTransform);
Vector3f suppA = Vector3f.ZERO; // Support point of object A Vector3f suppA = Vector3f.ZERO; // Support point of object A
Vector3f suppB = Vector3f.ZERO; // Support point of object B Vector3f suppB = Vector3f.ZERO; // Support point of object B
Vector3f w = Vector3f.ZERO; // Support point of Minkowski difference A-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 // Initialize the upper bound for the square distance
float distSquare = Float.MAX_VALUE; float distSquare = Float.MAX_VALUE;
//Log.error(" T1=" + transform1 + " T2=" + transform2); //LOGGER.error(" T1=" + transform1 + " T2=" + transform2);
//Log.error(" BT1=" + body2Tobody1 + " RT2=" + rotateToBody2); //LOGGER.error(" BT1=" + body2Tobody1 + " RT2=" + rotateToBody2);
//Log.error(" M=" + FMath.floatToString(margin) + " M2=" + FMath.floatToString(marginSquare)); //LOGGER.error(" M=" + FMath.floatToString(margin) + " M2=" + FMath.floatToString(marginSquare));
//Log.error(" v=" + cacheSeparatingAxis); //LOGGER.error(" v=" + cacheSeparatingAxis);
do { do {
//Log.error("------------------"); //LOGGER.error("------------------");
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.value.multiply(-1.0f), shape1CachedCollisionData); suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.value.multiply(-1.0f), shape1CachedCollisionData);
suppB = body2Tobody1.multiply(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiply(cacheSeparatingAxis.value), shape2CachedCollisionData)); suppB = body2Tobody1.multiply(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiply(cacheSeparatingAxis.value), shape2CachedCollisionData));
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA.less(suppB); w = suppA.less(suppB);
vDotw = cacheSeparatingAxis.value.dot(w); vDotw = cacheSeparatingAxis.value.dot(w);
//Log.error(" suppA=" + suppA); //LOGGER.error(" suppA=" + suppA);
//Log.error(" suppB=" + suppB); //LOGGER.error(" suppB=" + suppB);
//Log.error(" w=" + w); //LOGGER.error(" w=" + w);
// If the enlarge objects (with margins) do not intersect // If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0f && vDotw * vDotw > distSquare * marginSquare) { if (vDotw > 0.0f && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence // 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 the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * GJKAlgorithm.REL_ERROR_SQUARE) { 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) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
// Project those two points on the margins to have the closest points of both // 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); simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent // If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) { if (simplex.isAffinelyDependent()) {
//Log.error("222222 "); //LOGGER.error("222222 ");
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
// Project those two points on the margins to have the closest points of both // 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) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0f) { if (penetrationDepth <= 0.0f) {
//Log.info("penetration depth " + penetrationDepth); //LOGGER.info("penetration depth " + penetrationDepth);
return; return;
} }
@ -342,7 +342,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
// If the computation of the closest point fail // If the computation of the closest point fail
Vector3f tmp = simplex.computeClosestPoint(cacheSeparatingAxis.value); Vector3f tmp = simplex.computeClosestPoint(cacheSeparatingAxis.value);
if (tmp == null) { if (tmp == null) {
//Log.error("3333333333 "); //LOGGER.error("3333333333 ");
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o); simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
// Project those two points on the margins to have the closest points of both // 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(); distSquare = cacheSeparatingAxis.value.length2();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
//Log.error("444444444 "); //LOGGER.error("444444444 ");
cacheSeparatingAxis.value = simplex.backupClosestPointInSimplex(); cacheSeparatingAxis.value = simplex.backupClosestPointInSimplex();
// Get the new squared distance // Get the new squared distance

18
src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java
View File

@ -120,7 +120,7 @@ public class Simplex {
/// point : support point of object (A-B) => point = suppPointA - suppPointB /// point : support point of object (A-B) => point = suppPointA - suppPointB
public void addPoint(final Vector3f point, final Vector3f suppPointA, final Vector3f suppPointB) { public void addPoint(final Vector3f point, final Vector3f suppPointA, final Vector3f suppPointB) {
assert (!isFull()); assert (!isFull());
//Log.warning("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")"); //LOGGER.warn("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")");
this.lastFound = 0; this.lastFound = 0;
this.lastFoundBit = 0x1; this.lastFoundBit = 0x1;
@ -130,8 +130,8 @@ public class Simplex {
this.lastFound++; this.lastFound++;
this.lastFoundBit <<= 1; this.lastFoundBit <<= 1;
} }
//Log.warning(" this.lastFound " + this.lastFound); //LOGGER.warn(" this.lastFound " + this.lastFound);
//Log.warning(" this.lastFoundBit " + this.lastFoundBit); //LOGGER.warn(" this.lastFoundBit " + this.lastFoundBit);
assert (this.lastFound < 4); assert (this.lastFound < 4);
@ -139,7 +139,7 @@ public class Simplex {
this.points[this.lastFound] = point; this.points[this.lastFound] = point;
this.pointsLengthSquare[this.lastFound] = point.dot(point); this.pointsLengthSquare[this.lastFound] = point.dot(point);
this.allBits = this.bitsCurrentSimplex | this.lastFoundBit; this.allBits = this.bitsCurrentSimplex | this.lastFoundBit;
//Log.warning(" this.allBits " + this.allBits); //LOGGER.warn(" this.allBits " + this.allBits);
// Update the cached values // Update the cached values
updateCache(); updateCache();
@ -249,7 +249,7 @@ public class Simplex {
/// Compute the cached determinant values /// Compute the cached determinant values
private void computeDeterminants() { private void computeDeterminants() {
this.det.set(this.lastFoundBit, this.lastFound, 1.0f); 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 the current simplex is not empty
if (!isEmpty()) { if (!isEmpty()) {
// For each possible four points in the simplex set // 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 /// Update the cached values used during the GJK algorithm
private void updateCache() { private void updateCache() {
//Log.warning("simplex: update Cache()"); //LOGGER.warn("simplex: update Cache()");
// For each of the four possible points of the simplex // For each of the four possible points of the simplex
for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
//Log.warning("simplex: iii=" + iii); //LOGGER.warn("simplex: iii=" + iii);
//Log.warning("simplex: bit=" + bit); //LOGGER.warn("simplex: bit=" + bit);
// If the current points is in the simplex // If the current points is in the simplex
if (overlap(this.bitsCurrentSimplex, bit)) { if (overlap(this.bitsCurrentSimplex, bit)) {
//Log.warning("simplex: ==> overlap"); //LOGGER.warn("simplex: ==> overlap");
// Compute the distance between two points in the possible simplex set // Compute the distance between two points in the possible simplex set
final Vector3f tmp = this.points[iii].less(this.points[this.lastFound]); final Vector3f tmp = this.points[iii].less(this.points[this.lastFound]);
this.diffLength.set(iii, this.lastFound, tmp); this.diffLength.set(iii, this.lastFound, tmp);

6
src/org/atriasoft/ephysics/collision/shapes/AABB.java
View File

@ -170,10 +170,10 @@ public class AABB {
*/ */
public boolean testCollision(final AABB aabb) { public boolean testCollision(final AABB aabb) {
if (this == aabb) { if (this == aabb) {
///Log.info("test : AABB ==> same object "); ///LOGGER.info("test : AABB ==> same object ");
return true; 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()) { if (this.maxCoordinates.x() < aabb.minCoordinates.x() || aabb.maxCoordinates.x() < this.minCoordinates.x()) {
return false; return false;
} }
@ -183,7 +183,7 @@ public class AABB {
if (this.maxCoordinates.y() < aabb.minCoordinates.y() || aabb.maxCoordinates.y() < this.minCoordinates.y()) { if (this.maxCoordinates.y() < aabb.minCoordinates.y() || aabb.maxCoordinates.y() < this.minCoordinates.y()) {
return false; return false;
} }
//Log.info("detect collision "); //LOGGER.info("detect collision ");
return true; return true;
} }

4
src/org/atriasoft/ephysics/collision/shapes/BoxShape.java
View File

@ -76,8 +76,8 @@ public class BoxShape extends ConvexShape {
*/ */
@Override @Override
public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) { public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
//Log.error("getLocalSupportPointWithoutMargin(" + direction); //LOGGER.error("getLocalSupportPointWithoutMargin(" + direction);
//Log.error(" extends = " + this.extent); //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(), 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()); direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
} }

59
src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java
View File

@ -13,10 +13,11 @@ import org.atriasoft.ephysics.collision.broadphase.CallbackRaycast;
import org.atriasoft.ephysics.collision.broadphase.DTree; import org.atriasoft.ephysics.collision.broadphase.DTree;
import org.atriasoft.ephysics.collision.broadphase.DynamicAABBTree; import org.atriasoft.ephysics.collision.broadphase.DynamicAABBTree;
import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide; import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.Ray; import org.atriasoft.ephysics.mathematics.Ray;
import org.atriasoft.etk.math.Matrix3f; import org.atriasoft.etk.math.Matrix3f;
import org.atriasoft.etk.math.Vector3f; import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/** /**
* Represents a static concave mesh shape. Note that collision detection * 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. * this shape for a static mesh.
*/ */
public class ConcaveMeshShape extends ConcaveShape { public class ConcaveMeshShape extends ConcaveShape {
static final Logger LOGGER = LoggerFactory.getLogger(ConcaveMeshShape.class);
class ConcaveMeshRaycastCallback { class ConcaveMeshRaycastCallback {
private final ConcaveMeshShape concaveMeshShape; private final ConcaveMeshShape concaveMeshShape;
private final DynamicAABBTree dynamicAABBTree; private final DynamicAABBTree dynamicAABBTree;
@ -32,30 +35,31 @@ public class ConcaveMeshShape extends ConcaveShape {
private final ProxyShape proxyShape; private final ProxyShape proxyShape;
private final Ray ray; private final Ray ray;
private final RaycastInfo raycastInfo; private final RaycastInfo raycastInfo;
// Constructor // 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.dynamicAABBTree = dynamicAABBTree;
this.concaveMeshShape = concaveMeshShape; this.concaveMeshShape = concaveMeshShape;
this.proxyShape = proxyShape; this.proxyShape = proxyShape;
this.raycastInfo = raycastInfo; this.raycastInfo = raycastInfo;
this.ray = ray; this.ray = ray;
this.isHit = false; this.isHit = false;
} }
/// Return true if a raycast hit has been found /// Return true if a raycast hit has been found
public boolean getIsHit() { public boolean getIsHit() {
return this.isHit; return this.isHit;
} }
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree /// 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) { public float operatorparenthese(final DTree node, final Ray ray) {
// Add the id of the hit AABB node longo // Add the id of the hit AABB node longo
this.hitAABBNodes.add(node); this.hitAABBNodes.add(node);
return ray.maxFraction; return ray.maxFraction;
} }
/// Raycast all collision shapes that have been collected /// Raycast all collision shapes that have been collected
public void raycastTriangles() { public void raycastTriangles() {
float smallestHitFraction = this.ray.maxFraction; float smallestHitFraction = this.ray.maxFraction;
@ -68,8 +72,9 @@ public class ConcaveMeshShape extends ConcaveShape {
this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data0, data1, trianglePoints); this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data0, data1, trianglePoints);
// Create a triangle collision shape // Create a triangle collision shape
final float margin = this.concaveMeshShape.getTriangleMargin(); 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()); triangleShape.setRaycastTestType(this.concaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape // Ray casting test against the collision shape
final RaycastInfo raycastInfo = new RaycastInfo(); 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 DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
protected TriangleMesh triangleMesh; //!< Triangle mesh protected TriangleMesh triangleMesh; //!< Triangle mesh
public ConcaveMeshShape(final TriangleMesh triangleMesh) { public ConcaveMeshShape(final TriangleMesh triangleMesh) {
super(CollisionShapeType.CONCAVE_MESH); super(CollisionShapeType.CONCAVE_MESH);
this.triangleMesh = triangleMesh; this.triangleMesh = triangleMesh;
this.raycastTestType = TriangleRaycastSide.FRONT; this.raycastTestType = TriangleRaycastSide.FRONT;
initBVHTree(); initBVHTree();
} }
@Override @Override
public Matrix3f computeLocalInertiaTensor(final float mass) { public Matrix3f computeLocalInertiaTensor(final float mass) {
// Default inertia tensor // Default inertia tensor
@ -111,29 +116,32 @@ public class ConcaveMeshShape extends ConcaveShape {
// the inertia tensor is not used. // the inertia tensor is not used.
return new Matrix3f(mass, 0.0f, 0.0f, 0.0f, mass, 0.0f, 0.0f, 0.0f, mass); return new Matrix3f(mass, 0.0f, 0.0f, 0.0f, mass, 0.0f, 0.0f, 0.0f, mass);
} }
@Override @Override
public Bounds getLocalBounds() { public Bounds getLocalBounds() {
// Get the AABB of the whole tree // Get the AABB of the whole tree
final AABB treeAABB = this.dynamicAABBTree.getRootAABB(); final AABB treeAABB = this.dynamicAABBTree.getRootAABB();
return new Bounds(treeAABB.getMin(), treeAABB.getMax()); return new Bounds(treeAABB.getMin(), treeAABB.getMax());
} }
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the 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); assert (outTriangleVertices != null);
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart); final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart);
if (triangleVertexArray == null) { if (triangleVertexArray == null) {
Log.error("get null ..."); LOGGER.error("get null ...");
} }
final Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex); final Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
outTriangleVertices[0] = trianglePoints.get(0).multiply(this.scaling); outTriangleVertices[0] = trianglePoints.get(0).multiply(this.scaling);
outTriangleVertices[1] = trianglePoints.get(1).multiply(this.scaling); outTriangleVertices[1] = trianglePoints.get(1).multiply(this.scaling);
outTriangleVertices[2] = trianglePoints.get(2).multiply(this.scaling); outTriangleVertices[2] = trianglePoints.get(2).multiply(this.scaling);
} }
/// Insert all the triangles longo the dynamic AABB tree /// Insert all the triangles longo the dynamic AABB tree
protected void initBVHTree() { protected void initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree // 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 @Override
public boolean isConvex() { public boolean isConvex() {
return false; return false;
} }
@Override @Override
public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) { public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
// Create the callback object that will compute ray casting against triangles // 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. // 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 // The raycastCallback object will then compute ray casting against the triangles
// in the hit AABBs. // in the hit AABBs.
this.dynamicAABBTree.raycast(ray, new CallbackRaycast() { this.dynamicAABBTree.raycast(ray, new CallbackRaycast() {
@Override @Override
public float callback(final DTree node, final Ray ray) { public float callback(final DTree node, final Ray ray) {
return raycastCallback.operatorparenthese(node, ray); return raycastCallback.operatorparenthese(node, ray);
@ -179,14 +188,14 @@ public class ConcaveMeshShape extends ConcaveShape {
raycastCallback.raycastTriangles(); raycastCallback.raycastTriangles();
return raycastCallback.getIsHit(); return raycastCallback.getIsHit();
} }
@Override @Override
public void setLocalScaling(final Vector3f scaling) { public void setLocalScaling(final Vector3f scaling) {
super.setLocalScaling(scaling); super.setLocalScaling(scaling);
this.dynamicAABBTree.reset(); this.dynamicAABBTree.reset();
initBVHTree(); initBVHTree();
} }
@Override @Override
public void testAllTriangles(final ConcaveShape.TriangleCallback callback, final AABB localAABB) { public void testAllTriangles(final ConcaveShape.TriangleCallback callback, final AABB localAABB) {
// Ask the Dynamic AABB Tree to report all the triangles that are overlapping // Ask the Dynamic AABB Tree to report all the triangles that are overlapping

14
src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java
View File

@ -15,23 +15,23 @@ public abstract class ConvexShape extends CollisionShape {
// Return a local support point in a given direction with the object margin // Return a local support point in a given direction with the object margin
public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) { public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) {
////Log.error(" -> getLocalSupportPointWithMargin(" + direction); ////LOGGER.error(" -> getLocalSupportPointWithMargin(" + direction);
// Get the support point without margin // Get the support point without margin
Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
////Log.error(" -> supportPoint = " + supportPoint); ////LOGGER.error(" -> supportPoint = " + supportPoint);
////Log.error(" -> margin = " + FMath.floatToString(this.margin)); ////LOGGER.error(" -> margin = " + FMath.floatToString(this.margin));
if (this.margin != 0.0f) { if (this.margin != 0.0f) {
// Add the margin to the support point // Add the margin to the support point
Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f); Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f);
////Log.error(" -> direction.length2()=" + FMath.floatToString(direction.length2())); ////LOGGER.error(" -> direction.length2()=" + FMath.floatToString(direction.length2()));
////Log.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON)); ////LOGGER.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON));
if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) { if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
unitVec = direction.safeNormalize(); unitVec = direction.safeNormalize();
////Log.error(" -> unitVec= " + unitVec); ////LOGGER.error(" -> unitVec= " + unitVec);
} }
supportPoint = supportPoint.add(unitVec.multiply(this.margin)); supportPoint = supportPoint.add(unitVec.multiply(this.margin));
} }
////Log.error(" -> ==> supportPoint = " + supportPoint); ////LOGGER.error(" -> ==> supportPoint = " + supportPoint);
return supportPoint; return supportPoint;
} }

452
src/org/atriasoft/ephysics/engine/ContactSolver.java
View File

@ -119,21 +119,21 @@ public class ContactSolver {
* @param manifold Constraint to apply the impulse * @param manifold Constraint to apply the impulse
*/ */
private void applyImpulse(final Impulse impulse, final ContactManifoldSolver manifold) { private void applyImpulse(final Impulse impulse, final ContactManifoldSolver manifold) {
//Log.info(" -- manifold.massInverseBody1=" + manifold.massInverseBody1); //LOGGER.info(" -- manifold.massInverseBody1=" + manifold.massInverseBody1);
//Log.info(" -- manifold.inverseInertiaTensorBody1=" + manifold.inverseInertiaTensorBody1); //LOGGER.info(" -- manifold.inverseInertiaTensorBody1=" + manifold.inverseInertiaTensorBody1);
//Log.info(" -- manifold.massInverseBody2=" + manifold.massInverseBody2); //LOGGER.info(" -- manifold.massInverseBody2=" + manifold.massInverseBody2);
//Log.info(" -- manifold.inverseInertiaTensorBody2=" + manifold.inverseInertiaTensorBody2); //LOGGER.info(" -- manifold.inverseInertiaTensorBody2=" + manifold.inverseInertiaTensorBody2);
//Log.info(" -- impulse.angularImpulseBody2=" + impulse.angularImpulseBody2); //LOGGER.info(" -- impulse.angularImpulseBody2=" + impulse.angularImpulseBody2);
// Update the velocities of the body 1 by applying the impulse P // 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)); 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())); 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 // 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)); 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())); 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 * @return Impulse of the friction result
*/ */
private Impulse computeFriction1Impulse(final float deltaLambda, final ContactPointSolver contactPoint) { private Impulse computeFriction1Impulse(final float deltaLambda, final ContactPointSolver contactPoint) {
//Log.error("=== r1CrossT1=" + contactPoint.r1CrossT1); //LOGGER.error("=== r1CrossT1=" + contactPoint.r1CrossT1);
//Log.error("=== r2CrossT1=" + contactPoint.r2CrossT1); //LOGGER.error("=== r2CrossT1=" + contactPoint.r2CrossT1);
return new Impulse(contactPoint.frictionVector1.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT1.multiply(-1).multiply(deltaLambda), return new Impulse(contactPoint.frictionVector1.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT1.multiply(-1).multiply(deltaLambda),
contactPoint.frictionVector1.multiply(deltaLambda), contactPoint.r2CrossT1.multiply(deltaLambda)); contactPoint.frictionVector1.multiply(deltaLambda), contactPoint.r2CrossT1.multiply(deltaLambda));
} }
@ -177,8 +177,8 @@ public class ContactSolver {
* @return Impulse of the friction result * @return Impulse of the friction result
*/ */
private Impulse computeFriction2Impulse(final float deltaLambda, final ContactPointSolver contactPoint) { private Impulse computeFriction2Impulse(final float deltaLambda, final ContactPointSolver contactPoint) {
//Log.error("=== r1CrossT2=" + contactPoint.r1CrossT2); //LOGGER.error("=== r1CrossT2=" + contactPoint.r1CrossT2);
//Log.error("=== r2CrossT2=" + contactPoint.r2CrossT2); //LOGGER.error("=== r2CrossT2=" + contactPoint.r2CrossT2);
return new Impulse(contactPoint.frictionvec2.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT2.multiply(-1).multiply(deltaLambda), contactPoint.frictionvec2.multiply(deltaLambda), return new Impulse(contactPoint.frictionvec2.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT2.multiply(-1).multiply(deltaLambda), contactPoint.frictionvec2.multiply(deltaLambda),
contactPoint.r2CrossT2.multiply(deltaLambda)); contactPoint.r2CrossT2.multiply(deltaLambda));
} }
@ -200,11 +200,11 @@ public class ContactSolver {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent
// velocity difference // velocity difference
contactManifold.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity); contactManifold.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity);
//Log.error("===1==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1); //LOGGER.error("===1==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1);
} else { } else {
// Get any orthogonal vector to the normal as the first friction vector // Get any orthogonal vector to the normal as the first friction vector
contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector(); 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 // 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) { private void computeFrictionVectors(final Vector3f deltaVelocity, final ContactPointSolver contactPoint) {
assert (contactPoint.normal.length() > 0.0f); assert (contactPoint.normal.length() > 0.0f);
//Log.error("===3==>>>>>> contactPoint.normal=" + contactPoint.normal); //LOGGER.error("===3==>>>>>> contactPoint.normal=" + contactPoint.normal);
//Log.error("===3==>>>>>> deltaVelocity=" + deltaVelocity); //LOGGER.error("===3==>>>>>> deltaVelocity=" + deltaVelocity);
// Compute the velocity difference vector in the tangential plane // Compute the velocity difference vector in the tangential plane
final Vector3f normalVelocity = contactPoint.normal.multiply(deltaVelocity.dot(contactPoint.normal)); 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); 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 // If the velocty difference in the tangential plane is not zero
final float lengthTangenVelocity = tangentVelocity.length(); final float lengthTangenVelocity = tangentVelocity.length();
//Log.error("===3==>>>>>> lengthTangenVelocity=" + FMath.floatToString(lengthTangenVelocity)); //LOGGER.error("===3==>>>>>> lengthTangenVelocity=" + FMath.floatToString(lengthTangenVelocity));
if (lengthTangenVelocity > Constant.FLOAT_EPSILON) { if (lengthTangenVelocity > Constant.FLOAT_EPSILON) {
// Compute the first friction vector in the direction of the tangent // Compute the first friction vector in the direction of the tangent
// velocity difference // velocity difference
contactPoint.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity); contactPoint.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity);
//Log.error("===3==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1); //LOGGER.error("===3==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1);
} else { } else {
// Get any orthogonal vector to the normal as the first friction vector // Get any orthogonal vector to the normal as the first friction vector
contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector(); 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 // The second friction vector is computed by the cross product of the firs
// friction vector and the contact normal // friction vector and the contact normal
@ -265,8 +265,8 @@ public class ContactSolver {
private float computeMixedRestitutionFactor(final RigidBody body1, final RigidBody body2) { private float computeMixedRestitutionFactor(final RigidBody body1, final RigidBody body2) {
final float restitution1 = body1.getMaterial().getBounciness(); final float restitution1 = body1.getMaterial().getBounciness();
final float restitution2 = body2.getMaterial().getBounciness(); final float restitution2 = body2.getMaterial().getBounciness();
//Log.error("######################### restitution1=" + FMath.floatToString(restitution1)); //LOGGER.error("######################### restitution1=" + FMath.floatToString(restitution1));
//Log.error("######################### restitution2=" + FMath.floatToString(restitution2)); //LOGGER.error("######################### restitution2=" + FMath.floatToString(restitution2));
// Return the largest restitution factor // Return the largest restitution factor
return (restitution1 > restitution2) ? restitution1 : restitution2; return (restitution1 > restitution2) ? restitution1 : restitution2;
} }
@ -288,8 +288,8 @@ public class ContactSolver {
* @return Impulse of the penetration result * @return Impulse of the penetration result
*/ */
private Impulse computePenetrationImpulse(final float deltaLambda, final ContactPointSolver contactPoint) { private Impulse computePenetrationImpulse(final float deltaLambda, final ContactPointSolver contactPoint) {
//Log.error("=== r1CrossN=" + contactPoint.r1CrossN); //LOGGER.error("=== r1CrossN=" + contactPoint.r1CrossN);
//Log.error("=== r2CrossN=" + contactPoint.r2CrossN); //LOGGER.error("=== r2CrossN=" + contactPoint.r2CrossN);
return new Impulse(contactPoint.normal.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossN.multiply(-1).multiply(deltaLambda), contactPoint.normal.multiply(deltaLambda), return new Impulse(contactPoint.normal.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossN.multiply(-1).multiply(deltaLambda), contactPoint.normal.multiply(deltaLambda),
contactPoint.r2CrossN.multiply(deltaLambda)); contactPoint.r2CrossN.multiply(deltaLambda));
} }
@ -300,65 +300,65 @@ public class ContactSolver {
private void initializeContactConstraints() { private void initializeContactConstraints() {
// For each contact raint // For each contact raint
for (int ccc = 0; ccc < this.contactConstraints.size(); ccc++) { for (int ccc = 0; ccc < this.contactConstraints.size(); ccc++) {
//Log.warning("}}}} ccc=" + ccc); //LOGGER.warn("}}}} ccc=" + ccc);
final ContactManifoldSolver manifold = this.contactConstraints.get(ccc); final ContactManifoldSolver manifold = this.contactConstraints.get(ccc);
// Get the inertia tensors of both bodies // Get the inertia tensors of both bodies
final Matrix3f i1 = manifold.inverseInertiaTensorBody1; final Matrix3f i1 = manifold.inverseInertiaTensorBody1;
//Log.warning("}}}} I1=" + I1); //LOGGER.warn("}}}} I1=" + I1);
final Matrix3f i2 = manifold.inverseInertiaTensorBody2; 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 we solve the friction raints at the center of the contact manifold
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
manifold.normal = Vector3f.ZERO; 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 // Get the velocities of the bodies
final Vector3f v1 = this.linearVelocities[manifold.indexBody1]; final Vector3f v1 = this.linearVelocities[manifold.indexBody1];
final Vector3f w1 = this.angularVelocities[manifold.indexBody1]; final Vector3f w1 = this.angularVelocities[manifold.indexBody1];
final Vector3f v2 = this.linearVelocities[manifold.indexBody2]; final Vector3f v2 = this.linearVelocities[manifold.indexBody2];
final Vector3f w2 = this.angularVelocities[manifold.indexBody2]; final Vector3f w2 = this.angularVelocities[manifold.indexBody2];
//Log.warning("}}}} v1=" + v1); //LOGGER.warn("}}}} v1=" + v1);
//Log.warning("}}}} w1=" + w1); //LOGGER.warn("}}}} w1=" + w1);
//Log.warning("}}}} v2=" + v2); //LOGGER.warn("}}}} v2=" + v2);
//Log.warning("}}}} w2=" + w2); //LOGGER.warn("}}}} w2=" + w2);
// For each contact point raint // For each contact point raint
for (int iii = 0; iii < manifold.nbContacts; iii++) { for (int iii = 0; iii < manifold.nbContacts; iii++) {
//Log.warning("}}}} iii=" + iii); //LOGGER.warn("}}}} iii=" + iii);
final ContactPointSolver contactPoint = manifold.contacts[iii]; final ContactPointSolver contactPoint = manifold.contacts[iii];
//Log.warning("}}}} contactPoint.r1=" + contactPoint.r1); //LOGGER.warn("}}}} contactPoint.r1=" + contactPoint.r1);
//Log.warning("}}}} contactPoint.r2=" + contactPoint.r2); //LOGGER.warn("}}}} contactPoint.r2=" + contactPoint.r2);
//Log.warning("}}}} contactPoint.normal=" + contactPoint.normal); //LOGGER.warn("}}}} contactPoint.normal=" + contactPoint.normal);
final ContactPoint externalContact = contactPoint.externalContact; final ContactPoint externalContact = contactPoint.externalContact;
// Compute the velocity difference // Compute the velocity difference
final Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); 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.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal); contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
//Log.warning("}}}} contactPoint.r1CrossN=" + contactPoint.r1CrossN); //LOGGER.warn("}}}} contactPoint.r1CrossN=" + contactPoint.r1CrossN);
//Log.warning("}}}} contactPoint.r2CrossN=" + contactPoint.r2CrossN); //LOGGER.warn("}}}} contactPoint.r2CrossN=" + contactPoint.r2CrossN);
// Compute the inverse mass matrix K for the penetration raint // 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) 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); + ((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) { if (massPenetration > 0.0f) {
contactPoint.inversePenetrationMass = 1.0f / massPenetration; 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 // 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) { if (!this.isSolveFrictionAtContactManifoldCenterActive) {
// Compute the friction vectors // Compute the friction vectors
computeFrictionVectors(deltaV, contactPoint); computeFrictionVectors(deltaV, contactPoint);
//Log.warning("}}}} contactPoint.frictionVector1=" + contactPoint.frictionVector1); //LOGGER.warn("}}}} contactPoint.frictionVector1=" + contactPoint.frictionVector1);
contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1);
contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2); contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionvec2);
contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1);
contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionvec2);
//Log.warning("}}}} contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); //LOGGER.warn("}}}} contactPoint.r1CrossT1=" + contactPoint.r1CrossT1);
//Log.warning("}}}} contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); //LOGGER.warn("}}}} contactPoint.r1CrossT2=" + contactPoint.r1CrossT2);
//Log.warning("}}}} contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); //LOGGER.warn("}}}} contactPoint.r2CrossT1=" + contactPoint.r2CrossT1);
//Log.warning("}}}} contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); //LOGGER.warn("}}}} contactPoint.r2CrossT2=" + contactPoint.r2CrossT2);
// Compute the inverse mass matrix K for the friction // Compute the inverse mass matrix K for the friction
// raints at each contact point // raints at each contact point
final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossT1)).cross(contactPoint.r1)).dot(contactPoint.frictionVector1) 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 // 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 // velocity bellow a given threshold), we do not add a restitution velocity bias
contactPoint.restitutionBias = 0.0f; contactPoint.restitutionBias = 0.0f;
//Log.warning("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + contactPoint.restitutionBias); //LOGGER.warn("}}}}+++++++++++++++++++++ contactPoint.restitutionBias=" + contactPoint.restitutionBias);
final float deltaVDotN = deltaV.dot(contactPoint.normal); final float deltaVDotN = deltaV.dot(contactPoint.normal);
//Log.warning("}}}}+++++++++++++++++++++ deltaV=" + deltaV); //LOGGER.warn("}}}}+++++++++++++++++++++ deltaV=" + deltaV);
//Log.warning("}}}}+++++++++++++++++++++ contactPoint.normal=" + contactPoint.normal); //LOGGER.warn("}}}}+++++++++++++++++++++ contactPoint.normal=" + contactPoint.normal);
//Log.warning("}}}}+++++++++++++++++++++ deltaVDotN=" + FMath.floatToString(deltaVDotN)); //LOGGER.warn("}}}}+++++++++++++++++++++ deltaVDotN=" + FMath.floatToString(deltaVDotN));
//Log.warning("}}}}+++++++++++++++++++++ Configuration.RESTITUTIONVELOCITYTHRESHOLD=" + FMath.floatToString(Configuration.RESTITUTIONVELOCITYTHRESHOLD)); //LOGGER.warn("}}}}+++++++++++++++++++++ Configuration.RESTITUTIONVELOCITYTHRESHOLD=" + FMath.floatToString(Configuration.RESTITUTIONVELOCITYTHRESHOLD));
//Log.warning("}}}}+++++++++++++++++++++ manifold.restitutionFactor=" + FMath.floatToString(manifold.restitutionFactor)); //LOGGER.warn("}}}}+++++++++++++++++++++ manifold.restitutionFactor=" + FMath.floatToString(manifold.restitutionFactor));
if (deltaVDotN < -Configuration.RESTITUTION_VELOCITY_THRESHOLD) { if (deltaVDotN < -Configuration.RESTITUTION_VELOCITY_THRESHOLD) {
contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; 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 the warm starting of the contact solver is active
if (this.isWarmStartingActive) { if (this.isWarmStartingActive) {
@ -400,9 +400,9 @@ public class ContactSolver {
contactPoint.penetrationSplitImpulse = 0.0f; contactPoint.penetrationSplitImpulse = 0.0f;
// If we solve the friction raints at the center of the contact manifold // If we solve the friction raints at the center of the contact manifold
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
//Log.error("]]] contactPoint.normal=" + contactPoint.normal); //LOGGER.error("]]] contactPoint.normal=" + contactPoint.normal);
manifold.normal = manifold.normal.add(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 // 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 we solve the friction raints at the center of the contact manifold
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
//Log.error("]]] manifold.normal=" + manifold.normal); //LOGGER.error("]]] manifold.normal=" + manifold.normal);
manifold.normal = manifold.normal.normalize(); 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)); 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 // Compute the friction vectors
computeFrictionVectors(deltaVFrictionPoint, manifold); computeFrictionVectors(deltaVFrictionPoint, manifold);
// Compute the inverse mass matrix K for the friction raints at the center of // 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.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionvec2);
manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1);
manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2); manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionvec2);
//Log.warning("}}}} manifold.r1CrossT1=" + manifold.r1CrossT1); //LOGGER.warn("}}}} manifold.r1CrossT1=" + manifold.r1CrossT1);
//Log.warning("}}}} manifold.r1CrossT2=" + manifold.r1CrossT2); //LOGGER.warn("}}}} manifold.r1CrossT2=" + manifold.r1CrossT2);
//Log.warning("}}}} manifold.r2CrossT1=" + manifold.r2CrossT1); //LOGGER.warn("}}}} manifold.r2CrossT1=" + manifold.r2CrossT1);
//Log.warning("}}}} manifold.r2CrossT2=" + manifold.r2CrossT2); //LOGGER.warn("}}}} manifold.r2CrossT2=" + manifold.r2CrossT2);
final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(manifold.r1CrossT1)).cross(manifold.r1Friction)).dot(manifold.frictionVector1) 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); + ((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) 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); + ((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)) final float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1.multiply(manifold.normal))
+ manifold.normal.dot(manifold.inverseInertiaTensorBody2.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) { if (friction1Mass > 0.0f) {
manifold.inverseFriction1Mass = 1.0f / friction1Mass; 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) { if (friction2Mass > 0.0f) {
manifold.inverseFriction2Mass = 1.0f / friction2Mass; 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) { if (frictionTwistMass > 0.0f) {
manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass; 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) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 = new Vector3f(0.0f, 0.0f, 0.0f); internalManifold.frictionPointBody1 = new Vector3f(0.0f, 0.0f, 0.0f);
internalManifold.frictionPointBody2 = new Vector3f(0.0f, 0.0f, 0.0f); internalManifold.frictionPointBody2 = new Vector3f(0.0f, 0.0f, 0.0f);
//Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1);
//Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2);
} }
// For each contact point of the contact manifold // For each contact point of the contact manifold
for (int ccc = 0; ccc < externalManifold.getNbContactPoints(); ++ccc) { for (int ccc = 0; ccc < externalManifold.getNbContactPoints(); ++ccc) {
@ -536,33 +536,33 @@ public class ContactSolver {
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.add(p1); internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.add(p1);
internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.add(p2); internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.add(p2);
//Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1);
//Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2);
} }
} }
// If we solve the friction raints at the center of the contact manifold // If we solve the friction raints at the center of the contact manifold
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.divide(internalManifold.nbContacts); internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.divide(internalManifold.nbContacts);
internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.divide(internalManifold.nbContacts); internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.divide(internalManifold.nbContacts);
//Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); //LOGGER.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1);
//Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); //LOGGER.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2);
internalManifold.r1Friction = internalManifold.frictionPointBody1.less(x1); internalManifold.r1Friction = internalManifold.frictionPointBody1.less(x1);
internalManifold.r2Friction = internalManifold.frictionPointBody2.less(x2); internalManifold.r2Friction = internalManifold.frictionPointBody2.less(x2);
//Log.error("]]] internalManifold.r1Friction=" + internalManifold.r1Friction); //LOGGER.error("]]] internalManifold.r1Friction=" + internalManifold.r1Friction);
//Log.error("]]] internalManifold.r2Friction=" + internalManifold.r2Friction); //LOGGER.error("]]] internalManifold.r2Friction=" + internalManifold.r2Friction);
internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1(); internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1();
internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2(); internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2();
//Log.error("]]] internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1); //LOGGER.error("]]] internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1);
//Log.error("]]] internalManifold.oldFrictionvec2=" + internalManifold.oldFrictionvec2); //LOGGER.error("]]] internalManifold.oldFrictionvec2=" + internalManifold.oldFrictionvec2);
// If warm starting is active // If warm starting is active
if (this.isWarmStartingActive) { if (this.isWarmStartingActive) {
// Initialize the accumulated impulses with the previous step accumulated impulses // Initialize the accumulated impulses with the previous step accumulated impulses
internalManifold.friction1Impulse = externalManifold.getFrictionImpulse1(); internalManifold.friction1Impulse = externalManifold.getFrictionImpulse1();
internalManifold.friction2Impulse = externalManifold.getFrictionImpulse2(); internalManifold.friction2Impulse = externalManifold.getFrictionImpulse2();
internalManifold.frictionTwistImpulse = externalManifold.getFrictionTwistImpulse(); internalManifold.frictionTwistImpulse = externalManifold.getFrictionTwistImpulse();
//Log.error("]]] internalManifold.friction1Impulse=" + FMath.floatToString(internalManifold.friction1Impulse)); //LOGGER.error("]]] internalManifold.friction1Impulse=" + FMath.floatToString(internalManifold.friction1Impulse));
//Log.error("]]] internalManifold.friction2Impulse=" + FMath.floatToString(internalManifold.friction2Impulse)); //LOGGER.error("]]] internalManifold.friction2Impulse=" + FMath.floatToString(internalManifold.friction2Impulse));
//Log.error("]]] internalManifold.frictionTwistImpulse=" + FMath.floatToString(internalManifold.frictionTwistImpulse)); //LOGGER.error("]]] internalManifold.frictionTwistImpulse=" + FMath.floatToString(internalManifold.frictionTwistImpulse));
} else { } else {
// Initialize the accumulated impulses to zero // Initialize the accumulated impulses to zero
internalManifold.friction1Impulse = 0.0f; internalManifold.friction1Impulse = 0.0f;
@ -630,12 +630,12 @@ public class ContactSolver {
* Solve the contacts * Solve the contacts
*/ */
public void solve() { public void solve() {
//Log.warning("========================================"); //LOGGER.warn("========================================");
//Log.warning("== Contact SOLVER ... " + this.contactConstraints.size()); //LOGGER.warn("== Contact SOLVER ... " + this.contactConstraints.size());
//Log.warning("========================================"); //LOGGER.warn("========================================");
// For each contact manifold // For each contact manifold
for (int ccc = 0; ccc < this.contactConstraints.size(); ++ccc) { 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); final ContactManifoldSolver contactManifold = this.contactConstraints.get(ccc);
float sumPenetrationImpulse = 0.0f; float sumPenetrationImpulse = 0.0f;
// Get the rained velocities // Get the rained velocities
@ -643,47 +643,47 @@ public class ContactSolver {
final Vector3f w1 = this.angularVelocities[contactManifold.indexBody1]; final Vector3f w1 = this.angularVelocities[contactManifold.indexBody1];
final Vector3f v2 = this.linearVelocities[contactManifold.indexBody2]; final Vector3f v2 = this.linearVelocities[contactManifold.indexBody2];
final Vector3f w2 = this.angularVelocities[contactManifold.indexBody2]; final Vector3f w2 = this.angularVelocities[contactManifold.indexBody2];
//Log.warning(" v1=" + v1); //LOGGER.warn(" v1=" + v1);
//Log.warning(" w1=" + w1); //LOGGER.warn(" w1=" + w1);
//Log.warning(" v2=" + v2); //LOGGER.warn(" v2=" + v2);
//Log.warning(" w2=" + w2); //LOGGER.warn(" w2=" + w2);
for (int iii = 0; iii < contactManifold.nbContacts; ++iii) { for (int iii = 0; iii < contactManifold.nbContacts; ++iii) {
//Log.warning(" iii=" + iii); //LOGGER.warn(" iii=" + iii);
final ContactPointSolver contactPoint = contactManifold.contacts[iii]; final ContactPointSolver contactPoint = contactManifold.contacts[iii];
// --------- Penetration --------- // // --------- Penetration --------- //
// Compute J*v // Compute J*v
Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
final float deltaVDotN = deltaV.dot(contactPoint.normal); final float deltaVDotN = deltaV.dot(contactPoint.normal);
//Log.warning(" contactPoint.R1=" + contactPoint.r1); //LOGGER.warn(" contactPoint.R1=" + contactPoint.r1);
//Log.warning(" contactPoint.R2=" + contactPoint.r2); //LOGGER.warn(" contactPoint.R2=" + contactPoint.r2);
//Log.warning(" contactPoint.r1CrossN=" + contactPoint.r1CrossN); //LOGGER.warn(" contactPoint.r1CrossN=" + contactPoint.r1CrossN);
//Log.warning(" contactPoint.r2CrossN=" + contactPoint.r2CrossN); //LOGGER.warn(" contactPoint.r2CrossN=" + contactPoint.r2CrossN);
//Log.warning(" contactPoint.r1CrossT1=" + contactPoint.r1CrossT1); //LOGGER.warn(" contactPoint.r1CrossT1=" + contactPoint.r1CrossT1);
//Log.warning(" contactPoint.r2CrossT1=" + contactPoint.r2CrossT1); //LOGGER.warn(" contactPoint.r2CrossT1=" + contactPoint.r2CrossT1);
//Log.warning(" contactPoint.r1CrossT2=" + contactPoint.r1CrossT2); //LOGGER.warn(" contactPoint.r1CrossT2=" + contactPoint.r1CrossT2);
//Log.warning(" contactPoint.r2CrossT2=" + contactPoint.r2CrossT2); //LOGGER.warn(" contactPoint.r2CrossT2=" + contactPoint.r2CrossT2);
//Log.warning(" contactPoint.penetrationDepth=" + FMath.floatToString(contactPoint.penetrationDepth)); //LOGGER.warn(" contactPoint.penetrationDepth=" + FMath.floatToString(contactPoint.penetrationDepth));
//Log.warning(" contactPoint.normal=" + contactPoint.normal); //LOGGER.warn(" contactPoint.normal=" + contactPoint.normal);
//Log.warning(" deltaV=" + deltaV); //LOGGER.warn(" deltaV=" + deltaV);
float jv = deltaVDotN; float jv = deltaVDotN;
// Compute the bias "b" of the raint // Compute the bias "b" of the raint
final float beta = this.isSplitImpulseActive ? ContactSolver.BETA_SPLIT_IMPULSE : ContactSolver.BETA; final float beta = this.isSplitImpulseActive ? ContactSolver.BETA_SPLIT_IMPULSE : ContactSolver.BETA;
//Log.warning(" beta=" + FMath.floatToString(beta)); //LOGGER.warn(" beta=" + FMath.floatToString(beta));
//Log.warning(" BETA=" + FMath.floatToString(BETA)); //LOGGER.warn(" BETA=" + FMath.floatToString(BETA));
//Log.warning(" SLOP=" + FMath.floatToString(SLOP)); //LOGGER.warn(" SLOP=" + FMath.floatToString(SLOP));
//Log.warning(" this.timeStep=" + FMath.floatToString(this.timeStep)); //LOGGER.warn(" this.timeStep=" + FMath.floatToString(this.timeStep));
//Log.warning(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); //LOGGER.warn(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias));
float biasPenetrationDepth = 0.0f; float biasPenetrationDepth = 0.0f;
if (contactPoint.penetrationDepth > ContactSolver.SLOP) { if (contactPoint.penetrationDepth > ContactSolver.SLOP) {
//Log.warning(" (beta / this.timeStep)=" + FMath.floatToString((beta / this.timeStep))); //LOGGER.warn(" (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(" 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); 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)); //LOGGER.warn(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias));
//Log.warning(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); //LOGGER.warn(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth));
final float b = biasPenetrationDepth + contactPoint.restitutionBias; final float b = biasPenetrationDepth + contactPoint.restitutionBias;
//Log.warning(" b=" + FMath.floatToString(b)); //LOGGER.warn(" b=" + FMath.floatToString(b));
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
float deltaLambda; float deltaLambda;
if (this.isSplitImpulseActive) { if (this.isSplitImpulseActive) {
@ -691,24 +691,24 @@ public class ContactSolver {
} else { } else {
deltaLambda = -(jv + b) * contactPoint.inversePenetrationMass; deltaLambda = -(jv + b) * contactPoint.inversePenetrationMass;
} }
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
float lambdaTemp = contactPoint.penetrationImpulse; float lambdaTemp = contactPoint.penetrationImpulse;
contactPoint.penetrationImpulse = FMath.max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); contactPoint.penetrationImpulse = FMath.max(contactPoint.penetrationImpulse + deltaLambda, 0.0f);
deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; deltaLambda = contactPoint.penetrationImpulse - lambdaTemp;
//Log.warning(" contactPoint.penetrationImpulse=" + FMath.floatToString(contactPoint.penetrationImpulse)); //LOGGER.warn(" contactPoint.penetrationImpulse=" + FMath.floatToString(contactPoint.penetrationImpulse));
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
final Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); final Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint);
//Log.warning(" impulsePenetration.a1=" + impulsePenetration.angularImpulseBody1); //LOGGER.warn(" impulsePenetration.a1=" + impulsePenetration.angularImpulseBody1);
//Log.warning(" impulsePenetration.a2=" + impulsePenetration.angularImpulseBody2); //LOGGER.warn(" impulsePenetration.a2=" + impulsePenetration.angularImpulseBody2);
//Log.warning(" impulsePenetration.i1=" + impulsePenetration.linearImpulseBody1); //LOGGER.warn(" impulsePenetration.i1=" + impulsePenetration.linearImpulseBody1);
//Log.warning(" impulsePenetration.i2=" + impulsePenetration.linearImpulseBody2); //LOGGER.warn(" impulsePenetration.i2=" + impulsePenetration.linearImpulseBody2);
// Apply the impulse to the bodies of the raint // Apply the impulse to the bodies of the raint
applyImpulse(impulsePenetration, contactManifold); applyImpulse(impulsePenetration, contactManifold);
sumPenetrationImpulse += contactPoint.penetrationImpulse; sumPenetrationImpulse += contactPoint.penetrationImpulse;
//Log.warning(" sumpenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); //LOGGER.warn(" sumpenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse));
//Log.warning(" isSplitImpulseActive=" + this.isSplitImpulseActive); //LOGGER.warn(" isSplitImpulseActive=" + this.isSplitImpulseActive);
//Log.warning(" isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive); //LOGGER.warn(" isSolveFrictionAtContactManifoldCenterActive=" + this.isSolveFrictionAtContactManifoldCenterActive);
// If the split impulse position correction is active // If the split impulse position correction is active
if (this.isSplitImpulseActive) { if (this.isSplitImpulseActive) {
// Split impulse (position correction) // Split impulse (position correction)
@ -717,7 +717,7 @@ public class ContactSolver {
final Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; final Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2];
final Vector3f w2Split = this.splitAngularVelocities[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)); 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 jvSplit = deltaVSplit.dot(contactPoint.normal);
final float deltaLambdaSplit = -(jvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; final float deltaLambdaSplit = -(jvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
final float lambdaTempSplit = contactPoint.penetrationSplitImpulse; final float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
@ -725,10 +725,10 @@ public class ContactSolver {
deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
final Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint); final Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint);
//Log.warning(" splitImpulsePenetration.a1=" + splitImpulsePenetration.angularImpulseBody1); //LOGGER.warn(" splitImpulsePenetration.a1=" + splitImpulsePenetration.angularImpulseBody1);
//Log.warning(" splitImpulsePenetration.a2=" + splitImpulsePenetration.angularImpulseBody2); //LOGGER.warn(" splitImpulsePenetration.a2=" + splitImpulsePenetration.angularImpulseBody2);
//Log.warning(" splitImpulsePenetration.i1=" + splitImpulsePenetration.linearImpulseBody1); //LOGGER.warn(" splitImpulsePenetration.i1=" + splitImpulsePenetration.linearImpulseBody1);
//Log.warning(" splitImpulsePenetration.i2=" + splitImpulsePenetration.linearImpulseBody2); //LOGGER.warn(" splitImpulsePenetration.i2=" + splitImpulsePenetration.linearImpulseBody2);
applySplitImpulse(splitImpulsePenetration, contactManifold); applySplitImpulse(splitImpulsePenetration, contactManifold);
} }
// If we do not solve the friction raints at the center of the contact manifold // 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 // Compute the Lagrange multiplier lambda
deltaLambda = -jv; deltaLambda = -jv;
deltaLambda *= contactPoint.inverseFriction1Mass; deltaLambda *= contactPoint.inverseFriction1Mass;
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction1Impulse; lambdaTemp = contactPoint.friction1Impulse;
contactPoint.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); contactPoint.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction1Impulse + deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction1Impulse - lambdaTemp; deltaLambda = contactPoint.friction1Impulse - lambdaTemp;
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
final Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); final Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint);
//Log.warning(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); //LOGGER.warn(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1);
//Log.warning(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); //LOGGER.warn(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2);
//Log.warning(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); //LOGGER.warn(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1);
//Log.warning(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); //LOGGER.warn(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction1, contactManifold); applyImpulse(impulseFriction1, contactManifold);
@ -763,18 +763,18 @@ public class ContactSolver {
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
deltaLambda = -jv; deltaLambda = -jv;
deltaLambda *= contactPoint.inverseFriction2Mass; deltaLambda *= contactPoint.inverseFriction2Mass;
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
lambdaTemp = contactPoint.friction2Impulse; lambdaTemp = contactPoint.friction2Impulse;
contactPoint.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); contactPoint.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactPoint.friction2Impulse + deltaLambda, frictionLimit));
deltaLambda = contactPoint.friction2Impulse - lambdaTemp; deltaLambda = contactPoint.friction2Impulse - lambdaTemp;
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
final Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); final Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint);
//Log.warning(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); //LOGGER.warn(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1);
//Log.warning(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); //LOGGER.warn(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2);
//Log.warning(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); //LOGGER.warn(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1);
//Log.warning(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); //LOGGER.warn(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction2, contactManifold); applyImpulse(impulseFriction2, contactManifold);
// --------- Rolling resistance raint --------- // // --------- Rolling resistance raint --------- //
@ -789,82 +789,82 @@ public class ContactSolver {
deltaLambdaRolling = contactPoint.rollingResistanceImpulse.less(lambdaTempRolling); deltaLambdaRolling = contactPoint.rollingResistanceImpulse.less(lambdaTempRolling);
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
final Impulse impulseRolling = new Impulse(Vector3f.ZERO, deltaLambdaRolling.multiply(-1), Vector3f.ZERO, deltaLambdaRolling); final Impulse impulseRolling = new Impulse(Vector3f.ZERO, deltaLambdaRolling.multiply(-1), Vector3f.ZERO, deltaLambdaRolling);
//Log.warning(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); //LOGGER.warn(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1);
//Log.warning(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); //LOGGER.warn(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2);
//Log.warning(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); //LOGGER.warn(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1);
//Log.warning(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); //LOGGER.warn(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseRolling, contactManifold); applyImpulse(impulseRolling, contactManifold);
} }
} }
} }
//Log.info(" w1=" + w1); //LOGGER.info(" w1=" + w1);
//Log.info(" w2=" + w2); //LOGGER.info(" w2=" + w2);
// If we solve the friction raints at the center of the contact manifold // If we solve the friction raints at the center of the contact manifold
if (this.isSolveFrictionAtContactManifoldCenterActive) { if (this.isSolveFrictionAtContactManifoldCenterActive) {
//Log.warning(" HHH isSolveFrictionAtContactManifoldCenterActive"); //LOGGER.warn(" HHH isSolveFrictionAtContactManifoldCenterActive");
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
// ------ First friction raint at the center of the contact manifol ------ // // ------ First friction raint at the center of the contact manifol ------ //
// Compute J*v // Compute J*v
Vector3f deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction)); Vector3f deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction));
float jv = deltaV.dot(contactManifold.frictionVector1); float jv = deltaV.dot(contactManifold.frictionVector1);
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
//Log.warning(" Jv=" + FMath.floatToString(Jv)); //LOGGER.warn(" Jv=" + FMath.floatToString(Jv));
//Log.warning(" contactManifold.frictionVector1=" + contactManifold.frictionVector1); //LOGGER.warn(" contactManifold.frictionVector1=" + contactManifold.frictionVector1);
//Log.warning(" contactManifold.inverseFriction1Mass=" + FMath.floatToString(contactManifold.inverseFriction1Mass)); //LOGGER.warn(" contactManifold.inverseFriction1Mass=" + FMath.floatToString(contactManifold.inverseFriction1Mass));
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
float deltaLambda = -jv * contactManifold.inverseFriction1Mass; float deltaLambda = -jv * contactManifold.inverseFriction1Mass;
//Log.warning(" contactManifold.frictionCoefficient=" + FMath.floatToString(contactManifold.frictionCoefficient)); //LOGGER.warn(" contactManifold.frictionCoefficient=" + FMath.floatToString(contactManifold.frictionCoefficient));
//Log.warning(" sumPenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse)); //LOGGER.warn(" sumPenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse));
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; float frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
float lambdaTemp = contactManifold.friction1Impulse; float lambdaTemp = contactManifold.friction1Impulse;
//Log.warning(" frictionLimit=" + FMath.floatToString(frictionLimit)); //LOGGER.warn(" frictionLimit=" + FMath.floatToString(frictionLimit));
//Log.warning(" lambdaTemp=" + FMath.floatToString(lambdaTemp)); //LOGGER.warn(" lambdaTemp=" + FMath.floatToString(lambdaTemp));
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
contactManifold.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); contactManifold.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction1Impulse + deltaLambda, frictionLimit));
deltaLambda = contactManifold.friction1Impulse - lambdaTemp; deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(deltaLambda); Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(deltaLambda);
Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiply(-1).multiply(deltaLambda); Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiply(-1).multiply(deltaLambda);
Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(deltaLambda); Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(deltaLambda);
Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.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); final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
//Log.warning(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1); //LOGGER.warn(" impulseFriction1.a1=" + impulseFriction1.angularImpulseBody1);
//Log.warning(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2); //LOGGER.warn(" impulseFriction1.a2=" + impulseFriction1.angularImpulseBody2);
//Log.warning(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1); //LOGGER.warn(" impulseFriction1.i1=" + impulseFriction1.linearImpulseBody1);
//Log.warning(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2); //LOGGER.warn(" impulseFriction1.i2=" + impulseFriction1.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction1, contactManifold); applyImpulse(impulseFriction1, contactManifold);
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
//Log.info(" w2=" + w2); //LOGGER.info(" w2=" + w2);
//Log.info(" v1=" + v1); //LOGGER.info(" v1=" + v1);
//Log.info(" w1=" + w1); //LOGGER.info(" w1=" + w1);
//Log.info(" contactManifold.r2Friction=" + contactManifold.r2Friction); //LOGGER.info(" contactManifold.r2Friction=" + contactManifold.r2Friction);
//Log.info(" contactManifold.r1Friction=" + contactManifold.r1Friction); //LOGGER.info(" contactManifold.r1Friction=" + contactManifold.r1Friction);
// ------ Second friction raint at the center of the contact manifol ----- // // ------ Second friction raint at the center of the contact manifol ----- //
// Compute J*v // Compute J*v
deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction)); 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); jv = deltaV.dot(contactManifold.frictionvec2);
//Log.warning(">>>> Jv=" + FMath.floatToString(Jv)); //LOGGER.warn(">>>> Jv=" + FMath.floatToString(Jv));
//Log.warning(">>>> contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass)); //LOGGER.warn(">>>> contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass));
// Compute the Lagrange multiplier lambda // Compute the Lagrange multiplier lambda
deltaLambda = -jv * contactManifold.inverseFriction2Mass; deltaLambda = -jv * contactManifold.inverseFriction2Mass;
//Log.warning(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(">>>> deltaLambda=" + FMath.floatToString(deltaLambda));
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
lambdaTemp = contactManifold.friction2Impulse; lambdaTemp = contactManifold.friction2Impulse;
//Log.warning(">>>> lambdaTemp=" + FMath.floatToString(lambdaTemp)); //LOGGER.warn(">>>> lambdaTemp=" + FMath.floatToString(lambdaTemp));
//Log.warning(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); //LOGGER.warn(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse));
//Log.warning(">>>>** frictionLimit=" + FMath.floatToString(frictionLimit)); //LOGGER.warn(">>>>** frictionLimit=" + FMath.floatToString(frictionLimit));
contactManifold.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction2Impulse + deltaLambda, 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; deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
//Log.warning(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(">>>> deltaLambda=" + FMath.floatToString(deltaLambda));
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
linearImpulseBody1 = contactManifold.frictionvec2.multiply(-deltaLambda); linearImpulseBody1 = contactManifold.frictionvec2.multiply(-deltaLambda);
angularImpulseBody1 = contactManifold.r1CrossT2.multiply(-deltaLambda); angularImpulseBody1 = contactManifold.r1CrossT2.multiply(-deltaLambda);
@ -872,36 +872,36 @@ public class ContactSolver {
angularImpulseBody2 = contactManifold.r2CrossT2.multiply(deltaLambda); angularImpulseBody2 = contactManifold.r2CrossT2.multiply(deltaLambda);
final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
//Log.warning(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1); //LOGGER.warn(" impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1);
//Log.warning(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2); //LOGGER.warn(" impulseFriction2.a2=" + impulseFriction2.angularImpulseBody2);
//Log.warning(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1); //LOGGER.warn(" impulseFriction2.i1=" + impulseFriction2.linearImpulseBody1);
//Log.warning(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2); //LOGGER.warn(" impulseFriction2.i2=" + impulseFriction2.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction2, contactManifold); applyImpulse(impulseFriction2, contactManifold);
//Log.info(" v2=" + v2); //LOGGER.info(" v2=" + v2);
//Log.info(" w2=" + w2); //LOGGER.info(" w2=" + w2);
//Log.info(" v1=" + v1); //LOGGER.info(" v1=" + v1);
//Log.info(" w1=" + w1); //LOGGER.info(" w1=" + w1);
// ------ Twist friction raint at the center of the contact manifol ------ // // ------ Twist friction raint at the center of the contact manifol ------ //
// Compute J*v // Compute J*v
deltaV = w2.less(w1); deltaV = w2.less(w1);
//Log.warning(" deltaV=" + deltaV); //LOGGER.warn(" deltaV=" + deltaV);
//Log.warning(" contactManifold.normal=" + contactManifold.normal); //LOGGER.warn(" contactManifold.normal=" + contactManifold.normal);
jv = deltaV.dot(contactManifold.normal); jv = deltaV.dot(contactManifold.normal);
//Log.warning(" Jv=" + FMath.floatToString(Jv)); //LOGGER.warn(" Jv=" + FMath.floatToString(Jv));
//Log.warning(" contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass); //LOGGER.warn(" contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass);
deltaLambda = -jv * (contactManifold.inverseTwistFrictionMass); deltaLambda = -jv * (contactManifold.inverseTwistFrictionMass);
//Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); //LOGGER.warn(" deltaLambda=" + FMath.floatToString(deltaLambda));
frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
//Log.warning(" contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient); //LOGGER.warn(" contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient);
//Log.warning(" sumPenetrationImpulse=" + sumPenetrationImpulse); //LOGGER.warn(" sumPenetrationImpulse=" + sumPenetrationImpulse);
//Log.warning(" frictionLimit=" + FMath.floatToString(frictionLimit)); //LOGGER.warn(" frictionLimit=" + FMath.floatToString(frictionLimit));
lambdaTemp = contactManifold.frictionTwistImpulse; lambdaTemp = contactManifold.frictionTwistImpulse;
//Log.warning(" lambdaTemp=" + lambdaTemp); //LOGGER.warn(" lambdaTemp=" + lambdaTemp);
contactManifold.frictionTwistImpulse = FMath.max(-frictionLimit, FMath.min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); 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; deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
//Log.warning(" deltaLambda=" + deltaLambda); //LOGGER.warn(" deltaLambda=" + deltaLambda);
// Compute the impulse P=J^T * lambda // Compute the impulse P=J^T * lambda
linearImpulseBody1 = Vector3f.ZERO; linearImpulseBody1 = Vector3f.ZERO;
angularImpulseBody1 = contactManifold.normal.multiply(-deltaLambda); angularImpulseBody1 = contactManifold.normal.multiply(-deltaLambda);
@ -909,10 +909,10 @@ public class ContactSolver {
angularImpulseBody2 = contactManifold.normal.multiply(deltaLambda); angularImpulseBody2 = contactManifold.normal.multiply(deltaLambda);
final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
//Log.warning(" impulseTwistFriction.a1=" + impulseTwistFriction.angularImpulseBody1); //LOGGER.warn(" impulseTwistFriction.a1=" + impulseTwistFriction.angularImpulseBody1);
//Log.warning(" impulseTwistFriction.a2=" + impulseTwistFriction.angularImpulseBody2); //LOGGER.warn(" impulseTwistFriction.a2=" + impulseTwistFriction.angularImpulseBody2);
//Log.warning(" impulseTwistFriction.i1=" + impulseTwistFriction.linearImpulseBody1); //LOGGER.warn(" impulseTwistFriction.i1=" + impulseTwistFriction.linearImpulseBody1);
//Log.warning(" impulseTwistFriction.i2=" + impulseTwistFriction.linearImpulseBody2); //LOGGER.warn(" impulseTwistFriction.i2=" + impulseTwistFriction.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseTwistFriction, contactManifold); applyImpulse(impulseTwistFriction, contactManifold);
// --------- Rolling resistance raint at the center of the contact manifold --------- // // --------- Rolling resistance raint at the center of the contact manifold --------- //
@ -930,10 +930,10 @@ public class ContactSolver {
angularImpulseBody2 = deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling;
final Impulse impulseRolling = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2); final Impulse impulseRolling = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2);
//Log.warning(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1); //LOGGER.warn(" impulseRolling.a1=" + impulseRolling.angularImpulseBody1);
//Log.warning(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2); //LOGGER.warn(" impulseRolling.a2=" + impulseRolling.angularImpulseBody2);
//Log.warning(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1); //LOGGER.warn(" impulseRolling.i1=" + impulseRolling.linearImpulseBody1);
//Log.warning(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2); //LOGGER.warn(" impulseRolling.i2=" + impulseRolling.linearImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseRolling, contactManifold); applyImpulse(impulseRolling, contactManifold);
} }
@ -1036,11 +1036,11 @@ public class ContactSolver {
final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiply(contactManifold.friction1Impulse) final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiply(contactManifold.friction1Impulse)
.add(contactManifold.oldFrictionvec2.multiply(contactManifold.friction2Impulse)); .add(contactManifold.oldFrictionvec2.multiply(contactManifold.friction2Impulse));
//Log.error("]]] oldFrictionImpulse=" + oldFrictionImpulse); //LOGGER.error("]]] oldFrictionImpulse=" + oldFrictionImpulse);
contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1); contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2); contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2);
//Log.error("]]] contactManifold.friction1Impulse=" + contactManifold.friction1Impulse); //LOGGER.error("]]] contactManifold.friction1Impulse=" + contactManifold.friction1Impulse);
//Log.error("]]] contactManifold.friction2Impulse=" + contactManifold.friction2Impulse); //LOGGER.error("]]] contactManifold.friction2Impulse=" + contactManifold.friction2Impulse);
// ------ First friction raint at the center of the contact manifold ------ // // ------ First friction raint at the center of the contact manifold ------ //
// Compute the impulse P = J^T * lambda // Compute the impulse P = J^T * lambda
Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(contactManifold.friction1Impulse); Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(contactManifold.friction1Impulse);
@ -1048,10 +1048,10 @@ public class ContactSolver {
Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(contactManifold.friction1Impulse); Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(contactManifold.friction1Impulse);
Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(contactManifold.friction1Impulse); Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(contactManifold.friction1Impulse);
//Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1);
//Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1);
//Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2);
//Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2);
final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction1, contactManifold); applyImpulse(impulseFriction1, contactManifold);
@ -1062,10 +1062,10 @@ public class ContactSolver {
linearImpulseBody2 = contactManifold.frictionvec2.multiply(contactManifold.friction2Impulse); linearImpulseBody2 = contactManifold.frictionvec2.multiply(contactManifold.friction2Impulse);
angularImpulseBody2 = contactManifold.r2CrossT2.multiply(contactManifold.friction2Impulse); angularImpulseBody2 = contactManifold.r2CrossT2.multiply(contactManifold.friction2Impulse);
//Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1);
//Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1);
//Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2);
//Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2);
final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseFriction2, contactManifold); applyImpulse(impulseFriction2, contactManifold);
@ -1076,10 +1076,10 @@ public class ContactSolver {
linearImpulseBody2 = Vector3f.ZERO; linearImpulseBody2 = Vector3f.ZERO;
angularImpulseBody2 = contactManifold.normal.multiply(contactManifold.frictionTwistImpulse); angularImpulseBody2 = contactManifold.normal.multiply(contactManifold.frictionTwistImpulse);
//Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); //LOGGER.error("]]] linearImpulseBody1=" + linearImpulseBody1);
//Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1);
//Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); //LOGGER.error("]]] linearImpulseBody2=" + linearImpulseBody2);
//Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2);
final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseTwistFriction, contactManifold); applyImpulse(impulseTwistFriction, contactManifold);
@ -1088,8 +1088,8 @@ public class ContactSolver {
angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiply(-1); angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiply(-1);
angularImpulseBody2 = contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
//Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); //LOGGER.error("]]] angularImpulseBody1=" + angularImpulseBody1);
//Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); //LOGGER.error("]]] angularImpulseBody2=" + angularImpulseBody2);
final Impulse impulseRollingResistance = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2); final Impulse impulseRollingResistance = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2);
// Apply the impulses to the bodies of the raint // Apply the impulses to the bodies of the raint
applyImpulse(impulseRollingResistance, contactManifold); applyImpulse(impulseRollingResistance, contactManifold);

326
src/org/atriasoft/ephysics/engine/DynamicsWorld.java
View File

@ -27,12 +27,13 @@ import org.atriasoft.ephysics.constraint.JointListElement;
import org.atriasoft.ephysics.constraint.JointsPositionCorrectionTechnique; import org.atriasoft.ephysics.constraint.JointsPositionCorrectionTechnique;
import org.atriasoft.ephysics.constraint.SliderJoint; import org.atriasoft.ephysics.constraint.SliderJoint;
import org.atriasoft.ephysics.constraint.SliderJointInfo; import org.atriasoft.ephysics.constraint.SliderJointInfo;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.Set; import org.atriasoft.ephysics.mathematics.Set;
import org.atriasoft.etk.math.FMath; import org.atriasoft.etk.math.FMath;
import org.atriasoft.etk.math.Quaternion; import org.atriasoft.etk.math.Quaternion;
import org.atriasoft.etk.math.Transform3D; import org.atriasoft.etk.math.Transform3D;
import org.atriasoft.etk.math.Vector3f; import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/** /**
* This class represents a dynamics world. This class inherits from * 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. * and their movements are simulated using the laws of physics.
*/ */
public class DynamicsWorld extends CollisionWorld { public class DynamicsWorld extends CollisionWorld {
static final Logger LOGGER = LoggerFactory.getLogger(DynamicsWorld.class);
private static int kkk = 0; private static int kkk = 0;
protected ContactSolver contactSolver; //!< Contact solver protected ContactSolver contactSolver; //!< Contact solver
protected Vector3f gravity = Vector3f.ZERO; //!< Gravity vector of the world 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 sleepAngularVelocity; //!< Sleep angular velocity threshold
protected float sleepLinearVelocity; //!< Sleep linear velocity threshold protected float sleepLinearVelocity; //!< Sleep linear velocity threshold
protected Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) 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 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. 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) public float timeStep; //!< Current frame time step (in seconds)
/** /**
* Constructor * Constructor
* @param gravity Gravity vector in the world (in meters per second squared) * @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.sleepAngularVelocity = Configuration.DEFAULT_SLEEP_ANGULAR_VELOCITY;
this.timeBeforeSleep = Configuration.DEFAULT_TIME_BEFORE_SLEEP; this.timeBeforeSleep = Configuration.DEFAULT_TIME_BEFORE_SLEEP;
} }
/** /**
* Add the joint to the list of joints of the two bodies involved in the joint * Add the joint to the list of joints of the two bodies involved in the joint
* @param joint Joint to add at the body. * @param joint Joint to add at the body.
*/ */
protected void addJointToBody(final Joint joint) { protected void addJointToBody(final Joint joint) {
if (joint == null) { if (joint == null) {
//Log.warning("Request add null joint"); //LOGGER.warn("Request add null joint");
return; return;
} }
// Add the joint at the beginning of the linked list of joints of the first body // 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 // 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); joint.getBody2().jointsList = new JointListElement(joint, joint.getBody2().jointsList);
} }
/** /**
* Compute the islands of awake bodies. * Compute the islands of awake bodies.
* An island is an isolated group of rigid bodies that have raints (joints or contacts) * 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++) { for (int iii = 0; iii < nbBodies; iii++) {
stackBodiesToVisit.add(null); stackBodiesToVisit.add(null);
} }
// For each rigid body of the world // For each rigid body of the world
for (final RigidBody body : this.rigidBodies) { for (final RigidBody body : this.rigidBodies) {
// If the body has already been added to an island, we go to the next body // 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 // For each contact manifold in which the current body is involded
ContactManifoldListElement contactElement; 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; final ContactManifold contactManifold = contactElement.contactManifold;
assert (contactManifold.getNbContactPoints() > 0); assert (contactManifold.getNbContactPoints() > 0);
// Check if the current contact manifold has already been added into an island // 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(); 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 * 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 * @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 the pointer to the created joint
return newJoint; return newJoint;
} }
/** /**
* Create a rigid body into the physics world * Create a rigid body into the physics world
* @param transform Transform3Dation from body local-space to world-space * @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 the pointer to the rigid body
return rigidBody; return rigidBody;
} }
/** /**
* Destroy a joint * Destroy a joint
* @param joint Pointer to the joint you want to destroy * @param joint Pointer to the joint you want to destroy
*/ */
public void destroyJoint(Joint joint) { public void destroyJoint(Joint joint) {
if (joint == null) { if (joint == null) {
//Log.warning("Request destroy null joint"); //LOGGER.warn("Request destroy null joint");
return; return;
} }
// If the collision between the two bodies of the raint was disabled // 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.getBody2().removeJointFromjointsList(joint);
joint = null; joint = null;
} }
/** /**
* Destroy a rigid body and all the joints which it beints * Destroy a rigid body and all the joints which it beints
* @param rigidBody Pointer to the body you want to destroy * @param rigidBody Pointer to the body you want to destroy
@ -323,7 +326,7 @@ public class DynamicsWorld extends CollisionWorld {
this.rigidBodies.remove(rigidBody); this.rigidBodies.remove(rigidBody);
rigidBody = null; rigidBody = null;
} }
/** /**
* Enable/Disable the sleeping technique. * Enable/Disable the sleeping technique.
* The sleeping technique is used to put bodies that are not moving into sleep * 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. * Get list of all contacts.
* @return The list of all contacts of the world * @return The list of all contacts of the world
@ -360,7 +363,7 @@ public class DynamicsWorld extends CollisionWorld {
// Return all the contact manifold // Return all the contact manifold
return contactManifolds; return contactManifolds;
} }
/** /**
* Get the gravity vector of the world * Get the gravity vector of the world
* @return The current gravity vector (in meter per seconds squared) * @return The current gravity vector (in meter per seconds squared)
@ -368,18 +371,18 @@ public class DynamicsWorld extends CollisionWorld {
public Vector3f getGravity() { public Vector3f getGravity() {
return this.gravity; return this.gravity;
} }
public List<Island> getIslands() { public List<Island> getIslands() {
return this.islands; return this.islands;
} }
/** /**
* Get the number of iterations for the position raint solver * Get the number of iterations for the position raint solver
*/ */
public int getNbIterationsPositionSolver() { public int getNbIterationsPositionSolver() {
return this.nbPositionSolverIterations; return this.nbPositionSolverIterations;
} }
/** /**
* Get the number of iterations for the velocity raint solver * Get the number of iterations for the velocity raint solver
* @return Number if iteration. * @return Number if iteration.
@ -387,7 +390,7 @@ public class DynamicsWorld extends CollisionWorld {
public int getNbIterationsVelocitySolver() { public int getNbIterationsVelocitySolver() {
return this.nbVelocitySolverIterations; return this.nbVelocitySolverIterations;
} }
/** /**
* Get the number of all joints * Get the number of all joints
* @return Number of joints in the world * @return Number of joints in the world
@ -395,7 +398,7 @@ public class DynamicsWorld extends CollisionWorld {
public int getNbJoints() { public int getNbJoints() {
return this.joints.size(); return this.joints.size();
} }
/** /**
* Get the number of rigid bodies in the world * Get the number of rigid bodies in the world
* @return 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() { public int getNbRigidBodies() {
return this.rigidBodies.size(); return this.rigidBodies.size();
} }
/** /**
* Get an iterator to the beginning of the bodies of the physics world * Get an iterator to the beginning of the bodies of the physics world
* @return Starting iterator of the set of rigid bodies * @return Starting iterator of the set of rigid bodies
@ -411,7 +414,7 @@ public class DynamicsWorld extends CollisionWorld {
public List<RigidBody> getRigidBodies() { public List<RigidBody> getRigidBodies() {
return this.rigidBodies; return this.rigidBodies;
} }
/** /**
* Return the current sleep angular velocity * Return the current sleep angular velocity
* @return The sleep angular velocity (in radian per second) * @return The sleep angular velocity (in radian per second)
@ -419,7 +422,7 @@ public class DynamicsWorld extends CollisionWorld {
public float getSleepAngularVelocity() { public float getSleepAngularVelocity() {
return this.sleepAngularVelocity; return this.sleepAngularVelocity;
} }
/** /**
* Get the sleep linear velocity * Get the sleep linear velocity
* @return The current sleep linear velocity (in meters per second) * @return The current sleep linear velocity (in meters per second)
@ -427,7 +430,7 @@ public class DynamicsWorld extends CollisionWorld {
public float getSleepLinearVelocity() { public float getSleepLinearVelocity() {
return this.sleepLinearVelocity; return this.sleepLinearVelocity;
} }
/** /**
* Get the time a body is required to stay still before sleeping * 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) * @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() { public float getTimeBeforeSleep() {
return this.timeBeforeSleep; return this.timeBeforeSleep;
} }
/** /**
* Initialize the bodies velocities arrays for the next simulation step. * Initialize the bodies velocities arrays for the next simulation step.
*/ */
@ -479,25 +482,25 @@ public class DynamicsWorld extends CollisionWorld {
indexBody++; indexBody++;
} }
} }
/** /**
* Integrate position and orientation of the rigid bodies. * Integrate position and orientation of the rigid bodies.
* The positions and orientations of the bodies are integrated using * The positions and orientations of the bodies are integrated using
* the sympletic Euler time stepping scheme. * the sympletic Euler time stepping scheme.
*/ */
protected void integrateRigidBodiesPositions() { protected void integrateRigidBodiesPositions() {
//Log.error("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++integrateRigidBodiesPositions"); //LOGGER.error("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++integrateRigidBodiesPositions");
// For each island of the world // For each island of the world
for (int i = 0; i < this.islands.size(); i++) { for (final Island element : this.islands) {
final List<RigidBody> bodies = this.islands.get(i).getBodies(); final List<RigidBody> bodies = element.getBodies();
// For each body of the island // For each body of the island
for (int b = 0; b < bodies.size(); b++) { for (final RigidBody element2 : bodies) {
//Log.error(" [" + b + "/" + bodies.size() + "]"); //LOGGER.error(" [" + b + "/" + bodies.size() + "]");
// Get the rained velocity // 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 newLinVelocity = this.rainedLinearVelocities[indexArray];
Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray]; Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray];
//Log.error(" newAngVelocity = " + newAngVelocity); //LOGGER.error(" newAngVelocity = " + newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used // Add the split impulse velocity from Contact Solver (only used
// to update the position) // to update the position)
if (this.contactSolver.isSplitImpulseActive()) { if (this.contactSolver.isSplitImpulseActive()) {
@ -505,32 +508,35 @@ public class DynamicsWorld extends CollisionWorld {
newAngVelocity = newAngVelocity.add(this.splitAngularVelocities[indexArray]); newAngVelocity = newAngVelocity.add(this.splitAngularVelocities[indexArray]);
} }
// Get current position and orientation of the body // Get current position and orientation of the body
final Vector3f currentPosition = bodies.get(b).centerOfMassWorld; final Vector3f currentPosition = element2.centerOfMassWorld;
//Log.error(" bodies.get(b).getTransform() = " + bodies.get(b).getTransform()); //LOGGER.error(" bodies.get(b).getTransform() = " + bodies.get(b).getTransform());
final Quaternion currentOrientation = bodies.get(b).getTransform().getOrientation(); final Quaternion currentOrientation = element2.getTransform().getOrientation();
// Update the new rained position and orientation of the body // Update the new rained position and orientation of the body
this.rainedPositions[indexArray] = newLinVelocity.multiply(this.timeStep).add(currentPosition); this.rainedPositions[indexArray] = newLinVelocity.multiply(this.timeStep).add(currentPosition);
//Log.error(" currentOrientation = " + currentOrientation); //LOGGER.error(" currentOrientation = " + currentOrientation);
//Log.error(" newAngVelocity = " + newAngVelocity); //LOGGER.error(" newAngVelocity = " + newAngVelocity);
//Log.error(" this.timeStep = " + FMath.floatToString(this.timeStep)); //LOGGER.error(" this.timeStep = " + FMath.floatToString(this.timeStep));
this.rainedOrientations[indexArray] = currentOrientation.add(new Quaternion(0, newAngVelocity).multiply(currentOrientation).multiply(0.5f * this.timeStep)); this.rainedOrientations[indexArray] = currentOrientation.add(
//Log.error(" this.rainedPositions[indexArray] = " + this.rainedPositions[indexArray]); new Quaternion(0, newAngVelocity).multiply(currentOrientation).multiply(0.5f * this.timeStep));
//Log.error(" this.rainedOrientations[indexArray] = " + this.rainedOrientations[indexArray]); //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++) { for (int iii = 1; iii < this.rainedPositions.length; iii++) {
Log.error(DynamicsWorld.kkk + " this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]); LOGGER.error(
Log.error(DynamicsWorld.kkk + " this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]); DynamicsWorld.kkk + " this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]);
LOGGER.error(
DynamicsWorld.kkk + " this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]);
} }
//Log.error("------------------------------------------------------------------------------------------------"); //LOGGER.error("------------------------------------------------------------------------------------------------");
//Log.error("------------------------------------------------------------------------------------------------"); //LOGGER.error("------------------------------------------------------------------------------------------------");
//Log.error("------------------------------------------------------------------------------------------------"); //LOGGER.error("------------------------------------------------------------------------------------------------");
//Log.error("------------------------------------------------------------------------------------------------"); //LOGGER.error("------------------------------------------------------------------------------------------------");
if (DynamicsWorld.kkk++ == 98) { if (DynamicsWorld.kkk++ == 98) {
//System.exit(-1); //System.exit(-1);
} }
} }
/** /**
* Integrate the velocities of rigid bodies. * Integrate the velocities of rigid bodies.
* This method only set the temporary velocities but does not update * This method only set the temporary velocities but does not update
@ -541,33 +547,32 @@ public class DynamicsWorld extends CollisionWorld {
protected void integrateRigidBodiesVelocities() { protected void integrateRigidBodiesVelocities() {
// Initialize the bodies velocity arrays // Initialize the bodies velocity arrays
initVelocityArrays(); initVelocityArrays();
//Log.info("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"); //LOGGER.info("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@");
// For each island of the world // For each island of the world
for (int iii = 0; iii < this.islands.size(); iii++) { for (final Island element : this.islands) {
//Log.info("Manage island : " + iii + "/" + this.islands.size()); //LOGGER.info("Manage island : " + iii + "/" + this.islands.size());
final List<RigidBody> bodies = this.islands.get(iii).getBodies(); final List<RigidBody> bodies = element.getBodies();
// For each body of the island // For each body of the island
for (int bbb = 0; bbb < bodies.size(); bbb++) { for (final RigidBody tmpval : bodies) {
//Log.info(" body : " + bbb + "/" + bodies.size());
// Insert the body into the map of rained velocities
final RigidBody tmpval = bodies.get(bbb);
final int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval); final int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval);
//Log.info(" indexBody=" + indexBody); //LOGGER.info(" indexBody=" + indexBody);
assert (this.splitLinearVelocities[indexBody].isZero()); assert (this.splitLinearVelocities[indexBody].isZero());
assert (this.splitAngularVelocities[indexBody].isZero()); assert (this.splitAngularVelocities[indexBody].isZero());
// Integrate the external force to get the new velocity of the body // 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)); this.rainedLinearVelocities[indexBody] = tmpval.getLinearVelocity()
//Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); .add(tmpval.externalForce.multiply(this.timeStep * tmpval.massInverse));
this.rainedAngularVelocities[indexBody] = bodies.get(bbb).getAngularVelocity() //LOGGER.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]);
.add(bodies.get(bbb).getInertiaTensorInverseWorld().multiply(bodies.get(bbb).externalTorque.multiply(this.timeStep))); this.rainedAngularVelocities[indexBody] = tmpval.getAngularVelocity().add(
//Log.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); 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 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 // 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 // Apply the velocity damping
// Damping force : Fc = -c' * v (c=damping factor) // Damping force : Fc = -c' * v (c=damping factor)
// Equation : m * dv/dt = -c' * v // 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! + ... // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ...
// => e^(-x) ~ 1 - x // => e^(-x) ~ 1 - x
// => v2 = v1 * (1 - c * dt) // => v2 = v1 * (1 - c * dt)
final float linDampingFactor = bodies.get(bbb).getLinearDamping(); final float linDampingFactor = tmpval.getLinearDamping();
//Log.info(" linDampingFactor=" + FMath.floatToString(linDampingFactor)); //LOGGER.info(" linDampingFactor=" + FMath.floatToString(linDampingFactor));
final float angDampingFactor = bodies.get(bbb).getAngularDamping(); final float angDampingFactor = tmpval.getAngularDamping();
//Log.info(" angDampingFactor=" + FMath.floatToString(angDampingFactor)); //LOGGER.info(" angDampingFactor=" + FMath.floatToString(angDampingFactor));
final float linearDamping = FMath.pow(1.0f - linDampingFactor, this.timeStep); 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); 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); this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody].multiply(linearDamping);
//Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); //LOGGER.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]);
this.rainedAngularVelocities[indexBody] = this.rainedAngularVelocities[indexBody].multiply(angularDamping); this.rainedAngularVelocities[indexBody] = this.rainedAngularVelocities[indexBody]
//Log.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); .multiply(angularDamping);
//LOGGER.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]);
} }
} }
} }
/** /**
* Get if the gravity is enaled * Get if the gravity is enaled
* @return True if the gravity is enabled in the world * @return True if the gravity is enabled in the world
@ -604,7 +610,7 @@ public class DynamicsWorld extends CollisionWorld {
public boolean isGravityEnabled() { public boolean isGravityEnabled() {
return this.isGravityEnabled; return this.isGravityEnabled;
} }
/** /**
* Get if the sleeping technique is enabled * Get if the sleeping technique is enabled
* @return True if the sleeping technique is enabled and false otherwise * @return True if the sleeping technique is enabled and false otherwise
@ -612,7 +618,7 @@ public class DynamicsWorld extends CollisionWorld {
public boolean isSleepingEnabled() { public boolean isSleepingEnabled() {
return this.isSleepingEnabled; return this.isSleepingEnabled;
} }
/** /**
* Reset the external force and torque applied to the bodies * Reset the external force and torque applied to the bodies
*/ */
@ -623,7 +629,7 @@ public class DynamicsWorld extends CollisionWorld {
obj.externalTorque = Vector3f.ZERO; obj.externalTorque = Vector3f.ZERO;
}); });
} }
/** /**
* Set the position correction technique used for contacts * Set the position correction technique used for contacts
* @param technique Technique used for the position correction (Baumgarte or Split Impulses) * @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); this.contactSolver.setIsSplitImpulseActive(true);
} }
} }
/** /**
* Set an event listener object to receive events callbacks. * Set an event listener object to receive events callbacks.
* @note If you use null as an argument, the events callbacks will be disabled. * @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) { public void setEventListener(final EventListener eventListener) {
this.eventListener = eventListener; this.eventListener = eventListener;
} }
/** /**
* Set the gravity vector of the world * Set the gravity vector of the world
* @param gravity The gravity vector (in meter per seconds squared) * @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) { public void setGravity(final Vector3f gravity) {
this.gravity = gravity; this.gravity = gravity;
} }
/** /**
* Enable/Disable the gravity * Enable/Disable the gravity
* @param isGravityEnabled True if you want to enable the gravity in the world * @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) { public void setIsGratityEnabled(final boolean isGravityEnabled) {
this.isGravityEnabled = isGravityEnabled; this.isGravityEnabled = isGravityEnabled;
} }
/** /**
* Activate or deactivate the solving of friction raints at the center of the contact * Activate or deactivate the solving of friction raints at the center of the contact
* manifold instead of solving them at each contact point * manifold instead of solving them at each contact point
@ -672,7 +678,7 @@ public class DynamicsWorld extends CollisionWorld {
public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean isActive) { public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean isActive) {
this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
} }
/** /**
* Set the position correction technique used for joints * Set the position correction technique used for joints
* @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) * @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); this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
} }
} }
/** /**
* Set the number of iterations for the position raint solver * Set the number of iterations for the position raint solver
* @param nbIterations Number of iterations for the position 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) { public void setNbIterationsPositionSolver(final int nbIterations) {
this.nbPositionSolverIterations = nbIterations; this.nbPositionSolverIterations = nbIterations;
} }
/** /**
* Set the number of iterations for the velocity raint solver * Set the number of iterations for the velocity raint solver
* @param nbIterations Number of iterations for the velocity 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) { public void setNbIterationsVelocitySolver(final int nbIterations) {
this.nbVelocitySolverIterations = nbIterations; this.nbVelocitySolverIterations = nbIterations;
} }
/** /**
* Set the sleep angular velocity. * Set the sleep angular velocity.
* When the velocity of a body becomes smaller than the sleep linear/angular * 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) { public void setSleepAngularVelocity(final float sleepAngularVelocity) {
if (sleepAngularVelocity < 0.0f) { if (sleepAngularVelocity < 0.0f) {
//Log.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 "); //LOGGER.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 ");
return; return;
} }
this.sleepAngularVelocity = sleepAngularVelocity; this.sleepAngularVelocity = sleepAngularVelocity;
} }
/** /**
* Set the sleep linear velocity * Set the sleep linear velocity
* @param sleepLinearVelocity The new sleep linear velocity (in meters per second) * @param sleepLinearVelocity The new sleep linear velocity (in meters per second)
*/ */
public void setSleepLinearVelocity(final float sleepLinearVelocity) { public void setSleepLinearVelocity(final float sleepLinearVelocity) {
if (sleepLinearVelocity < 0.0f) { if (sleepLinearVelocity < 0.0f) {
//Log.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 "); //LOGGER.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 ");
return; return;
} }
this.sleepLinearVelocity = sleepLinearVelocity; this.sleepLinearVelocity = sleepLinearVelocity;
} }
/** /**
* Set the time a body is required to stay still before sleeping * 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) * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds)
*/ */
public void setTimeBeforeSleep(final float timeBeforeSleep) { public void setTimeBeforeSleep(final float timeBeforeSleep) {
if (timeBeforeSleep < 0.0f) { if (timeBeforeSleep < 0.0f) {
//Log.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 "); //LOGGER.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 ");
return; return;
} }
this.timeBeforeSleep = timeBeforeSleep; this.timeBeforeSleep = timeBeforeSleep;
} }
/** /**
* Solve the contacts and raints * Solve the contacts and raints
*/ */
@ -749,10 +755,10 @@ public class DynamicsWorld extends CollisionWorld {
this.contactSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); this.contactSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities);
this.raintSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); this.raintSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities);
this.raintSolver.setConstrainedPositionsArrays(this.rainedPositions, this.rainedOrientations); this.raintSolver.setConstrainedPositionsArrays(this.rainedPositions, this.rainedOrientations);
//Log.info(")))))))))))))))"); //LOGGER.info(")))))))))))))))");
for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { for (final Vector3f element : this.rainedAngularVelocities) {
//Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); //LOGGER.info(" " + iii + " : " + this.rainedAngularVelocities[iii]);
} }
// ---------- Solve velocity raints for joints and contacts ---------- // // ---------- Solve velocity raints for joints and contacts ---------- //
final int idIsland = 0; final int idIsland = 0;
@ -761,7 +767,7 @@ public class DynamicsWorld extends CollisionWorld {
// Check if there are contacts and raints to solve // Check if there are contacts and raints to solve
final boolean isConstraintsToSolve = island.getNbJoints() > 0; final boolean isConstraintsToSolve = island.getNbJoints() > 0;
final boolean isContactsToSolve = island.getNbContactManifolds() > 0; final boolean isContactsToSolve = island.getNbContactManifolds() > 0;
//Log.info("solveContactsAndConstraints : " + idIsland + " " + isConstraintsToSolve + " " + isContactsToSolve); //LOGGER.info("solveContactsAndConstraints : " + idIsland + " " + isConstraintsToSolve + " " + isContactsToSolve);
if (!isConstraintsToSolve && !isContactsToSolve) { if (!isConstraintsToSolve && !isContactsToSolve) {
continue; continue;
} }
@ -795,12 +801,12 @@ public class DynamicsWorld extends CollisionWorld {
this.contactSolver.cleanup(); this.contactSolver.cleanup();
} }
} }
//Log.info("(((((((((((((("); //LOGGER.info("((((((((((((((");
for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { for (final Vector3f element : this.rainedAngularVelocities) {
//Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); //LOGGER.info(" " + iii + " : " + this.rainedAngularVelocities[iii]);
} }
} }
/** /**
* Solve the position error correction of the raints * Solve the position error correction of the raints
*/ */
@ -810,16 +816,16 @@ public class DynamicsWorld extends CollisionWorld {
return; return;
} }
// For each island of the world // 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 ---------- // // ---------- Solve the position error correction for the raints ---------- //
// For each iteration of the position (error correction) solver // For each iteration of the position (error correction) solver
for (int i = 0; i < this.nbPositionSolverIterations; i++) { for (int i = 0; i < this.nbPositionSolverIterations; i++) {
// Solve the position raints // Solve the position raints
this.raintSolver.solvePositionConstraints(this.islands.get(islandIndex)); this.raintSolver.solvePositionConstraints(element);
} }
} }
} }
@Override @Override
public void testCollision(final CollisionBody body1, final CollisionBody body2, final CollisionCallback callback) { public void testCollision(final CollisionBody body1, final CollisionBody body2, final CollisionCallback callback) {
// Create the sets of shapes // Create the sets of shapes
@ -834,7 +840,7 @@ public class DynamicsWorld extends CollisionWorld {
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
} }
@Override @Override
public void testCollision(final CollisionBody body, final CollisionCallback callback) { public void testCollision(final CollisionBody body, final CollisionCallback callback) {
// Create the sets of shapes // Create the sets of shapes
@ -847,7 +853,7 @@ public class DynamicsWorld extends CollisionWorld {
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
} }
/// Test and report collisions between all shapes of the world /// Test and report collisions between all shapes of the world
@Override @Override
public void testCollision(final CollisionCallback callback) { public void testCollision(final CollisionCallback callback) {
@ -855,7 +861,7 @@ public class DynamicsWorld extends CollisionWorld {
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
} }
@Override @Override
public void testCollision(final ProxyShape shape, final CollisionCallback callback) { public void testCollision(final ProxyShape shape, final CollisionCallback callback) {
// Create the sets of shapes // Create the sets of shapes
@ -865,7 +871,7 @@ public class DynamicsWorld extends CollisionWorld {
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
} }
@Override @Override
public void testCollision(final ProxyShape shape1, final ProxyShape shape2, final CollisionCallback callback) { public void testCollision(final ProxyShape shape1, final ProxyShape shape2, final CollisionCallback callback) {
// Create the sets of shapes // Create the sets of shapes
@ -876,18 +882,18 @@ public class DynamicsWorld extends CollisionWorld {
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
} }
/** /**
* Update the physics simulation * Update the physics simulation
* @param timeStep The amount of time to step the simulation by (in seconds) * @param timeStep The amount of time to step the simulation by (in seconds)
*/ */
public void update(final float timeStep) { 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) { for (final CollisionBody elem : this.bodies) {
//Log.info(" " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info(" " + elem.getID() + " / " + elem.getAABB());
//Log.info(" " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info(" " + elem.getID() + " / " + elem.getTransform());
} }
this.timeStep = timeStep; this.timeStep = timeStep;
// Notify the event listener about the beginning of an internal tick // Notify the event listener about the beginning of an internal tick
if (this.eventListener != null) { 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) // Compute the islands (separate groups of bodies with raints between each others)
computeIslands(); computeIslands();
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("111 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("111 " + elem.getID() + " / " + elem.getAABB());
//Log.info("111 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("111 " + elem.getID() + " / " + elem.getTransform());
} }
// Integrate the velocities // Integrate the velocities
integrateRigidBodiesVelocities(); integrateRigidBodiesVelocities();
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("222 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("222 " + elem.getID() + " / " + elem.getAABB());
//Log.info("222 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("222 " + elem.getID() + " / " + elem.getTransform());
} }
// Solve the contacts and raints // Solve the contacts and raints
solveContactsAndConstraints(); solveContactsAndConstraints();
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("333 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("333 " + elem.getID() + " / " + elem.getAABB());
//Log.info("333 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("333 " + elem.getID() + " / " + elem.getTransform());
} }
// Integrate the position and orientation of each body // Integrate the position and orientation of each body
integrateRigidBodiesPositions(); integrateRigidBodiesPositions();
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("444 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("444 " + elem.getID() + " / " + elem.getAABB());
//Log.info("444 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("444 " + elem.getID() + " / " + elem.getTransform());
} }
// Solve the position correction for raints // Solve the position correction for raints
solvePositionCorrection(); solvePositionCorrection();
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("555 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("555 " + elem.getID() + " / " + elem.getAABB());
//Log.info("555 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("555 " + elem.getID() + " / " + elem.getTransform());
} }
// Update the state (positions and velocities) of the bodies // 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 ? 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) { for (final CollisionBody elem : this.bodies) {
//Log.info("--- " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("--- " + elem.getID() + " / " + elem.getAABB());
//Log.info("--- " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("--- " + elem.getID() + " / " + elem.getTransform());
} }
if (this.isSleepingEnabled) { if (this.isSleepingEnabled) {
updateSleepingBodies(); updateSleepingBodies();
} }
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("666 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("666 " + elem.getID() + " / " + elem.getAABB());
//Log.info("666 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("666 " + elem.getID() + " / " + elem.getTransform());
} }
// Notify the event listener about the end of an internal tick // Notify the event listener about the end of an internal tick
if (this.eventListener != null) { if (this.eventListener != null) {
this.eventListener.endInternalTick(); this.eventListener.endInternalTick();
} }
for (final CollisionBody elem : this.bodies) { for (final CollisionBody elem : this.bodies) {
//Log.info("777 " + elem.getID() + " / " + elem.getAABB()); //LOGGER.info("777 " + elem.getID() + " / " + elem.getAABB());
//Log.info("777 " + elem.getID() + " / " + elem.getTransform()); //LOGGER.info("777 " + elem.getID() + " / " + elem.getTransform());
} }
// Reset the external force and torque applied to the bodies // Reset the external force and torque applied to the bodies
resetBodiesForceAndTorque(); resetBodiesForceAndTorque();
//Log.error("<< ============= end compute "); //LOGGER.error("<< ============= end compute ");
} }
/** /**
* Update the postion/orientation of the bodies * Update the postion/orientation of the bodies
*/ */
protected void updateBodiesState() { protected void updateBodiesState() {
// For each island of the world // 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 // For each body of the island
final List<RigidBody> bodies = this.islands.get(islandIndex).getBodies(); final List<RigidBody> bodies = element.getBodies();
for (int b = 0; b < this.islands.get(islandIndex).getNbBodies(); b++) { for (int b = 0; b < element.getNbBodies(); b++) {
final int index = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b)); final int index = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b));
// Update the linear and angular velocity of the body // Update the linear and angular velocity of the body
bodies.get(b).linearVelocity = this.rainedLinearVelocities[index]; 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 // Update the position of the center of mass of the body
bodies.get(b).centerOfMassWorld = this.rainedPositions[index]; bodies.get(b).centerOfMassWorld = this.rainedPositions[index];
// Update the orientation of the body // Update the orientation of the body
//Log.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]); //LOGGER.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]);
//Log.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew()); //LOGGER.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew());
if (bodies.get(b).isAngularReactionEnable()) { 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) // Update the transform of the body (using the new center of mass and new orientation)
bodies.get(b).updateTransformWithCenterOfMass(); bodies.get(b).updateTransformWithCenterOfMass();
// Update the broad-phase state of the body // Update the broad-phase state of the body
//Log.info(" " + b + " ==> updateBroadPhaseState"); //LOGGER.info(" " + b + " ==> updateBroadPhaseState");
bodies.get(b).updateBroadPhaseState(); bodies.get(b).updateBroadPhaseState();
} }
} }
} }
/** /**
* Put bodies to sleep if needed. * Put bodies to sleep if needed.
* For each island, if all the bodies have been almost still for a int enough period of * 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 sleepLinearVelocitySquare = this.sleepLinearVelocity * this.sleepLinearVelocity;
final float sleepAngularVelocitySquare = this.sleepAngularVelocity * this.sleepAngularVelocity; final float sleepAngularVelocitySquare = this.sleepAngularVelocity * this.sleepAngularVelocity;
// For each island of the world // 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; float minSleepTime = Float.MAX_VALUE;
// For each body of the island // For each body of the island
final List<RigidBody> bodies = this.islands.get(i).getBodies(); final List<RigidBody> bodies = element.getBodies();
for (int b = 0; b < bodies.size(); b++) { for (final RigidBody element2 : bodies) {
// Skip static bodies // Skip static bodies
if (bodies.get(b).getType() == BodyType.STATIC) { if (element2.getType() == BodyType.STATIC) {
continue; continue;
} }
// If the body is velocity is large enough to stay awake // 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); LOGGER.error(" check if ready to sleep: linear = " + element2.getLinearVelocity().length2() + " > "
Log.error(" check if ready to sleep: angular = " + bodies.get(b).getAngularVelocity().length2() + " > " + sleepAngularVelocitySquare); + sleepLinearVelocitySquare);
if (bodies.get(b).getLinearVelocity().length2() > sleepLinearVelocitySquare || bodies.get(b).getAngularVelocity().length2() > sleepAngularVelocitySquare LOGGER.error(" check if ready to sleep: angular = " + element2.getAngularVelocity().length2() + " > "
|| !bodies.get(b).isAllowedToSleep()) { + sleepAngularVelocitySquare);
if (element2.getLinearVelocity().length2() > sleepLinearVelocitySquare
|| element2.getAngularVelocity().length2() > sleepAngularVelocitySquare
|| !element2.isAllowedToSleep()) {
// Reset the sleep time of the body // Reset the sleep time of the body
bodies.get(b).sleepTime = 0.0f; element2.sleepTime = 0.0f;
minSleepTime = 0.0f; minSleepTime = 0.0f;
} else { // If the body velocity is bellow the sleeping velocity threshold } else { // If the body velocity is bellow the sleeping velocity threshold
// Increase the sleep time // Increase the sleep time
bodies.get(b).sleepTime += this.timeStep; element2.sleepTime += this.timeStep;
if (bodies.get(b).sleepTime < minSleepTime) { if (element2.sleepTime < minSleepTime) {
minSleepTime = bodies.get(b).sleepTime; minSleepTime = element2.sleepTime;
} }
} }
} }
@ -1030,11 +1040,11 @@ public class DynamicsWorld extends CollisionWorld {
// the time required to become a sleeping body // the time required to become a sleeping body
if (minSleepTime >= this.timeBeforeSleep) { if (minSleepTime >= this.timeBeforeSleep) {
// Put all the bodies of the island to sleep // 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); bodies.get(b).setIsSleeping(true);
} }
} }
} }
} }
} }

48
src/org/atriasoft/ephysics/engine/Island.java
View File

@ -7,17 +7,19 @@ import org.atriasoft.ephysics.body.BodyType;
import org.atriasoft.ephysics.body.RigidBody; import org.atriasoft.ephysics.body.RigidBody;
import org.atriasoft.ephysics.collision.ContactManifold; import org.atriasoft.ephysics.collision.ContactManifold;
import org.atriasoft.ephysics.constraint.Joint; 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 * An island represent an isolated group of awake bodies that are connected with each other by
* some contraints (contacts or joints). * some contraints (contacts or joints).
*/ */
public class Island { public class Island {
static final Logger LOGGER = LoggerFactory.getLogger(Island.class);
private final List<RigidBody> bodies = new ArrayList<>(); //!< Array with all the bodies of the island private final List<RigidBody> bodies = new ArrayList<>(); //!< Array with all the bodies of the island
private final List<ContactManifold> contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island private final List<ContactManifold> contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island
private final List<Joint> joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island private final List<Joint> joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island
/** /**
* Constructor * Constructor
*/ */
@ -27,76 +29,76 @@ public class Island {
//this.contactManifolds.reserve(nbMaxContactManifolds); //this.contactManifolds.reserve(nbMaxContactManifolds);
//this.joints.reserve(nbMaxJoints); //this.joints.reserve(nbMaxJoints);
} }
/** /**
* Add a body. * Add a body.
*/ */
public void addBody(final RigidBody body) { public void addBody(final RigidBody body) {
if (body.isSleeping()) { if (body.isSleeping()) {
Log.error("Try to add a body that is sleeping ..."); LOGGER.error("Try to add a body that is sleeping ...");
return; return;
} }
this.bodies.add(body); this.bodies.add(body);
} }
/** /**
* Add a contact manifold. * Add a contact manifold.
*/ */
public void addContactManifold(final ContactManifold contactManifold) { public void addContactManifold(final ContactManifold contactManifold) {
this.contactManifolds.add(contactManifold); this.contactManifolds.add(contactManifold);
} }
/** /**
* Add a joint. * Add a joint.
*/ */
public void addJoint(final Joint joint) { public void addJoint(final Joint joint) {
this.joints.add(joint); this.joints.add(joint);
} }
/** /**
* Return a pointer to the array of bodies * Return a pointer to the array of bodies
*/ */
public List<RigidBody> getBodies() { public List<RigidBody> getBodies() {
return this.bodies; return this.bodies;
} }
/** /**
* Return a pointer to the array of contact manifolds * Return a pointer to the array of contact manifolds
*/ */
public List<ContactManifold> getContactManifold() { public List<ContactManifold> getContactManifold() {
return this.contactManifolds; return this.contactManifolds;
} }
/** /**
* Return a pointer to the array of joints * Return a pointer to the array of joints
*/ */
public List<Joint> getJoints() { public List<Joint> getJoints() {
return this.joints; return this.joints;
} }
/** /**
* Get the number of body * Get the number of body
* @return Number of bodies. * @return Number of bodies.
*/ */
public int getNbBodies() { public int getNbBodies() {
return this.bodies.size(); return this.bodies.size();
} }
/** /**
* @ Get the number of contact manifolds * @ Get the number of contact manifolds
* Return the number of contact manifolds in the island * Return the number of contact manifolds in the island
*/ */
public int getNbContactManifolds() { public int getNbContactManifolds() {
return this.contactManifolds.size(); return this.contactManifolds.size();
} }
/** /**
* Return the number of joints in the island * Return the number of joints in the island
*/ */
public int getNbJoints() { public int getNbJoints() {
return this.joints.size(); return this.joints.size();
} }
/** /**
* Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands * 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 {
} }
} }
} }
} }

68
src/org/atriasoft/ephysics/internal/Log.java
View File

@ -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() {}
}