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

36
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
@ -68,12 +70,14 @@ public class CollisionBody extends Body {
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);
} }
} }
@ -122,17 +126,18 @@ 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;
} }
@ -343,12 +348,14 @@ public class CollisionBody extends Body {
* @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();
@ -398,6 +405,7 @@ public class CollisionBody extends 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);
} }
} }

66
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,6 +45,7 @@ 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;
@ -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);
@ -201,12 +206,13 @@ 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;
} }
@ -229,7 +235,8 @@ 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());
} }
/** /**
@ -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,14 +337,16 @@ 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
@ -408,7 +418,8 @@ 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)));
} }
/** /**
@ -492,19 +503,22 @@ public class RigidBody extends CollisionBody {
*/ */
@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();
} }
@ -546,12 +560,12 @@ public class RigidBody extends CollisionBody {
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);
} }
} }
@ -561,12 +575,16 @@ public class RigidBody extends CollisionBody {
*/ */
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);
} }
} }

110
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,7 +53,7 @@ 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;
@ -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);
@ -168,10 +173,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
/// 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();
} }
@ -181,11 +186,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
// 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());
} }
} }
@ -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,9 +236,11 @@ 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
@ -250,7 +254,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
} }
/// 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,11 +320,14 @@ 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());
final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(callback); 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);
@ -338,13 +352,16 @@ public class CollisionDetection implements NarrowPhaseCallback {
// 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];
} }
@ -365,7 +382,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
/// 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
@ -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();
@ -412,26 +430,29 @@ public class CollisionDetection implements NarrowPhaseCallback {
} }
/// 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,
final Set<DTree> shapes1,
final Set<DTree> shapes2) {
// For each possible collision pair of bodies for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
while (it.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
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,8 +463,10 @@ 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) {
@ -473,7 +496,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
} }
/// 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
@ -491,7 +517,11 @@ public class CollisionDetection implements NarrowPhaseCallback {
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);

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

@ -3,10 +3,12 @@ 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) {
@ -43,7 +45,7 @@ public class TrianglesStore {
/// 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) {
@ -52,7 +54,7 @@ public class TrianglesStore {
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)
@ -63,7 +65,7 @@ public class TrianglesStore {
/// 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());
} }

21
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;
@ -34,7 +37,8 @@ public class ConcaveMeshShape extends ConcaveShape {
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;
@ -69,7 +73,8 @@ public class ConcaveMeshShape extends ConcaveShape {
// 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();
@ -121,12 +126,15 @@ public class ConcaveMeshShape extends ConcaveShape {
/// 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);
@ -165,7 +173,8 @@ public class ConcaveMeshShape extends ConcaveShape {
@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.

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);

222
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
@ -89,7 +91,7 @@ public class DynamicsWorld extends CollisionWorld {
*/ */
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
@ -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
@ -284,7 +287,7 @@ public class DynamicsWorld extends CollisionWorld {
*/ */
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
@ -486,18 +489,18 @@ public class DynamicsWorld extends CollisionWorld {
* 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,27 +508,30 @@ 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);
} }
@ -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,18 +586,19 @@ 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]);
} }
} }
} }
@ -710,7 +716,7 @@ 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;
@ -722,7 +728,7 @@ public class DynamicsWorld extends CollisionWorld {
*/ */
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;
@ -734,7 +740,7 @@ public class DynamicsWorld extends CollisionWorld {
*/ */
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;
@ -750,9 +756,9 @@ public class DynamicsWorld extends CollisionWorld {
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,9 +801,9 @@ 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]);
} }
} }
@ -810,12 +816,12 @@ 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);
} }
} }
} }
@ -882,10 +888,10 @@ public class DynamicsWorld extends CollisionWorld {
* @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;
@ -904,59 +910,59 @@ 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 ");
} }
/** /**
@ -964,10 +970,10 @@ public class DynamicsWorld extends CollisionWorld {
*/ */
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,15 +983,16 @@ 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();
} }
} }
@ -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,7 +1040,7 @@ 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);
} }
} }

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

@ -7,13 +7,15 @@ 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
@ -33,7 +35,7 @@ public class Island {
*/ */
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);

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