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/Mathematics.java',
'src/org/atriasoft/ephysics/configuration/Defaults.java',
'src/org/atriasoft/ephysics/internal/Log.java',
'src/org/atriasoft/ephysics/internal/LOGGER.java',
'src/org/atriasoft/ephysics/engine/DynamicsWorld.java',
'src/org/atriasoft/ephysics/engine/CollisionWorld.java',
'src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java',

2
out/eclipse/.gitignore vendored
View File

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

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

@ -30,16 +30,18 @@ import org.atriasoft.ephysics.collision.ProxyShape;
import org.atriasoft.ephysics.collision.shapes.AABB;
import org.atriasoft.ephysics.collision.shapes.CollisionShape;
import org.atriasoft.ephysics.engine.CollisionWorld;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.ephysics.mathematics.Ray;
import org.atriasoft.etk.math.Transform3D;
import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
*
*/
public class CollisionBody extends Body {
static final Logger LOGGER = LoggerFactory.getLogger(CollisionBody.class);
// First element of the linked list of contact manifolds involving this body
public ContactManifoldListElement contactManifoldsList;
// Number of collision shapes
@ -52,7 +54,7 @@ public class CollisionBody extends Body {
protected BodyType type;
// Reference to the world the body belongs to
protected CollisionWorld world;
/**
* Constructor
* @param transform The transform of the body
@ -67,20 +69,22 @@ public class CollisionBody extends Body {
this.numberCollisionShapes = 0;
this.contactManifoldsList = null;
this.world = world;
//Log.debug(" set transform: " + transform);
//LOGGER.debug(" set transform: " + transform);
if (Float.isNaN(transform.getPosition().x())) {
Log.critical(" set transform: " + transform);
LOGGER.error("[CRITICAL] set transform: " + transform);
System.exit(-1);
}
if (Float.isInfinite(transform.getOrientation().z())) {
Log.critical(" set transform: " + transform);
LOGGER.error("[CRITICAL] set transform: " + transform);
System.exit(-1);
}
}
/**
* Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
* when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program.
*
*
* This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the
* returned proxy shape to get and set information about the corresponding collision shape for that body.
* @param collisionShape A pointer to the collision shape you want to add to the body
@ -103,7 +107,7 @@ public class CollisionBody extends Body {
this.numberCollisionShapes++;
return proxyShape;
}
/**
* Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
*/
@ -112,7 +116,7 @@ public class CollisionBody extends Body {
this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape);
}
}
/**
* Compute and return the AABB of the body by merging all proxy shapes AABBs
* @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
@ -122,20 +126,21 @@ public class CollisionBody extends Body {
if (this.proxyCollisionShapes == null) {
return bodyAABB;
}
//Log.info("tmp this.transform : " + this.transform);
//Log.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
//Log.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform()));
//LOGGER.info("tmp this.transform : " + this.transform);
//LOGGER.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
//LOGGER.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB,
this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform()));
for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) {
final AABB aabb = new AABB();
shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
bodyAABB.mergeWithAABB(aabb);
}
//Log.info("tmp aabb : " + bodyAABB);
//Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
//LOGGER.info("tmp aabb : " + bodyAABB);
//LOGGER.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
return bodyAABB;
}
/**
* Get the first element of the linked list of contact manifolds involving this body
* @return A pointer to the first element of the linked-list with the contact manifolds of this body
@ -143,7 +148,7 @@ public class CollisionBody extends Body {
public ContactManifoldListElement getContactManifoldsList() {
return this.contactManifoldsList;
}
/**
* Get the body local-space coordinates of a point given in the world-space coordinates
* @param worldPoint A point in world-space coordinates
@ -152,7 +157,7 @@ public class CollisionBody extends Body {
public Vector3f getLocalPoint(final Vector3f worldPoint) {
return this.transform.inverseNew().multiply(worldPoint);
}
/**
* Get the body local-space coordinates of a vector given in the world-space coordinates
* @param worldVector A vector in world-space coordinates
@ -161,7 +166,7 @@ public class CollisionBody extends Body {
public Vector3f getLocalVector(final Vector3f worldVector) {
return this.transform.getOrientation().inverse().multiply(worldVector);
}
/**
* Get the linked list of proxy shapes of that body
* @return The pointer of the first proxy shape of the linked-list of all the
@ -170,7 +175,7 @@ public class CollisionBody extends Body {
public ProxyShape getProxyShapesList() {
return this.proxyCollisionShapes;
}
/**
* Return the current position and orientation
* @return The current transformation of the body that transforms the local-space of the body int32to world-space
@ -178,7 +183,7 @@ public class CollisionBody extends Body {
public Transform3D getTransform() {
return this.transform;
}
/**
* Return the type of the body
* @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
@ -186,11 +191,11 @@ public class CollisionBody extends Body {
public BodyType getType() {
return this.type;
}
public CollisionWorld getWorld() {
return this.world;
}
/**
* Get the world-space coordinates of a point given the local-space coordinates of the body
* @param localPoint A point in the local-space coordinates of the body
@ -199,7 +204,7 @@ public class CollisionBody extends Body {
public Vector3f getWorldPoint(final Vector3f localPoint) {
return this.transform.multiply(localPoint);
}
/**
* Get the world-space vector of a vector given in local-space coordinates of the body
* @param localVector A vector in the local-space coordinates of the body
@ -208,7 +213,7 @@ public class CollisionBody extends Body {
public Vector3f getWorldVector(final Vector3f localVector) {
return this.transform.getOrientation().multiply(localVector);
}
/**
* Raycast method with feedback information
* The method returns the closest hit among all the collision shapes of the body
@ -221,7 +226,7 @@ public class CollisionBody extends Body {
return false;
}
boolean isHit = false;
final Ray rayTemp = new Ray(ray);
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
// Test if the ray hits the collision shape
@ -232,7 +237,7 @@ public class CollisionBody extends Body {
}
return isHit;
}
/**
* Remove all the collision shapes
*/
@ -250,7 +255,7 @@ public class CollisionBody extends Body {
}
this.proxyCollisionShapes = null;
}
/**
* Remove a collision shape from the body
* To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body
@ -286,7 +291,7 @@ public class CollisionBody extends Body {
current = current.next;
}
}
/**
* Reset the contact manifold lists
*/
@ -294,7 +299,7 @@ public class CollisionBody extends Body {
// Delete the linked list of contact manifolds of that body
this.contactManifoldsList = null;
}
/**
* Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
* This method also returns the number of contact manifolds of the body.
@ -311,7 +316,7 @@ public class CollisionBody extends Body {
}
return nbManifolds;
}
/**
* Set whether or not the body is active
* @param isActive True if you want to activate the body
@ -337,23 +342,25 @@ public class CollisionBody extends Body {
resetContactManifoldsList();
}
}
/**
* Set the current position and orientation
* @param transform The transformation of the body that transforms the local-space of the body int32to world-space
*/
public void setTransform(final Transform3D transform) {
//Log.info(" set transform: " + this.transform + " ==> " + transform);
//LOGGER.info(" set transform: " + this.transform + " ==> " + transform);
if (Float.isNaN(transform.getPosition().x())) {
Log.critical(" set transform: " + this.transform + " ==> " + transform);
LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform);
System.exit(-1);
}
if (Float.isInfinite(transform.getOrientation().z())) {
Log.critical(" set transform: " + this.transform + " ==> " + transform);
LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform);
System.exit(-1);
}
this.transform = transform;
updateBroadPhaseState();
}
/**
* Set the type of the body
* @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
@ -365,7 +372,7 @@ public class CollisionBody extends Body {
updateBroadPhaseState();
}
}
/**
* Return true if a point is inside the collision body
* This method returns true if a point is inside any collision shape of the body
@ -380,7 +387,7 @@ public class CollisionBody extends Body {
}
return false;
}
/**
* Update the broad-phase state for this body (because it has moved for instance)
*/
@ -391,13 +398,14 @@ public class CollisionBody extends Body {
updateProxyShapeInBroadPhase(shape, false);
}
}
/**
* Update the broad-phase state of a proxy collision shape of the body
*/
public void updateProxyShapeInBroadPhase(final ProxyShape proxyShape, final boolean forceReinsert /* false */) {
final AABB aabb = new AABB();
proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiply(proxyShape.getLocalToBodyTransform()));
this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0), forceReinsert);
this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0),
forceReinsert);
}
}

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

