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

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

@ -30,16 +30,18 @@ import org.atriasoft.ephysics.collision.ProxyShape;
import org.atriasoft.ephysics.collision.shapes.AABB;
import org.atriasoft.ephysics.collision.shapes.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
@ -68,12 +70,14 @@ public class CollisionBody extends Body {
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);
}
}
@ -122,17 +126,18 @@ 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;
}
@ -343,12 +348,14 @@ public class CollisionBody extends Body {
* @param transform The transformation of the body that transforms the local-space of the body int32to world-space
*/
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();
@ -398,6 +405,7 @@ public class CollisionBody extends 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);
}
}

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

@ -32,10 +32,11 @@ import org.atriasoft.ephysics.constraint.JointListElement;
import org.atriasoft.ephysics.engine.CollisionWorld;
import org.atriasoft.ephysics.engine.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,6 +45,7 @@ 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;
@ -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);
@ -201,12 +206,13 @@ 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;
}
@ -229,7 +235,8 @@ 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());
}
/**
@ -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,14 +337,16 @@ 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
@ -408,7 +418,8 @@ 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)));
}
/**
@ -492,19 +503,22 @@ public class RigidBody extends CollisionBody {
*/
@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();
}
@ -546,12 +560,12 @@ public class RigidBody extends CollisionBody {
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);
}
}
@ -561,12 +575,16 @@ public class RigidBody extends CollisionBody {
*/
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);
}
}

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

@ -2,6 +2,7 @@ package org.atriasoft.ephysics.collision;
import java.util.Iterator;
import java.util.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,7 +53,7 @@ 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;
@ -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);
@ -168,10 +173,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
/// 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();
}
@ -181,11 +186,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
// 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()) {
}
}
@ -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,9 +236,11 @@ 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 shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(),
shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
@ -250,7 +254,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
}
/// Compute the narrow-phase collision detection
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,11 +320,14 @@ 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 shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(),
shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(callback);
final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(
callback);
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision
narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback);
@ -338,13 +352,16 @@ public class CollisionDetection implements NarrowPhaseCallback {
// 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];
}
@ -365,7 +382,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
/// 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
@ -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();
@ -412,26 +430,29 @@ public class CollisionDetection implements NarrowPhaseCallback {
}
/// Report collision between two sets of shapes
public void reportCollisionBetweenShapes(final CollisionCallback callback, final Set<DTree> shapes1, final Set<DTree> shapes2) {
public void reportCollisionBetweenShapes(
final CollisionCallback callback,
final Set<DTree> shapes1,
final Set<DTree> shapes2) {
// For each possible collision pair of bodies
final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
while (it.hasNext()) {
final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
final OverlappingPair pair = entry.getValue();
final 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,8 +463,10 @@ 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) {
@ -473,7 +496,10 @@ public class CollisionDetection implements NarrowPhaseCallback {
}
/// 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
@ -491,7 +517,11 @@ public class CollisionDetection implements NarrowPhaseCallback {
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);

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

@ -3,10 +3,12 @@ package org.atriasoft.ephysics.collision.narrowphase.EPA;
import java.util.ArrayList;
import java.util.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) {
@ -43,7 +45,7 @@ public class TrianglesStore {
/// 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) {
@ -52,7 +54,7 @@ public class TrianglesStore {
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)
@ -63,7 +65,7 @@ public class TrianglesStore {
/// 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());
}

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

@ -13,10 +13,11 @@ import org.atriasoft.ephysics.collision.broadphase.CallbackRaycast;
import org.atriasoft.ephysics.collision.broadphase.DTree;
import org.atriasoft.ephysics.collision.broadphase.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;
@ -34,7 +37,8 @@ public class ConcaveMeshShape extends ConcaveShape {
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;
@ -69,7 +73,8 @@ public class ConcaveMeshShape extends ConcaveShape {
// 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();
@ -121,12 +126,15 @@ public class ConcaveMeshShape extends ConcaveShape {
/// 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);
@ -165,7 +173,8 @@ public class ConcaveMeshShape extends ConcaveShape {
@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.

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

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

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

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

@ -7,13 +7,15 @@ import org.atriasoft.ephysics.body.BodyType;
import org.atriasoft.ephysics.body.RigidBody;
import org.atriasoft.ephysics.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
@ -33,7 +35,7 @@ public class Island {
*/
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);

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