@ -32,10 +32,11 @@ import org.atriasoft.ephysics.constraint.JointListElement;
import org.atriasoft.ephysics.engine.CollisionWorld;
import org.atriasoft.ephysics.engine.DynamicsWorld;
import org.atriasoft.ephysics.engine.Material;
import org.atriasoft.ephysics.internal.Log;
import org.atriasoft.etk.math.Matrix3f;
import org.atriasoft.etk.math.Transform3D;
import org.atriasoft.etk.math.Vector3f;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
/**
* This class represents a rigid body of the physics
@ -44,7 +45,8 @@ import org.atriasoft.etk.math.Vector3f;
* CollisionBody class.
*/
public class RigidBody extends CollisionBody {
static final Logger LOGGER = LoggerFactory.getLogger(RigidBody.class);
protected float angularDamping = 0.0f; //!< Angular velocity damping factor
protected boolean angularReactionEnable = true;
public Vector3f angularVelocity = Vector3f.ZERO; //!< Angular velocity of the body
@ -61,7 +63,7 @@ public class RigidBody extends CollisionBody {
public Vector3f linearVelocity = Vector3f.ZERO; //!< Linear velocity of the body
public float massInverse; //!< Inverse of the mass of the body
protected Material material = new Material(); //!< Material properties of the rigid body
/**
* Constructor
* @param transform The transformation of the body
@ -73,7 +75,7 @@ public class RigidBody extends CollisionBody {
this.centerOfMassWorld = transform.getPosition();
this.massInverse = 1.0f / this.initMass;
}
/**
* Add a collision shape to the body.
* When you add a collision shape to the body, an intternal copy of this collision shape will be created internally.
@ -85,7 +87,10 @@ public class RigidBody extends CollisionBody {
* @param mass Mass (in kilograms) of the collision shape you want to add
* @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
*/
public ProxyShape addCollisionShape(final CollisionShape collisionShape, final Transform3D transform, final float mass) {
public ProxyShape addCollisionShape(
final CollisionShape collisionShape,
final Transform3D transform,
final float mass) {
assert (mass > 0.0f);
// Create a new proxy collision shape to attach the collision shape to the body
final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, mass);
@ -105,7 +110,7 @@ public class RigidBody extends CollisionBody {
recomputeMassInformation();
return proxyShape;
}
/**
* Apply an external force to the body at a given point (in world-space coordinates).
* If the point is not at the center of mass of the body, it will also
@ -127,7 +132,7 @@ public class RigidBody extends CollisionBody {
this.externalForce = this.externalForce.add(force);
this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force));
}
/**
* Apply an external force to the body at its center of mass.
* If the body is sleeping, calling this method will wake it up. Note that the
@ -145,7 +150,7 @@ public class RigidBody extends CollisionBody {
}
this.externalForce = this.externalForce.add(force);
}
/**
* Apply an external torque to the body.
* If the body is sleeping, calling this method will wake it up. Note that the
@ -163,7 +168,7 @@ public class RigidBody extends CollisionBody {
}
this.externalTorque = this.externalTorque.add(torque);
}
/**
* Set the variable to know if the gravity is applied to this rigid body
* @param isEnabled True if you want the gravity to be applied to this body
@ -171,7 +176,7 @@ public class RigidBody extends CollisionBody {
public void enableGravity(final boolean isEnabled) {
this.isGravityEnabled = isEnabled;
}
/**
* Get the angular velocity damping factor
* @return The angular damping factor of this body
@ -179,7 +184,7 @@ public class RigidBody extends CollisionBody {
public float getAngularDamping() {
return this.angularDamping;
}
/**
* Get the angular velocity of the body
* @return The angular velocity vector of the body
@ -187,7 +192,7 @@ public class RigidBody extends CollisionBody {
public Vector3f getAngularVelocity() {
return this.angularVelocity;
}
/**
* Get the inverse of the inertia tensor in world coordinates.
* The inertia tensor Iw in world coordinates is computed with the
@ -201,15 +206,16 @@ public class RigidBody extends CollisionBody {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates
//Log.error("}}} this.transform=" + this.transform);
//Log.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
//Log.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
//Log.error("}}} this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix());
final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transpose());
//Log.error("}}} tmp=" + tmp);
//LOGGER.error("}}} this.transform=" + this.transform);
//LOGGER.error("}}} this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
//LOGGER.error("}}} this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
//LOGGER.error("}}} this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix());
final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse)
.multiply(this.transform.getOrientation().getMatrix().transpose());
//LOGGER.error("}}} tmp=" + tmp);
return tmp;
}
/**
* Get the local inertia tensor of the body (in local-space coordinates)
* @return The 3x3 inertia tensor matrix of the body
@ -217,7 +223,7 @@ public class RigidBody extends CollisionBody {
public Matrix3f getInertiaTensorLocal() {
return this.inertiaTensorLocal;
}
/**
* Get the inertia tensor in world coordinates.
* The inertia tensor Iw in world coordinates is computed
@ -229,9 +235,10 @@ public class RigidBody extends CollisionBody {
*/
public Matrix3f getInertiaTensorWorld() {
// Compute and return the inertia tensor in world coordinates
return this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocal).multiply(this.transform.getOrientation().getMatrix().transpose());
return this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocal)
.multiply(this.transform.getOrientation().getMatrix().transpose());
}
/**
* Get the first element of the linked list of joints involving this body
* @return The first element of the linked-list of all the joints involving this body
@ -239,7 +246,7 @@ public class RigidBody extends CollisionBody {
public JointListElement getJointsList() {
return this.jointsList;
}
/**
* Get the linear velocity damping factor
* @return The linear damping factor of this body
@ -247,7 +254,7 @@ public class RigidBody extends CollisionBody {
public float getLinearDamping() {
return this.linearDamping;
}
/**
* Get the linear velocity
* @return The linear velocity vector of the body
@ -255,7 +262,7 @@ public class RigidBody extends CollisionBody {
public Vector3f getLinearVelocity() {
return this.linearVelocity;
}
/**
* Get the mass of the body
* @return The mass (in kilograms) of the body
@ -263,7 +270,7 @@ public class RigidBody extends CollisionBody {
public float getMass() {
return this.initMass;
}
/**
* get a reference to the material properties of the rigid body
* @return A reference to the material of the body
@ -271,11 +278,11 @@ public class RigidBody extends CollisionBody {
public Material getMaterial() {
return this.material;
}
public boolean isAngularReactionEnable() {
return this.angularReactionEnable;
}
/**
* get the need of gravity appling to this rigid body
* @return True if the gravity is applied to the body
@ -283,7 +290,7 @@ public class RigidBody extends CollisionBody {
public boolean isGravityEnabled() {
return this.isGravityEnabled;
}
/**
* Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body.
*/
@ -302,7 +309,8 @@ public class RigidBody extends CollisionBody {
// Compute the total mass of the body
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
this.initMass += shape.getMass();
this.centerOfMassLocal = this.centerOfMassLocal.add(shape.getLocalToBodyTransform().getPosition().multiply(shape.getMass()));
this.centerOfMassLocal = this.centerOfMassLocal
.add(shape.getLocalToBodyTransform().getPosition().multiply(shape.getMass()));
}
if (this.initMass > 0.0f) {
this.massInverse = 1.0f / this.initMass;
@ -329,22 +337,24 @@ public class RigidBody extends CollisionBody {
final Vector3f off1 = offset.multiply(-offset.x());
final Vector3f off2 = offset.multiply(-offset.y());
final Vector3f off3 = offset.multiply(-offset.z());
Matrix3f offsetMatrix = new Matrix3f(off1.x() + offsetSquare, off1.y(), off1.z(), off2.x(), off2.y() + offsetSquare, off2.z(), off3.x(), off3.y(), off3.z() + offsetSquare);
Matrix3f offsetMatrix = new Matrix3f(off1.x() + offsetSquare, off1.y(), off1.z(), off2.x(),
off2.y() + offsetSquare, off2.z(), off3.x(), off3.y(), off3.z() + offsetSquare);
offsetMatrix = offsetMatrix.multiply(shape.getMass());
this.inertiaTensorLocal = this.inertiaTensorLocal.add(inertiaTensor.add(offsetMatrix));
}
// Compute the local inverse inertia tensor
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
// Update the linear velocity of the center of mass
this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
this.linearVelocity = this.linearVelocity
.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
}
@Override
public void removeCollisionShape(final ProxyShape proxyShape) {
super.removeCollisionShape(proxyShape);
recomputeMassInformation();
}
/**
* Remove a joint from the joints list
*/
@ -369,7 +379,7 @@ public class RigidBody extends CollisionBody {
}
}
}
/**
* Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
* @param angularDamping The angular damping factor of this body
@ -378,11 +388,11 @@ public class RigidBody extends CollisionBody {
assert (angularDamping >= 0.0f);
this.angularDamping = angularDamping;
}
public void setAngularReactionEnable(final boolean angularReactionEnable) {
this.angularReactionEnable = angularReactionEnable;
}
/**
* Set the angular velocity.
* @param angularVelocity The angular velocity vector of the body
@ -396,7 +406,7 @@ public class RigidBody extends CollisionBody {
setIsSleeping(false);
}
}
/**
* Set the local center of mass of the body (in local-space coordinates)
* @param centerOfMassLocal The center of mass of the body in local-space coordinates
@ -408,9 +418,10 @@ public class RigidBody extends CollisionBody {
final Vector3f oldCenterOfMass = this.centerOfMassWorld;
this.centerOfMassLocal = centerOfMassLocal;
this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
this.linearVelocity = this.linearVelocity
.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
}
/**
* Set the local inertia tensor of the body (in local-space coordinates)
* @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
@ -422,7 +433,7 @@ public class RigidBody extends CollisionBody {
this.inertiaTensorLocal = inertiaTensorLocal;
this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
}
/**
* Set the variable to know whether or not the body is sleeping
* @param isSleeping New sleeping state of the body
@ -437,7 +448,7 @@ public class RigidBody extends CollisionBody {
}
super.setIsSleeping(isSleeping);
}
/**
* Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
* @param linearDamping The linear damping factor of this body
@ -446,7 +457,7 @@ public class RigidBody extends CollisionBody {
assert (linearDamping >= 0.0f);
this.linearDamping = linearDamping;
}
/**
* Set the linear velocity of the rigid body.
* @param linearVelocity Linear velocity vector of the body
@ -460,7 +471,7 @@ public class RigidBody extends CollisionBody {
setIsSleeping(false);
}
}
/**
* Set the mass of the rigid body
* @param mass The mass (in kilograms) of the body
@ -477,7 +488,7 @@ public class RigidBody extends CollisionBody {
this.massInverse = 1.0f;
}
}
/**
* Set a new material for this rigid body
* @param material The material you want to set to the body
@ -485,29 +496,32 @@ public class RigidBody extends CollisionBody {
public void setMaterial(final Material material) {
this.material = material;
}
/**
* Set the current position and orientation
* @param transform The transformation of the body that transforms the local-space of the body into world-space
*/
@Override
public void setTransform(final Transform3D transform) {
//Log.info(" set transform: " + this.transform + " ==> " + transform);
//LOGGER.info(" set transform: " + this.transform + " ==> " + transform);
if (transform.getPosition().x() == Float.NaN) {
Log.critical(" set transform: " + this.transform + " ==> " + transform);
LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform);
System.exit(-1);
}
if (Float.isInfinite(transform.getOrientation().z())) {
Log.critical(" set transform: " + this.transform + " ==> " + transform);
LOGGER.error("[CRITICAL] set transform: " + this.transform + " ==> " + transform);
System.exit(-1);
}
this.transform = transform;
final Vector3f oldCenterOfMass = this.centerOfMassWorld;
// Compute the new center of mass in world-space coordinates
this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
// Update the linear velocity of the center of mass
this.linearVelocity = this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
this.linearVelocity = this.linearVelocity
.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
updateBroadPhaseState();
}
@Override
public void setType(final BodyType type) {
if (this.type == type) {
@ -536,38 +550,42 @@ public class RigidBody extends CollisionBody {
this.externalForce = Vector3f.ZERO;
this.externalTorque = Vector3f.ZERO;
}
@Override
public void updateBroadPhaseState() {
final DynamicsWorld world = (DynamicsWorld) this.world;
final Vector3f displacement = this.linearVelocity.multiply(world.timeStep);
// For all the proxy collision shapes of the body
for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
// Recompute the world-space AABB of the collision shape
final AABB aabb = new AABB();
//Log.info(" : " + aabb.getMin() + " " + aabb.getMax());
//Log.info(" this.transform: " + this.transform);
//LOGGER.info(" : " + aabb.getMin() + " " + aabb.getMax());
//LOGGER.info(" this.transform: " + this.transform);
shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
//Log.info(" : " + aabb.getMin() + " " + aabb.getMax());
//LOGGER.info(" : " + aabb.getMin() + " " + aabb.getMax());
// Update the broad-phase state for the proxy collision shape
//Log.warning(" ==> updateProxyCollisionShape");
//LOGGER.warn(" ==> updateProxyCollisionShape");
world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
}
}
/**
* Update the transform of the body after a change of the center of mass
*/
public void updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
this.transform = new Transform3D(this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)), this.transform.getOrientation());
this.transform = new Transform3D(
this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)),
this.transform.getOrientation());
if (Float.isNaN(this.transform.getPosition().x())) {
Log.critical("updateTransformWithCenterOfMass: " + this.transform);
LOGGER.error("[CRITICAL] updateTransformWithCenterOfMass: " + this.transform);
System.exit(-1);
}
if (Float.isInfinite(this.transform.getOrientation().z())) {
Log.critical(" set transform: " + this.transform);
LOGGER.error("[CRITICAL] set transform: " + this.transform);
System.exit(-1);
}
}
}

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -76,8 +76,8 @@ public class BoxShape extends ConvexShape {
*/
@Override
public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
//Log.error("getLocalSupportPointWithoutMargin(" + direction);
//Log.error(" extends = " + this.extent);
//LOGGER.error("getLocalSupportPointWithoutMargin(" + direction);
//LOGGER.error(" extends = " + this.extent);
return new Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(), direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
}

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

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

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
public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) {
////Log.error(" -> getLocalSupportPointWithMargin(" + direction);
////LOGGER.error(" -> getLocalSupportPointWithMargin(" + direction);
// Get the support point without margin
Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
////Log.error(" -> supportPoint = " + supportPoint);
////Log.error(" -> margin = " + FMath.floatToString(this.margin));
////LOGGER.error(" -> supportPoint = " + supportPoint);
////LOGGER.error(" -> margin = " + FMath.floatToString(this.margin));
if (this.margin != 0.0f) {
// Add the margin to the support point
Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f);
////Log.error(" -> direction.length2()=" + FMath.floatToString(direction.length2()));
////Log.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON));
////LOGGER.error(" -> direction.length2()=" + FMath.floatToString(direction.length2()));
////LOGGER.error(" -> Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON));
if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
unitVec = direction.safeNormalize();
////Log.error(" -> unitVec= " + unitVec);
////LOGGER.error(" -> unitVec= " + unitVec);
}
supportPoint = supportPoint.add(unitVec.multiply(this.margin));
}
////Log.error(" -> ==> supportPoint = " + supportPoint);
////LOGGER.error(" -> ==> supportPoint = " + supportPoint);
return supportPoint;
}

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

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

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

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

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

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

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