diff --git a/.classpath b/.classpath new file mode 100644 index 0000000..7bbb68c --- /dev/null +++ b/.classpath @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f8bdd50 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +/examples/target/ +/examples/framework.log.* +/target/ \ No newline at end of file diff --git a/.project b/.project new file mode 100644 index 0000000..a324e0f --- /dev/null +++ b/.project @@ -0,0 +1,18 @@ + + + atriasoft-ephysics + + + atriasoft-ephysics + + + + org.eclipse.jdt.core.javabuilder + + + + + + org.eclipse.jdt.core.javanature + + diff --git a/src/module-info.java b/src/module-info.java new file mode 100644 index 0000000..537600f --- /dev/null +++ b/src/module-info.java @@ -0,0 +1,21 @@ +/** Basic module interface. + * + * @author Edouard DUPIN */ + +open module org.atriasoft.ephysics { + exports org.atriasoft.ephysics.body; + exports org.atriasoft.ephysics.collision; + exports org.atriasoft.ephysics.collision.broadphase; + exports org.atriasoft.ephysics.collision.narrowphase; + exports org.atriasoft.ephysics.collision.narrowphase.EPA; + exports org.atriasoft.ephysics.collision.narrowphase.GJK; + exports org.atriasoft.ephysics.collision.shapes; + exports org.atriasoft.ephysics.configuration; + exports org.atriasoft.ephysics.constraint; + exports org.atriasoft.ephysics.engine; + exports org.atriasoft.ephysics.mathematics; + exports org.atriasoft.ephysics; + + requires transitive org.atriasoft.etk; + requires javafx.base; +} diff --git a/src/org/atriasoft/ephysics/Configuration.java b/src/org/atriasoft/ephysics/Configuration.java new file mode 100644 index 0000000..f159a05 --- /dev/null +++ b/src/org/atriasoft/ephysics/Configuration.java @@ -0,0 +1,54 @@ +package org.atriasoft.ephysics; + +import org.atriasoft.etk.math.Constant; + +public class Configuration { + + /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are + /// inflated with a ant gap to allow the collision shape to move a little bit + /// without triggering a large modification of the tree which can be costly + public static final float DYNAMIC_TREE_AABB_GAP = 0.1f; + + /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are + /// also inflated in direction of the linear motion of the body by mutliplying the + /// followin ant with the linear velocity and the elapsed time between two frames. + public static final float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = 1.7f; + + /// Distance threshold for two contact points for a valid persistent contact (in meters) + public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f; + + /// Default friction coefficient for a rigid body + public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f; + + /// Default bounciness factor for a rigid body + public static final float DEFAULT_BOUNCINESS = 0.5f; + + /// Default rolling resistance + public static final float DEFAULT_ROLLING_RESISTANCE = 0.0f; + + /// True if the spleeping technique is enabled + public static final boolean SPLEEPING_ENABLED = true; + + /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) + public static final float OBJECT_MARGIN = 0.04f; + + /// Velocity threshold for contact velocity restitution + public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f; + + /// Number of iterations when solving the velocity raints of the Sequential Impulse technique + public static final int DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10; + + /// Number of iterations when solving the position raints of the Sequential Impulse technique + public static final int DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; + + /// Time (in seconds) that a body must stay still to be idered sleeping + public static final float DEFAULT_TIME_BEFORE_SLEEP = 0.3f; //(old default : 3.0) + + /// A body with a linear velocity smaller than the sleep linear velocity (in m/s) + /// might enter sleeping mode. + public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.3f; //(old default : 0.01) + + /// A body with angular velocity smaller than the sleep angular velocity (in rad/s) + /// might enter sleeping mode + public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 5.0f * (Constant.PI / 180.0f); //(old default : 0.3) +} diff --git a/src/org/atriasoft/ephysics/RaycastCallback.java b/src/org/atriasoft/ephysics/RaycastCallback.java new file mode 100644 index 0000000..e746983 --- /dev/null +++ b/src/org/atriasoft/ephysics/RaycastCallback.java @@ -0,0 +1,20 @@ +package org.atriasoft.ephysics; + +public interface RaycastCallback { + /** + * @brief This method will be called for each ProxyShape that is hit by the + * ray. You cannot make any assumptions about the order of the + * calls. You should use the return value to control the continuation + * of the ray. The returned value is the next maxFraction value to use. + * If you return a fraction of 0.0, it means that the raycast should + * terminate. If you return a fraction of 1.0, it indicates that the + * ray is not clipped and the ray cast should continue as if no hit + * occurred. If you return the fraction in the parameter (hitFraction + * value in the RaycastInfo object), the current ray will be clipped + * to this fraction in the next queries. If you return -1.0, it will + * ignore this ProxyShape and continue the ray cast. + * @param[in] _raycastInfo Information about the raycast hit + * @return Value that controls the continuation of the ray after a hit + */ + float notifyRaycastHit(RaycastInfo _raycastInfo); +} diff --git a/src/org/atriasoft/ephysics/RaycastInfo.java b/src/org/atriasoft/ephysics/RaycastInfo.java new file mode 100644 index 0000000..d54ea2f --- /dev/null +++ b/src/org/atriasoft/ephysics/RaycastInfo.java @@ -0,0 +1,16 @@ +package org.atriasoft.ephysics; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.collision.ProxyShape; + +public class RaycastInfo { + public Vector3f worldPoint = new Vector3f(); //!< Hit point in world-space coordinates + public Vector3f worldNormal = new Vector3f(); //!< Surface normal at hit point in world-space coordinates + public float hitFraction = 0.0f; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1) + public long meshSubpart = -1; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) + public long triangleIndex = -1; //!< Hit triangle index (only used for triangles mesh and -1 otherwise) + public CollisionBody body; //!< Pointer to the hit collision body; + public ProxyShape proxyShape; //!< Pointer to the hit proxy collision shape +} diff --git a/src/org/atriasoft/ephysics/RaycastTest.java b/src/org/atriasoft/ephysics/RaycastTest.java new file mode 100644 index 0000000..bee1b57 --- /dev/null +++ b/src/org/atriasoft/ephysics/RaycastTest.java @@ -0,0 +1,28 @@ +package org.atriasoft.ephysics; + +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.mathematics.Ray; + +public class RaycastTest { + + public RaycastCallback userCallback; //!< User callback class + /// Constructor + + public RaycastTest(final RaycastCallback _callback) { + this.userCallback = _callback; + } + + /// Ray cast test against a proxy shape + public float raycastAgainstShape(final ProxyShape _shape, final Ray _ray) { + // Ray casting test against the collision shape + final RaycastInfo raycastInfo = new RaycastInfo(); + final boolean isHit = _shape.raycast(_ray, raycastInfo); + // If the ray hit the collision shape + if (isHit) { + // Report the hit to the user and return the + // user hit fraction value + return this.userCallback.notifyRaycastHit(raycastInfo); + } + return _ray.maxFraction; + } +} diff --git a/src/org/atriasoft/ephysics/body/Body.java b/src/org/atriasoft/ephysics/body/Body.java new file mode 100644 index 0000000..3f49ad7 --- /dev/null +++ b/src/org/atriasoft/ephysics/body/Body.java @@ -0,0 +1,154 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.body; + +/** + * This class is an abstract class to represent a body of the physics engine. + * + * @author Jason Sorensen + */ +public abstract class Body { + // Unique ID of the body + protected int id; // TODO check where it came from ... + // True if the body has already been added in an island (for sleeping technique) + public boolean isAlreadyInIsland = false; + // True if the body is allowed to go to sleep for better efficiency + protected boolean isAllowedToSleep = true; + /** + * True if the body is active. + * + * An inactive body does not participate in collision detection, is not simulated and will not be hit in a ray casting query. + * A body is active by default. If you set this value to "false", all the proxy shapes of this body will be removed from the broad-phase. + * If you set this value to "true", all the proxy shapes will be added to the broad-phase. + * A joint connected to an inactive body will also be inactive. + */ + protected boolean isActive = true; + // True if the body is sleeping (for sleeping technique) + protected boolean isSleeping = false; + // Elapsed time since the body velocity was bellow the sleep velocity + public float sleepTime = 0; + // user data + protected Object userData = null; + + /** + * @brief Constructor + * @param[in] _id ID of the new body + */ + public Body(final int bodyID) { + this.id = bodyID; + } + + // Return the id of the body + public int getID() { + return this.id; + } + + public float getSleepTime() { + return this.sleepTime; + } + + /** + * @brief Return a pointer to the user data attached to this body + * @return A pointer to the user data you have attached to the body + */ + Object getUserData() { + return this.userData; + } + + /** + * @brief Return the id of the body + * @return The ID of the body + */ + public boolean isActive() { + return this.isActive; + } + + /** + * @brief Set whether or not the body is allowed to go to sleep + * @param[in] _isAllowedToSleep True if the body is allowed to sleep + */ + public boolean isAllowedToSleep() { + return this.isAllowedToSleep; + } + + public boolean isAlreadyInIsland() { + return this.isAlreadyInIsland; + } + + /** + * @brief Return whether or not the body is sleeping + * @return True if the body is currently sleeping and false otherwise + */ + public boolean isSleeping() { + return this.isSleeping; + } + + /** + * @brief Set whether or not the body is active + * @param[in] _isActive True if you want to activate the body + */ + void setIsActive(final boolean isActive) { + this.isActive = isActive; + } + + // Set whether or not the body is allowed to go to sleep + public void setIsAllowedToSleep(final boolean isAllowedToSleep) { + this.isAllowedToSleep = isAllowedToSleep; + + if (!this.isAllowedToSleep) { + setIsSleeping(false); + } + } + + public void setIsAlreadyInIsland(final boolean isAlreadyInIsland) { + this.isAlreadyInIsland = isAlreadyInIsland; + } + + /** + * @brief Set the variable to know whether or not the body is sleeping + * @param[in] _isSleeping Set the new status + */ + public void setIsSleeping(final boolean isSleeping) { + if (isSleeping) { + this.sleepTime = 0.0f; + } else if (this.isSleeping) { + this.sleepTime = 0.0f; + } + this.isSleeping = isSleeping; + } + + public void setSleepTime(final float sleepTime) { + this.sleepTime = sleepTime; + } + + /** + * @brief Attach user data to this body + * @param[in] _userData A pointer to the user data you want to attach to the body + */ + public void setUserData(final Object userData) { + this.userData = userData; + } + +} diff --git a/src/org/atriasoft/ephysics/body/BodyType.java b/src/org/atriasoft/ephysics/body/BodyType.java new file mode 100644 index 0000000..c142fd8 --- /dev/null +++ b/src/org/atriasoft/ephysics/body/BodyType.java @@ -0,0 +1,7 @@ +package org.atriasoft.ephysics.body; + +public enum BodyType { + STATIC, //!< A static body has infinite mass, zero velocity but the position can be changed manually. A static body does not collide with other static or kinematic bodies. + KINEMATIC, //!< A kinematic body has infinite mass, the velocity can be changed manually and its position is computed by the physics engine. A kinematic body does not collide with other static or kinematic bodies. + DYNAMIC //!< A dynamic body has non-zero mass, non-zero velocity determined by forces and its position is determined by the physics engine. A dynamic body can collide with other dynamic, static or kinematic bodies. +} diff --git a/src/org/atriasoft/ephysics/body/CollisionBody.java b/src/org/atriasoft/ephysics/body/CollisionBody.java new file mode 100644 index 0000000..81a694e --- /dev/null +++ b/src/org/atriasoft/ephysics/body/CollisionBody.java @@ -0,0 +1,404 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.body; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ContactManifoldListElement; +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; + +/** + * This class represents a body that is able to collide with others bodies. This class inherits from the Body class. + * + * @author Jason Sorensen + */ +public class CollisionBody extends Body { + // Type of body (static, kinematic or dynamic) + protected BodyType type; + // Position and orientation of the body + protected Transform3D transform; + // First element of the linked list of proxy collision shapes of this body + protected ProxyShape proxyCollisionShapes; + // Number of collision shapes + protected long numberCollisionShapes; + // First element of the linked list of contact manifolds involving this body + public ContactManifoldListElement contactManifoldsList; + // Reference to the world the body belongs to + protected CollisionWorld world; + + /** + * @brief Constructor + * @param[in] _transform The transform of the body + * @param[in] _world The physics world where the body is created + * @param[in] _id ID of the body + */ + public CollisionBody(final Transform3D _transform, final CollisionWorld _world, final int _id) { + super(_id); + this.type = BodyType.DYNAMIC; + this.transform = _transform; + this.proxyCollisionShapes = null; + this.numberCollisionShapes = 0; + this.contactManifoldsList = null; + this.world = _world; + + //Log.debug(" set transform: " + _transform); + if (Float.isNaN(_transform.getPosition().x)) { + Log.critical(" set transform: " + _transform); + } + if (Float.isInfinite(_transform.getOrientation().z)) { + Log.critical(" set transform: " + _transform); + } + } + + /** + * @brief Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to + * when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program. + * + * This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the + * returned proxy shape to get and set information about the corresponding collision shape for that body. + * @param[in] collisionShape A pointer to the collision shape you want to add to the body + * @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape int32_to the local-space of the body + * @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) { + // Create a proxy collision shape to attach the collision shape to the body + final ProxyShape proxyShape = new ProxyShape(this, _collisionShape, _transform, 1.0f); + // Add it to the list of proxy collision shapes of the body + if (this.proxyCollisionShapes == null) { + this.proxyCollisionShapes = proxyShape; + } else { + proxyShape.next = this.proxyCollisionShapes; + this.proxyCollisionShapes = proxyShape; + } + final AABB aabb = new AABB(); + _collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform)); + this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb); + this.numberCollisionShapes++; + return proxyShape; + } + + /** + * @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved). + */ + protected void askForBroadPhaseCollisionCheck() { + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.getNext()) { + this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape); + } + } + + /** + * @brief Compute and return the AABB of the body by merging all proxy shapes AABBs + * @return The axis-aligned bounding box (AABB) of the body in world-space coordinates + */ + public AABB getAABB() { + final AABB bodyAABB = new AABB(); + 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.multiplyNew(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.multiplyNew(shape.getLocalToBodyTransform())); + bodyAABB.mergeWithAABB(aabb); + } + //Log.info("tmp aabb : " + bodyAABB); + //Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin())); + return bodyAABB; + } + + /** + * @brief Get the first element of the linked list of contact manifolds involving this body + * @return A pointer to the first element of the linked-list with the contact manifolds of this body + */ + public ContactManifoldListElement getContactManifoldsList() { + return this.contactManifoldsList; + } + + /** + * @brief Get the body local-space coordinates of a point given in the world-space coordinates + * @param[in] _worldPoint A point in world-space coordinates + * @return The point in the local-space coordinates of the body + */ + public Vector3f getLocalPoint(final Vector3f _worldPoint) { + return this.transform.inverseNew().multiply(_worldPoint); + } + + /** + * @brief Get the body local-space coordinates of a vector given in the world-space coordinates + * @param[in] _worldVector A vector in world-space coordinates + * @return The vector in the local-space coordinates of the body + */ + public Vector3f getLocalVector(final Vector3f _worldVector) { + return this.transform.getOrientation().inverseNew().multiply(_worldVector); + } + + /** + * @brief Get the linked list of proxy shapes of that body + * @return The pointer of the first proxy shape of the linked-list of all the + * proxy shapes of the body + */ + public ProxyShape getProxyShapesList() { + return this.proxyCollisionShapes; + } + + /** + * @brief Return the current position and orientation + * @return The current transformation of the body that transforms the local-space of the body int32_to world-space + */ + public Transform3D getTransform() { + return this.transform; + } + + /** + * @brief Return the type of the body + * @return the type of the body (STATIC, KINEMATIC, DYNAMIC) + */ + public BodyType getType() { + return this.type; + } + + public CollisionWorld getWorld() { + return this.world; + } + + /** + * @brief Get the world-space coordinates of a point given the local-space coordinates of the body + * @param[in] _localPoint A point in the local-space coordinates of the body + * @return The point in world-space coordinates + */ + public Vector3f getWorldPoint(final Vector3f _localPoint) { + return this.transform.multiplyNew(_localPoint); + } + + /** + * @brief Get the world-space vector of a vector given in local-space coordinates of the body + * @param[in] _localVector A vector in the local-space coordinates of the body + * @return The vector in world-space coordinates + */ + public Vector3f getWorldVector(final Vector3f _localVector) { + return this.transform.getOrientation().multiply(_localVector); + } + + /** + * @brief Raycast method with feedback information + * The method returns the closest hit among all the collision shapes of the body + * @param[in] _ray The ray used to raycast agains the body + * @param[out] _raycastInfo Structure that contains the result of the raycasting (valid only if the method returned true) + * @return True if the ray hit the body and false otherwise + */ + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo) { + if (this.isActive == false) { + return false; + } + boolean isHit = false; + + final Ray rayTemp = new Ray(_ray); + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + // Test if the ray hits the collision shape + if (shape.raycast(rayTemp, _raycastInfo)) { + rayTemp.maxFraction = _raycastInfo.hitFraction; + isHit = true; + } + } + return isHit; + } + + /** + * @brief Remove all the collision shapes + */ + public void removeAllCollisionShapes() { + ProxyShape current = this.proxyCollisionShapes; + // Look for the proxy shape that contains the collision shape in parameter + while (current != null) { + // Remove the proxy collision shape + final ProxyShape nextElement = current.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(current); + } + // Get the next element in the list + current = nextElement; + } + this.proxyCollisionShapes = null; + } + + /** + * @brief Remove a collision shape from the body + * To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body + * @param[in] _proxyShape The pointer of the proxy shape you want to remove + */ + public void removeCollisionShape(final ProxyShape _proxyShape) { + ProxyShape current = this.proxyCollisionShapes; + // If the the first proxy shape is the one to remove + if (current == _proxyShape) { + this.proxyCollisionShapes = current.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(current); + } + current = null; + this.numberCollisionShapes--; + return; + } + // Look for the proxy shape that contains the collision shape in parameter + while (current.next != null) { + // If we have found the collision shape to remove + if (current.next == _proxyShape) { + // Remove the proxy collision shape + ProxyShape elementToRemove = current.next; + current.next = elementToRemove.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(elementToRemove); + } + elementToRemove = null; + this.numberCollisionShapes--; + return; + } + // Get the next element in the list + current = current.next; + } + } + + /** + * @brief Reset the contact manifold lists + */ + public void resetContactManifoldsList() { + // Delete the linked list of contact manifolds of that body + this.contactManifoldsList = null; + } + + /** + * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds. + * This method also returns the number of contact manifolds of the body. + */ + public int resetIsAlreadyInIslandAndCountManifolds() { + this.isAlreadyInIsland = false; + int nbManifolds = 0; + // Reset the this.isAlreadyInIsland variable of the contact manifolds for this body + ContactManifoldListElement currentElement = this.contactManifoldsList; + while (currentElement != null) { + currentElement.contactManifold.isAlreadyInIsland = false; + currentElement = currentElement.next; + nbManifolds++; + } + return nbManifolds; + } + + /** + * @brief Set whether or not the body is active + * @param[in] _isActive True if you want to activate the body + */ + @Override + public void setIsActive(final boolean _isActive) { + // If the state does not change + if (this.isActive == _isActive) { + return; + } + super.setIsActive(_isActive); + // If we have to activate the body + if (_isActive == true) { + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + final AABB aabb = new AABB(); + shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.localToBodyTransform)); + this.world.collisionDetection.addProxyCollisionShape(shape, aabb); + } + } else { + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + this.world.collisionDetection.removeProxyCollisionShape(shape); + } + resetContactManifoldsList(); + } + } + + /** + * @brief Set the current position and orientation + * @param transform The transformation of the body that transforms the local-space of the body int32_to world-space + */ + public void setTransform(final Transform3D _transform) { + //Log.info(" set transform: " + this.transform + " ==> " + _transform); + if (Float.isNaN(_transform.getPosition().x)) { + Log.critical(" set transform: " + this.transform + " ==> " + _transform); + } + if (Float.isInfinite(_transform.getOrientation().z)) { + Log.critical(" set transform: " + this.transform + " ==> " + _transform); + } + this.transform = _transform; + updateBroadPhaseState(); + } + + /** + * @brief Set the type of the body + * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC) + */ + public void setType(final BodyType _type) { + this.type = _type; + if (this.type == BodyType.STATIC) { + // Update the broad-phase state of the body + updateBroadPhaseState(); + } + } + + /** + * @brief Return true if a point is inside the collision body + * This method returns true if a point is inside any collision shape of the body + * @param[in] _worldPoint The point to test (in world-space coordinates) + * @return True if the point is inside the body + */ + public boolean testPointInside(final Vector3f _worldPoint) { + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + if (shape.testPointInside(_worldPoint)) { + return true; + } + } + return false; + } + + /** + * @brief Update the broad-phase state for this body (because it has moved for instance) + */ + protected void updateBroadPhaseState() { + // For all the proxy collision shapes of the body + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.getNext()) { + // Update the proxy + updateProxyShapeInBroadPhase(shape, false); + } + } + + /** + * @brief Update the broad-phase state of a proxy collision shape of the body + */ + public void updateProxyShapeInBroadPhase(final ProxyShape _proxyShape, final boolean _forceReinsert /* false */) { + final AABB aabb = new AABB(); + _proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(_proxyShape.getLocalToBodyTransform())); + this.world.getCollisionDetection().updateProxyCollisionShape(_proxyShape, aabb, new Vector3f(0, 0, 0), _forceReinsert); + } +} diff --git a/src/org/atriasoft/ephysics/body/RigidBody.java b/src/org/atriasoft/ephysics/body/RigidBody.java new file mode 100644 index 0000000..683b5e9 --- /dev/null +++ b/src/org/atriasoft/ephysics/body/RigidBody.java @@ -0,0 +1,574 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.body; + +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.shapes.AABB; +import org.atriasoft.ephysics.collision.shapes.CollisionShape; +import org.atriasoft.ephysics.constraint.Joint; +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; + +/** + * This class represents a rigid body of the physics + * engine. A rigid body is a non-deformable body that + * has a ant mass. This class inherits from the + * CollisionBody class. + */ +public class RigidBody extends CollisionBody { + + protected float initMass = 1.0f; //!< Intial mass of the body + protected Vector3f centerOfMassLocal = new Vector3f(0, 0, 0); //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin + public Vector3f centerOfMassWorld = new Vector3f(); //!< Center of mass of the body in world-space coordinates + public Vector3f linearVelocity = new Vector3f(); //!< Linear velocity of the body + public Vector3f angularVelocity = new Vector3f(); //!< Angular velocity of the body + public Vector3f externalForce = new Vector3f(); //!< Current external force on the body + public Vector3f externalTorque = new Vector3f(); //!< Current external torque on the body + public Matrix3f inertiaTensorLocal = new Matrix3f(); //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body + public Matrix3f inertiaTensorLocalInverse = new Matrix3f(); //!< Inverse of the inertia tensor of the body + public float massInverse; //!< Inverse of the mass of the body + protected boolean isGravityEnabled = true; //!< True if the gravity needs to be applied to this rigid body + protected Material material = new Material(); //!< Material properties of the rigid body + protected float linearDamping = 0.0f; //!< Linear velocity damping factor + protected float angularDamping = 0.0f; //!< Angular velocity damping factor + public JointListElement jointsList = null; //!< First element of the linked list of joints involving this body + protected boolean angularReactionEnable = true; + + /** + * Constructor + * @param transform The transformation of the body + * @param world The world where the body has been added + * @param id The ID of the body + */ + public RigidBody(final Transform3D transform, final CollisionWorld world, final int id) { + super(transform, world, id); + this.centerOfMassWorld = new Vector3f(transform.getPosition()); + this.massInverse = 1.0f / this.initMass; + } + + /** + * Add a collision shape to the body. + * When you add a collision shape to the body, an intternal copy of this collision shape will be created internally. + * Therefore, you can delete it right after calling this method or use it later to add it to another body. + * This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. + * You can use the returned proxy shape to get and set information about the corresponding collision shape for that body. + * @param _collisionShape The collision shape you want to add to the body + * @param _transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body + * @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) { + 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); + // Add it to the list of proxy collision shapes of the body + if (this.proxyCollisionShapes == null) { + this.proxyCollisionShapes = proxyShape; + } else { + proxyShape.next = this.proxyCollisionShapes; + this.proxyCollisionShapes = proxyShape; + } + // Compute the world-space AABB of the new collision shape + final AABB aabb = new AABB(); + _collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform)); + // Notify the collision detection about this new collision shape + this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb); + this.numberCollisionShapes++; + recomputeMassInformation(); + return proxyShape; + } + + /** + * Apply an external force to the body at a given point (in world-space coordinates). + * If the point is not at the center of mass of the body, it will also + * generate some torque and therefore, change the angular velocity of the body. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied forces and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param _force The force to apply on the body + * @param _point The point where the force is applied (in world-space coordinates) + */ + public void applyForce(final Vector3f _force, final Vector3f _point) { + if (this.type != BodyType.DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalForce.add(_force); + this.externalTorque.add(_point.lessNew(this.centerOfMassWorld).cross(_force)); + } + + /** + * Apply an external force to the body at its center of mass. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied forces and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param _force The external force to apply on the center of mass of the body + */ + public void applyForceToCenterOfMass(final Vector3f _force) { + if (this.type != BodyType.DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalForce.add(_force); + } + + /** + * Apply an external torque to the body. + * If the body is sleeping, calling this method will wake it up. Note that the + * force will we added to the sum of the applied torques and that this sum will be + * reset to zero at the end of each call of the DynamicsWorld::update() method. + * You can only apply a force to a dynamic body otherwise, this method will do nothing. + * @param _torque The external torque to apply on the body + */ + public void applyTorque(final Vector3f _torque) { + if (this.type != BodyType.DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalTorque.add(_torque); + } + + /** + * Set the variable to know if the gravity is applied to this rigid body + * @param _isEnabled True if you want the gravity to be applied to this body + */ + public void enableGravity(final boolean _isEnabled) { + this.isGravityEnabled = _isEnabled; + } + + /** + * Get the angular velocity damping factor + * @return The angular damping factor of this body + */ + public float getAngularDamping() { + return this.angularDamping; + } + + /** + * Get the angular velocity of the body + * @return The angular velocity vector of the body + */ + public Vector3f getAngularVelocity() { + return this.angularVelocity; + } + + /** + * Get the inverse of the inertia tensor in world coordinates. + * The inertia tensor I_w in world coordinates is computed with the + * local inverse inertia tensor I_b^-1 in body coordinates + * by I_w = R * I_b^-1 * R^T + * where R is the rotation matrix (and R^T its transpose) of the + * current orientation quaternion of the body + * @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates + */ + public Matrix3f getInertiaTensorInverseWorld() { + // 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().multiplyNew(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transposeNew()); + //Log.error("}}} tmp=" + tmp); + return tmp; + } + + /** + * Get the local inertia tensor of the body (in local-space coordinates) + * @return The 3x3 inertia tensor matrix of the body + */ + public Matrix3f getInertiaTensorLocal() { + return this.inertiaTensorLocal; + } + + /** + * Get the inertia tensor in world coordinates. + * The inertia tensor I_w in world coordinates is computed + * with the local inertia tensor I_b in body coordinates + * by I_w = R * I_b * R^T + * where R is the rotation matrix (and R^T its transpose) of + * the current orientation quaternion of the body + * @return The 3x3 inertia tensor matrix of the body in world-space coordinates + */ + public Matrix3f getInertiaTensorWorld() { + // Compute and return the inertia tensor in world coordinates + return this.transform.getOrientation().getMatrix().multiplyNew(this.inertiaTensorLocal).multiply(this.transform.getOrientation().getMatrix().transposeNew()); + } + + /** + * Get the first element of the linked list of joints involving this body + * @return The first element of the linked-list of all the joints involving this body + */ + public JointListElement getJointsList() { + return this.jointsList; + } + + /** + * Get the linear velocity damping factor + * @return The linear damping factor of this body + */ + public float getLinearDamping() { + return this.linearDamping; + } + + /** + * Get the linear velocity + * @return The linear velocity vector of the body + */ + public Vector3f getLinearVelocity() { + return this.linearVelocity; + } + + /** + * Get the mass of the body + * @return The mass (in kilograms) of the body + */ + public float getMass() { + return this.initMass; + } + + /** + * get a reference to the material properties of the rigid body + * @return A reference to the material of the body + */ + public Material getMaterial() { + return this.material; + } + + public boolean isAngularReactionEnable() { + return this.angularReactionEnable; + } + + /** + * get the need of gravity appling to this rigid body + * @return True if the gravity is applied to the body + */ + public boolean isGravityEnabled() { + return this.isGravityEnabled; + } + + /** + * Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body. + */ + public void recomputeMassInformation() { + this.initMass = 0.0f; + this.massInverse = 0.0f; + this.inertiaTensorLocal.setZero(); + this.inertiaTensorLocalInverse.setZero(); + this.centerOfMassLocal.setZero(); + // If it is STATIC or KINEMATIC body + if (this.type == BodyType.STATIC || this.type == BodyType.KINEMATIC) { + this.centerOfMassWorld = this.transform.getPosition(); + return; + } + assert (this.type == BodyType.DYNAMIC); + // Compute the total mass of the body + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + this.initMass += shape.getMass(); + this.centerOfMassLocal.add(shape.getLocalToBodyTransform().getPosition().multiplyNew(shape.getMass())); + } + if (this.initMass > 0.0f) { + this.massInverse = 1.0f / this.initMass; + } else { + this.initMass = 1.0f; + this.massInverse = 1.0f; + } + // Compute the center of mass + final Vector3f oldCenterOfMass = this.centerOfMassWorld; + this.centerOfMassLocal.multiply(this.massInverse); + this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal); + // Compute the total mass and inertia tensor using all the collision shapes + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + // Get the inertia tensor of the collision shape in its local-space + Matrix3f inertiaTensor = new Matrix3f(); + shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass()); + // Convert the collision shape inertia tensor into the local-space of the body + final Transform3D shapeTransform = shape.getLocalToBodyTransform(); + final Matrix3f rotationMatrix = shapeTransform.getOrientation().getMatrix(); + inertiaTensor = rotationMatrix.multiplyNew(inertiaTensor).multiply(rotationMatrix.transposeNew()); + // Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape + // center into a inertia tensor w.r.t to the body origin. + final Vector3f offset = shapeTransform.getPosition().lessNew(this.centerOfMassLocal); + final float offsetSquare = offset.length2(); + final Vector3f off1 = offset.multiplyNew(-offset.x); + final Vector3f off2 = offset.multiplyNew(-offset.y); + final Vector3f off3 = offset.multiplyNew(-offset.z); + final 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.multiply(shape.getMass()); + this.inertiaTensorLocal.add(inertiaTensor.addNew(offsetMatrix)); + } + // Compute the local inverse inertia tensor + this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew(); + // Update the linear velocity of the center of mass + this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass))); + } + + @Override + public void removeCollisionShape(final ProxyShape _proxyShape) { + super.removeCollisionShape(_proxyShape); + recomputeMassInformation(); + } + + /** + * Remove a joint from the joints list + */ + public void removeJointFromjointsList(final Joint joint) { + assert (joint != null); + assert (this.jointsList != null); + // Remove the joint from the linked list of the joints of the first body + if (this.jointsList.joint == joint) { // If the first element is the one to remove + JointListElement elementToRemove = this.jointsList; + this.jointsList = elementToRemove.next; + elementToRemove = null; + } else { // If the element to remove is not the first one in the list + JointListElement currentElement = this.jointsList; + while (currentElement.next != null) { + if (currentElement.next.joint == joint) { + JointListElement elementToRemove = currentElement.next; + currentElement.next = elementToRemove.next; + elementToRemove = null; + break; + } + currentElement = currentElement.next; + } + } + } + + /** + * Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation. + * @param _angularDamping The angular damping factor of this body + */ + public void setAngularDamping(final float _angularDamping) { + assert (_angularDamping >= 0.0f); + this.angularDamping = _angularDamping; + } + + public void setAngularReactionEnable(final boolean angularReactionEnable) { + this.angularReactionEnable = angularReactionEnable; + } + + /** + * Set the angular velocity. + * @param _angularVelocity The angular velocity vector of the body + */ + public void setAngularVelocity(final Vector3f _angularVelocity) { + if (this.type == BodyType.STATIC) { + return; + } + this.angularVelocity = _angularVelocity; + if (this.angularVelocity.length2() > 0.0f) { + setIsSleeping(false); + } + } + + /** + * Set the local center of mass of the body (in local-space coordinates) + * @param _centerOfMassLocal The center of mass of the body in local-space coordinates + */ + public void setCenterOfMassLocal(final Vector3f centerOfMassLocal) { + if (this.type != BodyType.DYNAMIC) { + return; + } + final Vector3f oldCenterOfMass = this.centerOfMassWorld; + this.centerOfMassLocal = centerOfMassLocal; + this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal); + this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass))); + } + + /** + * Set the local inertia tensor of the body (in local-space coordinates) + * @param _inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates + */ + public void setInertiaTensorLocal(final Matrix3f inertiaTensorLocal) { + if (this.type != BodyType.DYNAMIC) { + return; + } + this.inertiaTensorLocal = inertiaTensorLocal; + this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew(); + } + + /** + * Set the variable to know whether or not the body is sleeping + * @param _isSleeping New sleeping state of the body + */ + @Override + public void setIsSleeping(final boolean _isSleeping) { + if (_isSleeping) { + this.linearVelocity.setZero(); + this.angularVelocity.setZero(); + this.externalForce.setZero(); + this.externalTorque.setZero(); + } + super.setIsSleeping(_isSleeping); + } + + /** + * Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation. + * @param _linearDamping The linear damping factor of this body + */ + public void setLinearDamping(final float _linearDamping) { + assert (_linearDamping >= 0.0f); + this.linearDamping = _linearDamping; + } + + /** + * Set the linear velocity of the rigid body. + * @param _linearVelocity Linear velocity vector of the body + */ + public void setLinearVelocity(final Vector3f _linearVelocity) { + if (this.type == BodyType.STATIC) { + return; + } + this.linearVelocity = _linearVelocity; + if (this.linearVelocity.length2() > 0.0f) { + setIsSleeping(false); + } + } + + /** + * Set the mass of the rigid body + * @param _mass The mass (in kilograms) of the body + */ + public void setMass(final float _mass) { + if (this.type != BodyType.DYNAMIC) { + return; + } + this.initMass = _mass; + if (this.initMass > 0.0f) { + this.massInverse = 1.0f / this.initMass; + } else { + this.initMass = 1.0f; + this.massInverse = 1.0f; + } + } + + /** + * Set a new material for this rigid body + * @param _material The material you want to set to the body + */ + public void setMaterial(final Material _material) { + this.material = _material; + } + + /** + * Set the current position and orientation + * @param _transform The transformation of the body that transforms the local-space of the body into world-space + */ + @Override + public void setTransform(final Transform3D _transform) { + //Log.info(" set transform: " + this.transform + " ==> " + _transform); + if (_transform.getPosition().x == Float.NaN) { + Log.critical(" set transform: " + this.transform + " ==> " + _transform); + } + if (Float.isInfinite(_transform.getOrientation().z)) { + Log.critical(" set transform: " + this.transform + " ==> " + _transform); + } + this.transform = _transform; + final Vector3f oldCenterOfMass = this.centerOfMassWorld; + // Compute the new center of mass in world-space coordinates + this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal); + // Update the linear velocity of the center of mass + this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass))); + updateBroadPhaseState(); + } + + @Override + public void setType(final BodyType _type) { + if (this.type == _type) { + return; + } + super.setType(_type); + recomputeMassInformation(); + if (this.type == BodyType.STATIC) { + // Reset the velocity to zero + this.linearVelocity.setZero(); + this.angularVelocity.setZero(); + } + if (this.type == BodyType.STATIC || this.type == BodyType.KINEMATIC) { + // Reset the inverse mass and inverse inertia tensor to zero + this.massInverse = 0.0f; + this.inertiaTensorLocal.setZero(); + this.inertiaTensorLocalInverse.setZero(); + } else { + this.massInverse = 1.0f / this.initMass; + this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew(); + } + setIsSleeping(false); + resetContactManifoldsList(); + // Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved) + askForBroadPhaseCollisionCheck(); + this.externalForce.setZero(); + this.externalTorque.setZero(); + } + + @Override + public void updateBroadPhaseState() { + final DynamicsWorld world = (DynamicsWorld) this.world; + final Vector3f displacement = this.linearVelocity.multiplyNew(world.timeStep); + + // For all the proxy collision shapes of the body + for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) { + // Recompute the world-space AABB of the collision shape + final AABB aabb = new AABB(); + //Log.info(" : " + aabb.getMin() + " " + aabb.getMax()); + //Log.info(" this.transform: " + this.transform); + shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.getLocalToBodyTransform())); + //Log.info(" : " + aabb.getMin() + " " + aabb.getMax()); + // Update the broad-phase state for the proxy collision shape + //Log.warning(" ==> updateProxyCollisionShape"); + world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); + } + } + + /** + * Update the transform of the body after a change of the center of mass + */ + public void updateTransformWithCenterOfMass() { + // Translate the body according to the translation of the center of mass position + this.transform.setPosition(this.centerOfMassWorld.lessNew(this.transform.getOrientation().multiply(this.centerOfMassLocal))); + if (Float.isNaN(this.transform.getPosition().x) == true) { + Log.critical("updateTransformWithCenterOfMass: " + this.transform); + } + if (Float.isInfinite(this.transform.getOrientation().z) == true) { + Log.critical(" set transform: " + this.transform); + } + } + +} diff --git a/src/org/atriasoft/ephysics/collision/CollisionDetection.java b/src/org/atriasoft/ephysics/collision/CollisionDetection.java new file mode 100644 index 0000000..3356833 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/CollisionDetection.java @@ -0,0 +1,497 @@ +package org.atriasoft.ephysics.collision; + +import java.util.Iterator; +import java.util.Map; +import java.util.TreeMap; + +import org.atriasoft.ephysics.RaycastCallback; +import org.atriasoft.ephysics.RaycastTest; +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.collision.broadphase.BroadPhaseAlgorithm; +import org.atriasoft.ephysics.collision.broadphase.DTree; +import org.atriasoft.ephysics.collision.broadphase.PairDTree; +import org.atriasoft.ephysics.collision.narrowphase.CollisionDispatch; +import org.atriasoft.ephysics.collision.narrowphase.DefaultCollisionDispatch; +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseAlgorithm; +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback; +import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm; +import org.atriasoft.ephysics.collision.shapes.AABB; +import org.atriasoft.ephysics.collision.shapes.CollisionShape; +import org.atriasoft.ephysics.collision.shapes.CollisionShapeType; +import org.atriasoft.ephysics.constraint.ContactPoint; +import org.atriasoft.ephysics.constraint.ContactPointInfo; +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; + +/* + * @brief It computes the collision detection algorithms. We first + * perform a broad-phase algorithm to know which pairs of bodies can + * collide and then we run a narrow-phase algorithm to compute the + * collision contacts between bodies. + */ +public class CollisionDetection implements NarrowPhaseCallback { + /// Reference on the physics world + private final CollisionWorld world; + /// Broad-phase algorithm + private final BroadPhaseAlgorithm broadPhaseAlgorithm; + /// Collision Detection Dispatch configuration + private CollisionDispatch collisionDispatch; + private final DefaultCollisionDispatch defaultCollisionDispatch = new DefaultCollisionDispatch(); + /// Collision detection matrix (algorithms to use) + private final NarrowPhaseAlgorithm[][] collisionMatrix = new NarrowPhaseAlgorithm[CollisionShapeType.NB_COLLISION_SHAPE_TYPES][CollisionShapeType.NB_COLLISION_SHAPE_TYPES]; + public Map overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs + private final Map contactOverlappingPair = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Overlapping pairs in contact (during the current Narrow-phase collision detection) + // TODO : Delete this + private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm + private final SetMultiple noCollisionPairs = new SetMultiple(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other + private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously + + /// Constructor + public CollisionDetection(final CollisionWorld _world) { + this.world = _world; + this.broadPhaseAlgorithm = new BroadPhaseAlgorithm(this); + this.isCollisionShapesAdded = false; + this.narrowPhaseGJKAlgorithm = new GJKAlgorithm(this); + // Set the default collision dispatch configuration + setCollisionDispatch(this.defaultCollisionDispatch); + // Fill-in the collision detection matrix with algorithms + fillInCollisionMatrix(); + } + + /// Add all the contact manifold of colliding pairs to their bodies + private void addAllContactManifoldsToBodies() { + // For each overlapping pairs in contact during the narrow-phase + for (final OverlappingPair it : this.contactOverlappingPair.values()) { + // Add all the contact manifolds of the pair int32_to the list of contact manifolds + // of the two bodies involved in the contact + addContactManifoldToBody(it); + } + } + + /// Add a contact manifold to the linked list of contact manifolds of the two bodies + /// involed in the corresponding contact. + private void addContactManifoldToBody(final OverlappingPair _pair) { + assert (_pair != null); + final CollisionBody body1 = _pair.getShape1().getBody(); + final CollisionBody body2 = _pair.getShape2().getBody(); + final ContactManifoldSet manifoldSet = _pair.getContactManifoldSet(); + // For each contact manifold in the set of manifolds in the pair + for (int i = 0; i < manifoldSet.getNbContactManifolds(); i++) { + final ContactManifold contactManifold = manifoldSet.getContactManifold(i); + assert (contactManifold.getNbContactPoints() > 0); + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + body1.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList); + + // Add the contact manifold at the beginning of the linked + // list of the contact manifolds of the second body + body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList); + + } + } + + /// Add a pair of bodies that cannot collide with each other + public void addNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) { + this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(_body1, _body2)); + } + + /// Add a proxy collision shape to the collision detection + public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) { + // Add the body to the broad-phase + this.broadPhaseAlgorithm.addProxyCollisionShape(_proxyShape, _aabb); + this.isCollisionShapesAdded = true; + } + + // Ask for a collision shape to be tested again during broad-phase. + /// We simply put the shape in the list of collision shape that have moved in the + /// previous frame so that it is tested for collision again in the broad-phase. + public void askForBroadPhaseCollisionCheck(final ProxyShape _shape) { + this.broadPhaseAlgorithm.addMovedCollisionShape(_shape.broadPhaseID); + } + + /// Allow the broadphase to notify the collision detection about an overlapping pair. + /// This method is called by the broad-phase collision detection algorithm + public void broadPhaseNotifyOverlappingPair(final ProxyShape _shape1, final ProxyShape _shape2) { + assert (_shape1.broadPhaseID != _shape2.broadPhaseID); + // If the two proxy collision shapes are from the same body, skip it + if (_shape1.getBody().getID() == _shape2.getBody().getID()) { + return; + } + //Log.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 ..."); + return; + } + // Compute the overlapping pair ID + final PairDTree pairID = OverlappingPair.computeID(_shape1, _shape2); + // Check if the overlapping pair already exists + if (this.overlappingPairs.get(pairID) != null) { + return; + } + // Compute the maximum number of contact manifolds for this pair + final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(_shape1.getCollisionShape().getType(), _shape2.getCollisionShape().getType()); + // Create the overlapping pair and add it int32_to the set of overlapping pairs + final OverlappingPair newPair = new OverlappingPair(_shape1, _shape2, nbMaxManifolds); + assert (newPair != null); + this.overlappingPairs.putIfAbsent(pairID, newPair); + // Wake up the two bodies + _shape1.getBody().setIsSleeping(false); + _shape2.getBody().setIsSleeping(false); + } + + /// Delete all the contact points in the currently overlapping pairs + private void clearContactPoints() { + // For each overlapping pair + this.overlappingPairs.forEach((key, value) -> value.clearContactPoints()); + } + + /// Compute the broad-phase collision detection + private void computeBroadPhase() { + // If new collision shapes have been added to bodies + if (this.isCollisionShapesAdded) { + // Ask the broad-phase to recompute the overlapping pairs of collision + // shapes. This call can only add new overlapping pairs in the collision + // detection. + this.broadPhaseAlgorithm.computeOverlappingPairs(); + } + } + + /// Compute the collision detection + public void computeCollisionDetection() { + //Log.info("computeBroadPhase();"); + // Compute the broad-phase collision detection + computeBroadPhase(); + //Log.info("computeNarrowPhase();"); + // Compute the narrow-phase collision detection + computeNarrowPhase(); + } + + /// Compute the narrow-phase collision detection + private void computeNarrowPhase() { + // Clear the set of overlapping pairs in narrow-phase contact + this.contactOverlappingPair.clear(); + { + //Log.info("list elements:"); + final Iterator> ittt = this.overlappingPairs.entrySet().iterator(); + while (ittt.hasNext()) { + final Map.Entry entry = ittt.next(); + //Log.info(" " + entry.getKey() + " " + entry.getValue()); + + } + } + // For each possible collision pair of bodies + final Iterator> it = this.overlappingPairs.entrySet().iterator(); + + while (it.hasNext()) { + final Map.Entry entry = it.next(); + final OverlappingPair pair = entry.getValue(); + final ProxyShape shape1 = pair.getShape1(); + final ProxyShape shape2 = pair.getShape2(); + assert (shape1.broadPhaseID != shape2.broadPhaseID); + // 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) + || !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 + it.remove(); + continue; + } + final CollisionBody body1 = shape1.getBody(); + final CollisionBody body2 = shape2.getBody(); + // Update the contact cache of the overlapping pair + pair.update(); + // Check that at least one body is awake and not static + final boolean isBody1Active = !body1.isSleeping() && body1.getType() != BodyType.STATIC; + final boolean isBody2Active = !body2.isSleeping() && body2.getType() != BodyType.STATIC; + if (!isBody1Active && !isBody2Active) { + continue; + } + // Check if the bodies are in the set of bodies that cannot collide between each other + final PairInt bodiesIndex = OverlappingPair.computeBodiesIndexPair(body1, body2); + if (this.noCollisionPairs.count(bodiesIndex) > 0) { + continue; + } + // Select the narrow phase algorithm to use according to the two collision shapes + final CollisionShapeType shape1Type = shape1.getCollisionShape().getType(); + final CollisionShapeType shape2Type = shape2.getCollisionShape().getType(); + final NarrowPhaseAlgorithm narrowPhaseAlgorithm = this.collisionMatrix[shape1Type.value][shape2Type.value]; + // If there is no collision algorithm between those two kinds of shapes + if (narrowPhaseAlgorithm == null) { + continue; + } + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); + // Create the CollisionShapeInfo objects + final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); + + final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); + + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision. If a collision occurs, the + // notifyContact() callback method will be called. + narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this); + } + + // Add all the contact manifolds (between colliding bodies) to the bodies + addAllContactManifoldsToBodies(); + + } + + /// Compute the narrow-phase collision detection + public void computeNarrowPhaseBetweenShapes(final CollisionCallback _callback, final Set _shapes1, final Set _shapes2) { + this.contactOverlappingPair.clear(); + // For each possible collision pair of bodies + final Iterator> it = this.overlappingPairs.entrySet().iterator(); + while (it.hasNext()) { + final Map.Entry entry = it.next(); + final OverlappingPair pair = entry.getValue(); + 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) + && (_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) { + continue; + } + 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) + || !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 + it.remove(); + continue; + } + final CollisionBody body1 = shape1.getBody(); + final CollisionBody body2 = shape2.getBody(); + // Update the contact cache of the overlapping pair + pair.update(); + // Check if the two bodies are allowed to collide, otherwise, we do not test for collision + if (body1.getType() != BodyType.DYNAMIC && body2.getType() != BodyType.DYNAMIC) { + continue; + } + final PairInt bodiesIndex = OverlappingPair.computeBodiesIndexPair(body1, body2); + if (this.noCollisionPairs.count(bodiesIndex) > 0) { + continue; + } + // Check if the two bodies are sleeping, if so, we do no test collision between them + if (body1.isSleeping() && body2.isSleeping()) { + continue; + } + // Select the narrow phase algorithm to use according to the two collision shapes + final CollisionShapeType shape1Type = shape1.getCollisionShape().getType(); + final CollisionShapeType shape2Type = shape2.getCollisionShape().getType(); + final NarrowPhaseAlgorithm narrowPhaseAlgorithm = this.collisionMatrix[shape1Type.value][shape2Type.value]; + // If there is no collision algorithm between those two kinds of shapes + if (narrowPhaseAlgorithm == null) { + continue; + } + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); + // Create the CollisionShapeInfo objects + final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData()); + + final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData()); + + final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(_callback); + // Use the narrow-phase collision detection algorithm to check + // if there really is a collision + narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback); + } + + // Add all the contact manifolds (between colliding bodies) to the bodies + addAllContactManifoldsToBodies(); + } + + void createContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) { + // Create a new contact + final ContactPoint contact = new ContactPoint(_contactInfo); + // Add the contact to the contact manifold set of the corresponding overlapping pair + _overlappingPair.addContact(contact); + // Add the overlapping pair int32_to the set of pairs in contact during narrow-phase + final PairDTree pairId = OverlappingPair.computeID(_overlappingPair.getShape1(), _overlappingPair.getShape2()); + this.contactOverlappingPair.put(pairId, _overlappingPair); + } + + /// Fill-in the collision detection matrix + private void fillInCollisionMatrix() { + // For each possible type of collision shape + for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) { + for (int j = 0; j < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; j++) { + this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i), CollisionShapeType.getType(j)); + } + } + } + + /// Return the Narrow-phase collision detection algorithm to use between two types of shapes + public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType _shape1Type, final CollisionShapeType _shape2Type) { + return this.collisionMatrix[_shape1Type.value][_shape2Type.value]; + } + + public GJKAlgorithm getNarrowPhaseGJKAlgorithm() { + return this.narrowPhaseGJKAlgorithm; + } + + /// Return a pointer to the world + public CollisionWorld getWorld() { + return this.world; + } + + /// Return the world event listener + public EventListener getWorldEventListener() { + return this.world.eventListener; + } + + /// Called by a narrow-phase collision algorithm when a new contact has been found + @Override + public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) { + Log.error("Notify Contact ... --------------------"); + // If it is the first contact since the pairs are overlapping + if (_overlappingPair.getNbContactPoints() == 0) { + // Trigger a callback event + if (this.world.eventListener != null) { + this.world.eventListener.beginContact(_contactInfo); + } + } + // Create a new contact + createContact(_overlappingPair, _contactInfo); + // Trigger a callback event for the new contact + if (this.world.eventListener != null) { + this.world.eventListener.newContact(_contactInfo); + } + } + + /// Ray casting method + public void raycast(final RaycastCallback _raycastCallback, final Ray _ray, final int _raycastWithCategoryMaskBits) { + final RaycastTest rayCastTest = new RaycastTest(_raycastCallback); + // Ask the broad-phase algorithm to call the testRaycastAgainstShape() + // callback method for each proxy shape hit by the ray in the broad-phase + this.broadPhaseAlgorithm.raycast(_ray, rayCastTest, _raycastWithCategoryMaskBits); + } + + /// Remove a pair of bodies that cannot collide with each other + public void removeNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) { + this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(_body1, _body2))); + } + + /// Remove a proxy collision shape from the collision detection + public void removeProxyCollisionShape(final ProxyShape _proxyShape) { + // Remove all the overlapping pairs involving this proxy shape + final Iterator> it = this.overlappingPairs.entrySet().iterator(); + while (it.hasNext()) { + final Map.Entry entry = it.next(); + final OverlappingPair pair = entry.getValue(); + if (pair.getShape1().broadPhaseID == _proxyShape.broadPhaseID || pair.getShape2().broadPhaseID == _proxyShape.broadPhaseID) { + // 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(); + } + } + // Remove the body from the broad-phase + this.broadPhaseAlgorithm.removeProxyCollisionShape(_proxyShape); + } + + /// Report collision between two sets of shapes + public void reportCollisionBetweenShapes(final CollisionCallback _callback, final Set _shapes1, final Set _shapes2) { + + // For each possible collision pair of bodies + final Iterator> it = this.overlappingPairs.entrySet().iterator(); + while (it.hasNext()) { + final Map.Entry entry = it.next(); + 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) + && (_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) { + continue; + } + 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 + final ContactManifoldSet manifoldSet = pair.getContactManifoldSet(); + for (int j = 0; j < manifoldSet.getNbContactManifolds(); j++) { + final ContactManifold manifold = manifoldSet.getContactManifold(j); + // For each contact manifold of the manifold set + 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(), + contactPoint.getLocalPointOnBody2()); + // Notify the collision callback about this new contact + if (_callback != null) { + _callback.notifyContact(contactInfo); + } + } + + } + } + } + + /// Set the collision dispatch configuration + public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) { + this.collisionDispatch = _collisionDispatch; + this.collisionDispatch.init(this); + // Fill-in the collision matrix with the new algorithms to use + fillInCollisionMatrix(); + } + + /// Test if the AABBs of two proxy shapes overlap + public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) { + // If one of the shape's body is not active, we return no overlap + if (!_shape1.getBody().isActive() || !_shape2.getBody().isActive()) { + return false; + } + return this.broadPhaseAlgorithm.testOverlappingShapes(_shape1, _shape2); + } + + /// Compute the collision detection + public void testCollisionBetweenShapes(final CollisionCallback _callback, final Set _shapes1, final Set _shapes2) { + // Compute the broad-phase collision detection + computeBroadPhase(); + // Delete all the contact points in the currently overlapping pairs + clearContactPoints(); + // Compute the narrow-phase collision detection among given sets of shapes + computeNarrowPhaseBetweenShapes(_callback, _shapes1, _shapes2); + } + + /// Update a proxy collision shape (that has moved for instance) + public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb) { + updateProxyCollisionShape(_shape, _aabb, new Vector3f(0, 0, 0), false); + } + + public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement) { + updateProxyCollisionShape(_shape, _aabb, _displacement, false); + } + + public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) { + this.broadPhaseAlgorithm.updateProxyCollisionShape(_shape, _aabb, _displacement); + } +} diff --git a/src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java b/src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java new file mode 100644 index 0000000..3e4fbe7 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java @@ -0,0 +1,30 @@ +package org.atriasoft.ephysics.collision; + +import org.atriasoft.etk.math.Transform3D; + +import org.atriasoft.ephysics.collision.shapes.CollisionShape; +import org.atriasoft.ephysics.engine.OverlappingPair; + +/** + * It regroups different things about a collision shape. This is + * used to pass information about a collision shape to a collision algorithm. + */ +public class CollisionShapeInfo { + + public final OverlappingPair overlappingPair; //!< Broadphase overlapping pair + public final ProxyShape proxyShape; //!< Proxy shape + public final CollisionShape collisionShape; //!< Pointer to the collision shape + public final Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space + public final Object cachedCollisionData; //!< Cached collision data of the proxy shape + /// Constructor + + public CollisionShapeInfo(final ProxyShape _proxyCollisionShape, final CollisionShape _shape, final Transform3D _shapeLocalToWorldTransform, final OverlappingPair _pair, + final Object _cachedData) { + this.overlappingPair = _pair; + this.proxyShape = _proxyCollisionShape; + this.collisionShape = _shape; + this.shapeToWorldTransform = _shapeLocalToWorldTransform; + this.cachedCollisionData = _cachedData; + + } +} diff --git a/src/org/atriasoft/ephysics/collision/ContactManifold.java b/src/org/atriasoft/ephysics/collision/ContactManifold.java new file mode 100644 index 0000000..c0525d3 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/ContactManifold.java @@ -0,0 +1,374 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ + +package org.atriasoft.ephysics.collision; + +import org.atriasoft.ephysics.Configuration; +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.constraint.ContactPoint; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents the set of contact points between two bodies. + * The contact manifold is implemented in a way to cache the contact + * points among the frames for better stability following the + * "Contact Generation" presentation of Erwin Coumans at GDC 2010 + * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). + * Some code of this class is based on the implementation of the + * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). + * The contacts between two bodies are added one after the other in the cache. + * When the cache is full, we have to remove one point. The idea is to keep + * the point with the deepest penetration depth and also to keep the + * points producing the larger area (for a more stable contact manifold). + * The new added point is always kept. + */ +public class ContactManifold { + + //!< Maximum number of contacts in the manifold + public static final int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;; + + private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact + + private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact + private final ContactPoint[] contactPoints = new ContactPoint[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact points in the manifold + private final int normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction) + private int nbContactPoints; //!< Number of contacts in the cache + private Vector3f frictionVector1 = new Vector3f(); //!< First friction vector of the contact manifold + private Vector3f frictionvec2 = new Vector3f(); //!< Second friction vector of the contact manifold + private float frictionImpulse1; //!< First friction raint accumulated impulse + + private float frictionImpulse2; //!< Second friction raint accumulated impulse + private float frictionTwistImpulse; //!< Twist friction raint accumulated impulse + private Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse + public boolean isAlreadyInIsland; //!< True if the contact manifold has already been added longo an island + /// Return the index of maximum area + /// Constructor + + public ContactManifold(final ProxyShape _shape1, final ProxyShape _shape2, final int _normalDirectionId) { + this.shape1 = _shape1; + this.shape2 = _shape2; + this.normalDirectionId = _normalDirectionId; + this.nbContactPoints = 0; + this.frictionImpulse1 = 0.0f; + this.frictionImpulse2 = 0.0f; + this.frictionTwistImpulse = 0.0f; + this.isAlreadyInIsland = false; + + } + + /// Add a contact point to the manifold + public void addContactPoint(final ContactPoint contact) { + // For contact already in the manifold + for (int iii = 0; iii < this.nbContactPoints; iii++) { + // Check if the new point point does not correspond to a same contact point + // already in the manifold. + final float distance = (this.contactPoints[iii].getWorldPointOnBody1().lessNew(contact.getWorldPointOnBody1()).length2()); + if (distance <= Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD) { + assert (this.nbContactPoints > 0); + return; + } + } + // If the contact manifold is full + if (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { + final int indexMaxPenetration = getIndexOfDeepestPenetration(contact); + final int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1()); + removeContactPoint(indexToRemove); + } + // Add the new contact point in the manifold + this.contactPoints[this.nbContactPoints] = contact; + this.nbContactPoints++; + assert (this.nbContactPoints > 0); + } + + /// Clear the contact manifold + public void clear() { + for (int iii = 0; iii < this.nbContactPoints; ++iii) { + this.contactPoints[iii] = null; + } + this.nbContactPoints = 0; + } + + /// Return the normalized averaged normal vector + public Vector3f getAverageContactNormal() { + if (this.nbContactPoints == 0) { + return new Vector3f(0, 0, 1); + } + final Vector3f averageNormal = new Vector3f(); + for (int iii = 0; iii < this.nbContactPoints; iii++) { + averageNormal.add(this.contactPoints[iii].getNormal()); + } + return averageNormal.safeNormalizeNew(); + } + + /// Return a pointer to the first body of the contact manifold + public CollisionBody getBody1() { + return this.shape1.getBody(); + } + + /// Return a pointer to the second body of the contact manifold + public CollisionBody getBody2() { + return this.shape2.getBody(); + } + + /// Return a contact point of the manifold + public ContactPoint getContactPoint(final int index) { + assert (index < this.nbContactPoints); + return this.contactPoints[index]; + } + + /// Return the first friction accumulated impulse + public float getFrictionImpulse1() { + return this.frictionImpulse1; + } + + /// Return the second friction accumulated impulse + public float getFrictionImpulse2() { + return this.frictionImpulse2; + } + + /// Return the friction twist accumulated impulse + public float getFrictionTwistImpulse() { + return this.frictionTwistImpulse; + } + + /// Return the second friction vector at the center of the contact manifold + public Vector3f getFrictionvec2() { + return this.frictionvec2; + } + + /// Return the first friction vector at the center of the contact manifold + public Vector3f getFrictionVector1() { + return this.frictionVector1; + } + + /** + * Return the index of the contact with the larger penetration depth. + * + * This corresponding contact will be kept in the cache. The method returns -1 is + * the new contact is the deepest. + */ + int getIndexOfDeepestPenetration(final ContactPoint newContact) { + assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + int indexMaxPenetrationDepth = -1; + float maxPenetrationDepth = newContact.getPenetrationDepth(); + // For each contact in the cache + for (int iii = 0; iii < this.nbContactPoints; iii++) { + // If the current contact has a larger penetration depth + if (this.contactPoints[iii].getPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = this.contactPoints[iii].getPenetrationDepth(); + indexMaxPenetrationDepth = iii; + } + } + // Return the index of largest penetration depth + return indexMaxPenetrationDepth; + } + + /** + * Return the index that will be removed. + * The index of the contact point with the larger penetration + * depth is given as a parameter. This contact won't be removed. Given this contact, we compute + * the different area and we want to keep the contacts with the largest area. The new point is also + * kept. In order to compute the area of a quadrilateral, we use the formula : + * Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that + * when we compute this area, we do not calculate it exactly but we + * only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, + * this is only a guess that is faster to compute. This idea comes from the Bullet Physics library + * by Erwin Coumans (http://wwww.bulletphysics.org). + */ + int getIndexToRemove(final int indexMaxPenetration, final Vector3f newPoint) { + assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + float area0 = 0.0f; // Area with contact 1,2,3 and newPoint + float area1 = 0.0f; // Area with contact 0,2,3 and newPoint + float area2 = 0.0f; // Area with contact 0,1,3 and newPoint + float area3 = 0.0f; // Area with contact 0,1,2 and newPoint + if (indexMaxPenetration != 0) { + // Compute the area + final Vector3f vector1 = newPoint.lessNew(this.contactPoints[1].getLocalPointOnBody1()); + final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1()); + final Vector3f crossProduct = vector1.cross(vector2); + area0 = crossProduct.length2(); + } + if (indexMaxPenetration != 1) { + // Compute the area + final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1()); + final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1()); + final Vector3f crossProduct = vector1.cross(vector2); + area1 = crossProduct.length2(); + } + if (indexMaxPenetration != 2) { + // Compute the area + final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1()); + final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1()); + final Vector3f crossProduct = vector1.cross(vector2); + area2 = crossProduct.length2(); + } + if (indexMaxPenetration != 3) { + // Compute the area + final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1()); + final Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1()); + final Vector3f crossProduct = vector1.cross(vector2); + area3 = crossProduct.length2(); + } + // Return the index of the contact to remove + return getMaxArea(area0, area1, area2, area3); + } + + /// Return the largest depth of all the contact points + public float getLargestContactDepth() { + float largestDepth = 0.0f; + for (int iii = 0; iii < this.nbContactPoints; iii++) { + final float depth = this.contactPoints[iii].getPenetrationDepth(); + if (depth > largestDepth) { + largestDepth = depth; + } + } + return largestDepth; + } + + private int getMaxArea(final float area0, final float area1, final float area2, final float area3) { + if (area0 < area1) { + if (area1 < area2) { + if (area2 < area3) { + return 3; + } else { + return 2; + } + } else if (area1 < area3) { + return 3; + } else { + return 1; + } + } else if (area0 < area2) { + if (area2 < area3) { + return 3; + } else { + return 2; + } + } else if (area0 < area3) { + return 3; + } else { + return 0; + } + } + + /// Return the number of contact points in the manifold + public int getNbContactPoints() { + return this.nbContactPoints; + } + + /// Return the normal direction Id + public int getNormalDirectionId() { + return this.normalDirectionId; + } + + public Vector3f getRollingResistanceImpulse() { + return this.rollingResistanceImpulse; + } + + /// Return a pointer to the first proxy shape of the contact + public ProxyShape getShape1() { + return this.shape1; + } + + /// Return a pointer to the second proxy shape of the contact + public ProxyShape getShape2() { + return this.shape2; + } + + /// Return true if the contact manifold has already been added longo an island + public boolean isAlreadyInIsland() { + return this.isAlreadyInIsland; + } + + /// Remove a contact point from the manifold + public void removeContactPoint(final int index) { + assert (index < this.nbContactPoints); + assert (this.nbContactPoints > 0); + this.contactPoints[index] = null; + // If we don't remove the last index + if (index < this.nbContactPoints - 1) { + this.contactPoints[index] = this.contactPoints[this.nbContactPoints - 1]; + } + this.nbContactPoints--; + } + + /// Set the first friction accumulated impulse + public void setFrictionImpulse1(final float frictionImpulse1) { + this.frictionImpulse1 = frictionImpulse1; + } + + /// Set the second friction accumulated impulse + public void setFrictionImpulse2(final float frictionImpulse2) { + this.frictionImpulse2 = frictionImpulse2; + } + + /// Set the friction twist accumulated impulse + public void setFrictionTwistImpulse(final float frictionTwistImpulse) { + this.frictionTwistImpulse = frictionTwistImpulse; + } + + /// set the second friction vector at the center of the contact manifold + public void setFrictionvec2(final Vector3f frictionvec2) { + this.frictionvec2 = frictionvec2; + } + + /// set the first friction vector at the center of the contact manifold + public void setFrictionVector1(final Vector3f frictionVector1) { + this.frictionVector1 = frictionVector1; + } + + /// Set the accumulated rolling resistance impulse + public void setRollingResistanceImpulse(final Vector3f rollingResistanceImpulse) { + this.rollingResistanceImpulse = rollingResistanceImpulse; + } + + /** + * Update the contact manifold. + * + * First the world space coordinates of the current contacts in the manifold are recomputed from + * the corresponding transforms of the bodies because they have moved. Then we remove the contacts + * with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also + * the contacts with a too large distance between the contact points in the plane orthogonal to the + * contact normal. + */ + public void update(final Transform3D transform1, final Transform3D transform2) { + if (this.nbContactPoints == 0) { + return; + } + // Update the world coordinates and penetration depth of the contact points in the manifold + for (int iii = 0; iii < this.nbContactPoints; iii++) { + this.contactPoints[iii].setWorldPointOnBody1(transform1.multiplyNew(this.contactPoints[iii].getLocalPointOnBody1())); + this.contactPoints[iii].setWorldPointOnBody2(transform2.multiplyNew(this.contactPoints[iii].getLocalPointOnBody2())); + this.contactPoints[iii] + .setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().lessNew(this.contactPoints[iii].getWorldPointOnBody2())).dot(this.contactPoints[iii].getNormal())); + } + final float squarePersistentContactThreshold = Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD; + // Remove the contact points that don't represent very well the contact manifold + for (int iii = (this.nbContactPoints) - 1; iii >= 0; iii--) { + assert (iii < (this.nbContactPoints)); + // Compute the distance between contact points in the normal direction + final float distanceNormal = -this.contactPoints[iii].getPenetrationDepth(); + // If the contacts points are too far from each other in the normal direction + if (distanceNormal > squarePersistentContactThreshold) { + removeContactPoint(iii); + } else { + // Compute the distance of the two contact points in the plane + // orthogonal to the contact normal + final Vector3f projOfPoint1 = this.contactPoints[iii].getNormal().multiplyNew(distanceNormal).add(this.contactPoints[iii].getWorldPointOnBody1()); + final Vector3f projDifference = this.contactPoints[iii].getWorldPointOnBody2().lessNew(projOfPoint1); + // If the orthogonal distance is larger than the valid distance + // threshold, we remove the contact + if (projDifference.length2() > squarePersistentContactThreshold) { + removeContactPoint(iii); + } + } + } + } +} diff --git a/src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java b/src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java new file mode 100644 index 0000000..022ee70 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java @@ -0,0 +1,14 @@ +package org.atriasoft.ephysics.collision; + +/** + * This structure represents a single element of a linked list of contact manifolds + */ +public class ContactManifoldListElement { + public ContactManifold contactManifold; //!< Pointer to the actual contact manifold + public ContactManifoldListElement next; //!< Next element of the list + + public ContactManifoldListElement(final ContactManifold _initContactManifold, final ContactManifoldListElement _initNext) { + this.contactManifold = _initContactManifold; + this.next = _initNext; + } +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/ContactManifoldSet.java b/src/org/atriasoft/ephysics/collision/ContactManifoldSet.java new file mode 100644 index 0000000..7c3c0b5 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/ContactManifoldSet.java @@ -0,0 +1,217 @@ +package org.atriasoft.ephysics.collision; + +import org.atriasoft.ephysics.constraint.ContactPoint; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a set of one or several contact manifolds. Typically a + * convex/convex collision will have a set with a single manifold and a convex-concave + * collision can have more than one manifolds. Note that a contact manifold can + * contains several contact points. + */ +public class ContactManifoldSet { + + private static final int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set + private static final int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap + + private final int nbMaxManifolds; //!< Maximum number of contact manifolds in the set + private int nbManifolds; //!< Current number of contact manifolds in the set + private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact + private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact + + private final ContactManifold[] manifolds = new ContactManifold[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; //!< Contact manifolds of the set + /// Create a new contact manifold and add it to the set + + /// Constructor + public ContactManifoldSet(final ProxyShape _shape1, final ProxyShape _shape2, final int _nbMaxManifolds) { + this.nbMaxManifolds = _nbMaxManifolds; + this.nbManifolds = 0; + this.shape1 = _shape1; + this.shape2 = _shape2; + assert (_nbMaxManifolds >= 1); + } + + /// Add a contact point to the manifold set + public void addContactPoint(final ContactPoint contact) { + // Compute an Id corresponding to the normal direction (using a cubemap) + final int normalDirectionId = computeCubemapNormalId(contact.getNormal()); + // If there is no contact manifold yet + if (this.nbManifolds == 0) { + createManifold(normalDirectionId); + this.manifolds[0].addContactPoint(contact); + assert (this.manifolds[this.nbManifolds - 1].getNbContactPoints() > 0); + for (int iii = 0; iii < this.nbManifolds; iii++) { + assert (this.manifolds[iii].getNbContactPoints() > 0); + } + return; + } + // Select the manifold with the most similar normal (if exists) + int similarManifoldIndex = 0; + if (this.nbMaxManifolds > 1) { + similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId); + } + // If a similar manifold has been found + if (similarManifoldIndex != -1) { + // Add the contact point to that similar manifold + this.manifolds[similarManifoldIndex].addContactPoint(contact); + assert (this.manifolds[similarManifoldIndex].getNbContactPoints() > 0); + return; + } + // If the maximum number of manifold has not been reached yet + if (this.nbManifolds < this.nbMaxManifolds) { + // Create a new manifold for the contact point + createManifold(normalDirectionId); + this.manifolds[this.nbManifolds - 1].addContactPoint(contact); + for (int iii = 0; iii < this.nbManifolds; iii++) { + assert (this.manifolds[iii].getNbContactPoints() > 0); + } + return; + } + // The contact point will be in a new contact manifold, we now have too much + // manifolds condidates. We need to remove one. We choose to keep the manifolds + // with the largest contact depth among their points + int smallestDepthIndex = -1; + float minDepth = contact.getPenetrationDepth(); + assert (this.nbManifolds == this.nbMaxManifolds); + for (int iii = 0; iii < this.nbManifolds; iii++) { + final float depth = this.manifolds[iii].getLargestContactDepth(); + if (depth < minDepth) { + minDepth = depth; + smallestDepthIndex = iii; + } + } + // If we do not want to keep to new manifold (not created yet) with the + // new contact point + if (smallestDepthIndex == -1) { + return; + } + assert (smallestDepthIndex >= 0 && smallestDepthIndex < this.nbManifolds); + // Here we need to replace an existing manifold with a new one (that contains + // the new contact point) + removeManifold(smallestDepthIndex); + createManifold(normalDirectionId); + this.manifolds[this.nbManifolds - 1].addContactPoint(contact); + assert (this.manifolds[this.nbManifolds - 1].getNbContactPoints() > 0); + for (int iii = 0; iii < this.nbManifolds; iii++) { + assert (this.manifolds[iii].getNbContactPoints() > 0); + } + return; + } + + /// Clear the contact manifold set + public void clear() { + for (int iii = this.nbManifolds - 1; iii >= 0; iii--) { + removeManifold(iii); + } + assert (this.nbManifolds == 0); + } + + // Map the normal vector longo a cubemap face bucket (a face contains 4x4 buckets) + // Each face of the cube is divided longo 4x4 buckets. This method maps the + // normal vector longo of the of the bucket and returns a unique Id for the bucket + private int computeCubemapNormalId(final Vector3f normal) { + assert (normal.length2() > Constant.FLOAT_EPSILON); + int faceNo; + float u, v; + final float max = FMath.max(FMath.abs(normal.x), FMath.abs(normal.y), FMath.abs(normal.z)); + final Vector3f normalScaled = normal.divideNew(max); + if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) { + faceNo = normalScaled.x > 0 ? 0 : 1; + u = normalScaled.y; + v = normalScaled.z; + } else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) { + faceNo = normalScaled.y > 0 ? 2 : 3; + u = normalScaled.x; + v = normalScaled.z; + } else { + faceNo = normalScaled.z > 0 ? 4 : 5; + u = normalScaled.x; + v = normalScaled.y; + } + int indexU = FMath.floor(((u + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + int indexV = FMath.floor(((v + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS); + if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) { + indexU--; + } + if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) { + indexV--; + } + final int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS; + return faceNo * 200 + indexU * nbSubDivInFace + indexV; + } + + private void createManifold(final int normalDirectionId) { + assert (this.nbManifolds < this.nbMaxManifolds); + this.manifolds[this.nbManifolds] = new ContactManifold(this.shape1, this.shape2, normalDirectionId); + this.nbManifolds++; + } + + /// Return a given contact manifold + public ContactManifold getContactManifold(final int index) { + assert (index >= 0 && index < this.nbManifolds); + return this.manifolds[index]; + } + + /// Return the number of manifolds in the set + public long getNbContactManifolds() { + return this.nbManifolds; + } + + /// Return the first proxy shape + public ProxyShape getShape1() { + return this.shape1; + } + + /// Return the second proxy shape + public ProxyShape getShape2() { + return this.shape2; + } + + /// Return the total number of contact points in the set of manifolds + public int getTotalNbContactPoints() { + int nbPoints = 0; + for (int iii = 0; iii < this.nbManifolds; iii++) { + nbPoints += this.manifolds[iii].getNbContactPoints(); + } + return nbPoints; + } + + /// Remove a contact manifold from the set + private void removeManifold(final int index) { + assert (this.nbManifolds > 0); + assert (index >= 0 && index < this.nbManifolds); + // Delete the new contact + this.manifolds[index] = null; + for (int iii = index; (iii + 1) < this.nbManifolds; iii++) { + this.manifolds[iii] = this.manifolds[iii + 1]; + } + this.nbManifolds--; + } + + // Return the index of the contact manifold with a similar average normal. + private int selectManifoldWithSimilarNormal(final int normalDirectionId) { + // Return the Id of the manifold with the same normal direction id (if exists) + for (int iii = 0; iii < this.nbManifolds; iii++) { + if (normalDirectionId == this.manifolds[iii].getNormalDirectionId()) { + return iii; + } + } + return -1; + } + + /// Update the contact manifolds + public void update() { + for (int iii = this.nbManifolds - 1; iii >= 0; iii--) { + // Update the contact manifold + this.manifolds[iii].update(this.shape1.getBody().getTransform().multiplyNew(this.shape1.getLocalToBodyTransform()), + this.shape2.getBody().getTransform().multiplyNew(this.shape2.getLocalToBodyTransform())); + // Remove the contact manifold if has no contact points anymore + if (this.manifolds[iii].getNbContactPoints() == 0) { + removeManifold(iii); + } + } + } + +} diff --git a/src/org/atriasoft/ephysics/collision/ProxyShape.java b/src/org/atriasoft/ephysics/collision/ProxyShape.java new file mode 100644 index 0000000..6464cc4 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/ProxyShape.java @@ -0,0 +1,201 @@ +package org.atriasoft.ephysics.collision; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.collision.broadphase.DTree; +import org.atriasoft.ephysics.collision.shapes.CollisionShape; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +public class ProxyShape { + protected CollisionBody body; //!< Pointer to the parent body + protected CollisionShape collisionShape; //!< Internal collision shape + public Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time) + protected float mass; //!< Mass (in kilogramms) of the corresponding collision shape + public ProxyShape next = null; //!< Pointer to the next proxy shape of the body (linked list) + public DTree broadPhaseID = null; //!< Broad-phase ID (node ID in the dynamic AABB tree) + + protected Object cachedCollisionData = null; //!< Cached collision data + + protected Object userData = null; //!< Pointer to user data + + /** + * @brief Bits used to define the collision category of this shape. + * You can set a single bit to one to define a category value for this + * shape. This value is one (0x0001) by default. This variable can be used + * together with the this.collideWithMaskBits variable so that given + * categories of shapes collide with each other and do not collide with + * other categories. + */ + protected int collisionCategoryBits = 0x0001; + /** + * @brief Bits mask used to state which collision categories this shape can + * collide with. This value is 0xFFFF by default. It means that this + * proxy shape will collide with every collision categories by default. + */ + protected int collideWithMaskBits = 0xFFFF; + + /** + * @param body Pointer to the parent body + * @param shape Pointer to the collision shape + * @param transform Transform3Dation from collision shape local-space to body local-space + * @param mass Mass of the collision shape (in kilograms) + */ + public ProxyShape(final CollisionBody body, final CollisionShape shape, final Transform3D transform, final float mass) { + this.body = body; + this.collisionShape = shape; + this.localToBodyTransform = transform; + this.mass = mass; + } + + /** + * @return Pointer to the parent body + */ + public CollisionBody getBody() { + return this.body; + } + + public DTree getBroadPhaseID() { + return this.broadPhaseID; + } + + /// Return the pointer to the cached collision data + public Object getCachedCollisionData() { + return this.cachedCollisionData; + } + + /// Return the collision bits mask + public int getCollideWithMaskBits() { + return this.collideWithMaskBits; + } + + /// Return the collision category bits + public int getCollisionCategoryBits() { + return this.collisionCategoryBits; + } + + /// Return the collision shape + public CollisionShape getCollisionShape() { + return this.collisionShape; + } + + /// Return the local scaling vector of the collision shape + public Vector3f getLocalScaling() { + return this.collisionShape.getScaling(); + } + + /** + * Return the local to parent body transform + * @return The transformation that transforms the local-space of the collision shape + * to the local-space of the parent body + */ + public Transform3D getLocalToBodyTransform() { + return this.localToBodyTransform; + } + + /// Return the local to world transform + public Transform3D getLocalToWorldTransform() { + return this.body.getTransform().multiplyNew(this.localToBodyTransform); + } + + /// Return the mass of the collision shape + public float getMass() { + return this.mass; + } + + /// Return the next proxy shape in the linked list of proxy shapes + public ProxyShape getNext() { + return this.next; + } + + /// Return a pointer to the user data attached to this body + public Object getUserData() { + return this.userData; + } + + /** + * @param ray Ray to use for the raycasting + * @param[out] raycastInfo Result of the raycasting that is valid only if the + * methods returned true + * @return True if the ray hit the collision shape + */ + public boolean raycast(final Ray ray, final RaycastInfo raycastInfo) { + + // If the corresponding body is not active, it cannot be hit by rays + if (!this.body.isActive()) { + return false; + } + + // Convert the ray longo the local-space of the collision shape + final Transform3D localToWorldTransform = getLocalToWorldTransform(); + final Transform3D worldToLocalTransform = localToWorldTransform.inverseNew(); + + final Ray rayLocal = new Ray(worldToLocalTransform.multiplyNew(ray.point1), worldToLocalTransform.multiplyNew(ray.point2), ray.maxFraction); + + final boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this); + if (isHit == true) { + // Convert the raycast info longo world-space + raycastInfo.worldPoint = localToWorldTransform.multiplyNew(raycastInfo.worldPoint); + raycastInfo.worldNormal = localToWorldTransform.getOrientation().multiply(raycastInfo.worldNormal); + raycastInfo.worldNormal.normalize(); + } + return isHit; + } + + public void setBroadPhaseID(final DTree broadPhase) { + this.broadPhaseID = broadPhase; + } + + /// Set the collision bits mask + public void setCollideWithMaskBits(final int collideWithMaskBits) { + this.collideWithMaskBits = collideWithMaskBits; + } + + /// Set the collision category bits + public void setCollisionCategoryBits(final int collisionCategoryBits) { + this.collisionCategoryBits = collisionCategoryBits; + } + + /** + * Set the local scaling vector of the collision shape + * @param scaling The new local scaling vector + */ + public void setLocalScaling(final Vector3f scaling) { + + // Set the local scaling of the collision shape + this.collisionShape.setLocalScaling(scaling); + + this.body.setIsSleeping(false); + + // Notify the body that the proxy shape has to be updated in the broad-phase + this.body.updateProxyShapeInBroadPhase(this, true); + } + + /// Set the local to parent body transform + public void setLocalToBodyTransform(final Transform3D transform) { + + this.localToBodyTransform = transform; + + this.body.setIsSleeping(false); + + // Notify the body that the proxy shape has to be updated in the broad-phase + this.body.updateProxyShapeInBroadPhase(this, true); + } + + /// Attach user data to this body + public void setUserData(final Object userData) { + this.userData = userData; + } + + /** + * @param worldPoint Point to test in world-space coordinates + * @return True if the point is inside the collision shape + */ + public boolean testPointInside(final Vector3f worldPoint) { + final Transform3D localToWorld = this.body.getTransform().multiplyNew(this.localToBodyTransform); + final Vector3f localPoint = localToWorld.inverseNew().multiply(worldPoint); + return this.collisionShape.testPointInside(localPoint, this); + } + +} diff --git a/src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java b/src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java new file mode 100644 index 0000000..e5b8873 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java @@ -0,0 +1,22 @@ +package org.atriasoft.ephysics.collision; + +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback; +import org.atriasoft.ephysics.constraint.ContactPointInfo; +import org.atriasoft.ephysics.engine.CollisionCallback; +import org.atriasoft.ephysics.engine.OverlappingPair; + +public class TestCollisionBetweenShapesCallback implements NarrowPhaseCallback { + + private final CollisionCallback collisionCallback; + + // Constructor + public TestCollisionBetweenShapesCallback(final CollisionCallback _callback) { + this.collisionCallback = _callback; + + } + + // Called by a narrow-phase collision algorithm when a new contact has been found + public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) { + this.collisionCallback.notifyContact(_contactInfo); + } +} diff --git a/src/org/atriasoft/ephysics/collision/Triangle.java b/src/org/atriasoft/ephysics/collision/Triangle.java new file mode 100644 index 0000000..3d6a928 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/Triangle.java @@ -0,0 +1,12 @@ +package org.atriasoft.ephysics.collision; + +import org.atriasoft.etk.math.Vector3f; + +public class Triangle { + public final Vector3f[] value = new Vector3f[3]; + + public Vector3f get(final int _id) { + return this.value[_id]; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/TriangleMesh.java b/src/org/atriasoft/ephysics/collision/TriangleMesh.java new file mode 100644 index 0000000..0de2d3c --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/TriangleMesh.java @@ -0,0 +1,36 @@ +package org.atriasoft.ephysics.collision; + +import java.util.ArrayList; +import java.util.List; + +public class TriangleMesh { + /// All the triangle arrays of the mesh (one triangle array per part) + protected List triangleArrays = new ArrayList<>(); + + /** + * Constructor + */ + public TriangleMesh() {} + + /** + * Add a subpart of the mesh + */ + public void addSubpart(final TriangleVertexArray _triangleVertexArray) { + this.triangleArrays.add(_triangleVertexArray); + } + + /** + * Get the number of subparts of the mesh + */ + public int getNbSubparts() { + return this.triangleArrays.size(); + } + + /** + * Get a pointer to a given subpart (triangle vertex array) of the mesh + */ + public TriangleVertexArray getSubpart(final int _indexSubpart) { + assert (_indexSubpart < this.triangleArrays.size()); + return this.triangleArrays.get(_indexSubpart); + } +} diff --git a/src/org/atriasoft/ephysics/collision/TriangleVertexArray.java b/src/org/atriasoft/ephysics/collision/TriangleVertexArray.java new file mode 100644 index 0000000..2c2c03f --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/TriangleVertexArray.java @@ -0,0 +1,84 @@ +package org.atriasoft.ephysics.collision; + +import java.util.List; + +import org.atriasoft.etk.math.Vector3f; + +/** + * This class is used to describe the vertices and faces of a triangular mesh. + * A TriangleVertexArray represents a continuous array of vertices and indexes + * of a triangular mesh. When you create a TriangleVertexArray, no data is copied + * longo the array. It only stores pointer to the data. The purpose is to allow + * the user to share vertices data between the physics engine and the rendering + * part. Therefore, make sure that the data pointed by a TriangleVertexArray + * remains valid during the TriangleVertexArray life. + */ +public class TriangleVertexArray { + /// Vertice list + protected final Vector3f[] vertices; + /// List of triangle (3 pos for each triangle) + protected final int[] triangles; + + public TriangleVertexArray(final List _vertices, final List _triangles) { + this.vertices = _vertices.toArray(new Vector3f[0]); + + this.triangles = new int[_triangles.size()]; + for (int iii = 0; iii < this.triangles.length; iii++) { + this.triangles[iii] = _triangles.get(iii); + } + } + + /** + * Constructor + * @param _vertices List Of all vertices + * @param _triangles List of all linked points + */ + public TriangleVertexArray(final Vector3f[] _vertices, final int[] _triangles) { + this.vertices = _vertices; + this.triangles = _triangles; + } + + /** + * Get The table of the triangle indice + * @return reference on the triangle indice + */ + public int[] getIndices() { + return this.triangles; + } + + /** + * Get the number of triangle + * @return Number of triangles + */ + public int getNbTriangles() { + return this.triangles.length / 3; + } + + /** + * Get the number of vertices + * @return Number of vertices + */ + public int getNbVertices() { + return this.vertices.length; + } + + /** + * Get a triangle at the specific ID + * @return Buffer of 3 points + */ + public Triangle getTriangle(final int _id) { + final Triangle out = new Triangle(); + out.value[0] = this.vertices[this.triangles[_id * 3]]; + out.value[1] = this.vertices[this.triangles[_id * 3 + 1]]; + out.value[2] = this.vertices[this.triangles[_id * 3 + 2]]; + return out; + } + + /** + * Get The table of the vertices + * @return reference on the vertices + */ + public Vector3f[] getVertices() { + return this.vertices; + } +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java b/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java new file mode 100644 index 0000000..c4a76d8 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java @@ -0,0 +1,256 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.collision.broadphase; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.Iterator; +import java.util.List; + +import org.atriasoft.ephysics.Configuration; +import org.atriasoft.ephysics.RaycastTest; +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.shapes.AABB; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Vector3f; + +/** + * It represents the broad-phase collision detection. The + * goal of the broad-phase collision detection is to compute the pairs of proxy shapes + * that have their AABBs overlapping. Only those pairs of bodies will be tested + * later for collision during the narrow-phase collision detection. A dynamic AABB + * tree data structure is used for fast broad-phase collision detection. + */ +public class BroadPhaseAlgorithm { + /** + * Callback called when the AABB of a leaf node is hit by a ray the + * broad-phase Dynamic AABB Tree. + */ + public class BroadPhaseRaycastCallback implements CallbackRaycast { + private final DynamicAABBTree dynamicAABBTree; + private final int raycastWithCategoryMaskBits; + private final RaycastTest raycastTest; + + // Constructor + public BroadPhaseRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final int _raycastWithCategoryMaskBits, final RaycastTest _raycastTest) { + this.dynamicAABBTree = _dynamicAABBTree; + this.raycastWithCategoryMaskBits = _raycastWithCategoryMaskBits; + this.raycastTest = _raycastTest; + + } + + @Override + public float callback(final DTree _node, final Ray _ray) { + float hitFraction = -1.0f; + // Get the proxy shape from the node + final ProxyShape proxyShape = (ProxyShape) this.dynamicAABBTree.getNodeDataPointer(_node); + // Check if the raycast filtering mask allows raycast against this shape + if ((this.raycastWithCategoryMaskBits & proxyShape.getCollisionCategoryBits()) != 0) { + // Ask the collision detection to perform a ray cast test against + // the proxy shape of this node because the ray is overlapping + // with the shape in the broad-phase + hitFraction = this.raycastTest.raycastAgainstShape(proxyShape, _ray); + } + return hitFraction; + } + }; + + /// A tree of data that is separated by a specific distance in AABB model. + protected DynamicAABBTree dynamicAABBTree; + /** + * Array with the broad-phase IDs of all collision shapes that have moved (or have been created) during the last simulation step. + * Those are the shapes that need to be tested for overlapping in the next simulation step. ==> and re-index in the overlapping tree + */ + protected List movedShapes = new ArrayList<>(); + /** + * Temporary array of potential overlapping pairs (with potential duplicates) + */ + protected List potentialPairs = new ArrayList<>(); + /** + * Reference to the collision detection object + */ + protected CollisionDetection collisionDetection; // TODO revoked with generic interface for the callback... + + public BroadPhaseAlgorithm(final CollisionDetection _collisionDetection) { + this.dynamicAABBTree = new DynamicAABBTree(Configuration.DYNAMIC_TREE_AABB_GAP); + this.collisionDetection = _collisionDetection; + } + + /// 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); + this.movedShapes.add(_broadPhaseID); + } + + /// Add a proxy collision shape longo the broad-phase collision detection + public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) { + // Add the collision shape longo the dynamic AABB tree and get its broad-phase ID + final DTree nodeId = this.dynamicAABBTree.addObject(_aabb, _proxyShape); + // Set the broad-phase ID of the proxy shape + _proxyShape.setBroadPhaseID(nodeId); + // Add the collision shape longo the array of bodies that have moved (or have been created) + // during the last simulation step + addMovedCollisionShape(_proxyShape.getBroadPhaseID()); + } + + /// Compute all the overlapping pairs of collision shapes + public void computeOverlappingPairs() { + 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()); + /* + for (final DTree it : this.movedShapes) { + Log.info(" - " + it); + } + */ + for (final DTree it : this.movedShapes) { + // Get the AABB of the shape + final AABB shapeAABB = this.dynamicAABBTree.getFatAABB(it); + final BroadPhaseAlgorithm self = this; + // Ask the dynamic AABB tree to report all collision shapes that overlap with + // this AABB. The method BroadPhase::notifiyOverlappingPair() will be called + // by the dynamic AABB tree for each potential overlapping pair. + this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, new CallbackOverlapping() { + @Override + public void callback(final DTree _nodeId) { + // TODO Auto-generated method stub// If both the nodes are the same, we do not create store the overlapping pair + if (it == _nodeId) { + return; + } + // Add the new potential pair longo the array of potential overlapping pairs + self.potentialPairs.add(new PairDTree(it, _nodeId)); + } + }); + } + //Log.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 + this.potentialPairs.sort(new Comparator() { + @Override + public int compare(final PairDTree _pair1, final PairDTree _pair2) { + if (_pair1.first.uid < _pair2.first.uid) { + return -1; + } + if (_pair1.first.uid == _pair2.first.uid) { + if (_pair1.second.uid < _pair2.second.uid) { + return -1; + } + if (_pair1.second.uid == _pair2.second.uid) { + return 0; + } + return +1; + } + return +1; + } + }); + // Check all the potential overlapping pairs avoiding duplicates to report unique + // overlapping pairs + int iii = 0; + while (iii < this.potentialPairs.size()) { + // Get a potential overlapping pair + final PairDTree pair = this.potentialPairs.get(iii); + ++iii; + // Get the two collision shapes of the pair + final ProxyShape shape1 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.first)); + final ProxyShape shape2 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.second)); + // Notify the collision detection about the overlapping pair + this.collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2); + // Skip the duplicate overlapping pairs + while (iii < this.potentialPairs.size()) { + // Get the next pair + final PairDTree nextPair = this.potentialPairs.get(iii); + // If the next pair is different from the previous one, we stop skipping pairs + // TODO check if it work without uid ... + if (nextPair.first.uid != pair.first.uid || nextPair.second.uid != pair.second.uid) { + break; + } + ++iii; + } + } + } + + /// Ray casting method + public void raycast(final Ray _ray, final RaycastTest _raycastTest, final int _raycastWithCategoryMaskBits) { + final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, _raycastWithCategoryMaskBits, _raycastTest); + this.dynamicAABBTree.raycast(_ray, broadPhaseRaycastCallback); + } + + /// Remove a collision shape from 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 removeMovedCollisionShape(final DTree _broadPhaseID) { + final Iterator it = this.movedShapes.iterator(); + while (it.hasNext()) { + final DTree elem = it.next(); + if (elem == _broadPhaseID) { + it.remove(); + } + } + } + + /// Remove a proxy collision shape from the broad-phase collision detection + public void removeProxyCollisionShape(final ProxyShape _proxyShape) { + final DTree broadPhaseID = _proxyShape.getBroadPhaseID(); + // Remove the collision shape from the dynamic AABB tree + this.dynamicAABBTree.removeObject(broadPhaseID); + // Remove the collision shape longo the array of shapes that have moved (or have been created) + // during the last simulation step + removeMovedCollisionShape(broadPhaseID); + } + + /// Return true if the two broad-phase collision shapes are overlapping + public boolean testOverlappingShapes(final ProxyShape _shape1, final ProxyShape _shape2) { + if (_shape1 == _shape2) { + return false; + } + // Get the two AABBs of the collision shapes + final AABB aabb1 = this.dynamicAABBTree.getFatAABB(_shape1.getBroadPhaseID()); + final AABB aabb2 = this.dynamicAABBTree.getFatAABB(_shape2.getBroadPhaseID()); + // Check if the two AABBs are overlapping + return aabb1.testCollision(aabb2); + } + + /// Notify the broad-phase that a collision shape has moved and need to be updated + public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement) { + updateProxyCollisionShape(_proxyShape, _aabb, _displacement, false); + } + + public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) { + final DTree broadPhaseID = _proxyShape.getBroadPhaseID(); + // 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); + // longo the tree). + if (hasBeenReInserted) { + // Add the collision shape longo the array of shapes that have moved (or have been created) + // during the last simulation step + addMovedCollisionShape(broadPhaseID); + } + } +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java b/src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java new file mode 100644 index 0000000..ea63903 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java @@ -0,0 +1,5 @@ +package org.atriasoft.ephysics.collision.broadphase; + +public interface CallbackOverlapping { + public void callback(DTree _nodeId); +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java b/src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java new file mode 100644 index 0000000..6429992 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java @@ -0,0 +1,7 @@ +package org.atriasoft.ephysics.collision.broadphase; + +import org.atriasoft.ephysics.mathematics.Ray; + +public interface CallbackRaycast { + public float callback(DTree _node, Ray _ray); +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DTree.java b/src/org/atriasoft/ephysics/collision/broadphase/DTree.java new file mode 100644 index 0000000..60e3ad2 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/DTree.java @@ -0,0 +1,28 @@ +package org.atriasoft.ephysics.collision.broadphase; + +import java.lang.ref.WeakReference; + +import org.atriasoft.ephysics.collision.shapes.AABB; + +/** + * It represents a node of the dynamic AABB tree. + */ +public class DTree { + private static int simpleCounter = 0; + public final int uid; + /** + * A node is either in the tree (has a parent) or in the free nodes list (has a next node) + */ + WeakReference parent = null; //!< Parent node ID (0 is ROOT) + int height = -1; //!< Height of the node in the tree TODO check the need... + AABB aabb = new AABB(); //!< Fat axis aligned bounding box (AABB) corresponding to the node + + public DTree() { + this.uid = simpleCounter++; + } + + boolean isLeaf() { + return false; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafData.java b/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafData.java new file mode 100644 index 0000000..1caf6d9 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafData.java @@ -0,0 +1,15 @@ +package org.atriasoft.ephysics.collision.broadphase; + +class DTreeLeafData extends DTree { + Object dataPointer = null; + + public DTreeLeafData(final Object dataPointer) { + super(); + this.dataPointer = dataPointer; + } + + @Override + boolean isLeaf() { + return true; + } +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java b/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java new file mode 100644 index 0000000..ad37a35 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java @@ -0,0 +1,17 @@ +package org.atriasoft.ephysics.collision.broadphase; + +class DTreeLeafInt extends DTree { + int dataInt_0 = 0; + int dataInt_1 = 0; + + public DTreeLeafInt(final int dataInt_0, final int dataInt_1) { + super(); + this.dataInt_0 = dataInt_0; + this.dataInt_1 = dataInt_1; + } + + @Override + boolean isLeaf() { + return true; + } +} diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java b/src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java new file mode 100644 index 0000000..17774f8 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java @@ -0,0 +1,6 @@ +package org.atriasoft.ephysics.collision.broadphase; + +class DTreeNode extends DTree { + DTree children_left = null; //!< Left child of the node + DTree children_right = null; //!< Right child of the node +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java b/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java new file mode 100644 index 0000000..322444f --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java @@ -0,0 +1,576 @@ +package org.atriasoft.ephysics.collision.broadphase; + +import java.lang.ref.WeakReference; +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; + +/** + * It implements a dynamic AABB tree that is used for broad-phase + * collision detection. This data structure is inspired by Nathanael Presson's + * dynamic tree implementation in BulletPhysics. The following implementation is + * based on the one from Erin Catto in Box2D as described in the book + * "Introduction to Game Physics with Box2D" by Ian Parberry. + */ +public class DynamicAABBTree { + + private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree + + 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 + + /// Constructor + public DynamicAABBTree() { + this(0.0f); + } + + public DynamicAABBTree(final float extraAABBGap) { + this.extraAABBGap = extraAABBGap; + init(); + } + + /// Add an object into the tree (where node data are two integers) + public DTree addObject(final AABB aabb, final int data1, final int data2) { + final DTreeLeafInt node = new DTreeLeafInt(data1, data2); + addObjectInternal(aabb, node); + return node; + } + + /// Add an object into the tree (where node data is a pointer) + public DTree addObject(final AABB aabb, final Object data) { + final DTreeLeafData node = new DTreeLeafData(data); + addObjectInternal(aabb, node); + return node; + } + + /// Internally add an object into the tree + private void addObjectInternal(final AABB aabb, final DTree leafNode) { + // Create the fat aabb to use in the tree + leafNode.aabb.setMin(aabb.getMin().lessNew(this.extraAABBGap)); + leafNode.aabb.setMax(aabb.getMax().addNew(this.extraAABBGap)); + // Set the height of the node in the tree + leafNode.height = 0; + // Insert the new leaf node in the tree + insertLeafNode(leafNode); + assert (leafNode.isLeaf()); + } + + /// Balance the sub-tree of a given node using left or right rotations. + private DTree balanceSubTreeAtNode(final DTree _node) { + assert (_node != null); + // If the node is a leaf or the height of A's sub-tree is less than 2 + if (_node.isLeaf() || _node.height < 2) { + // Do not perform any rotation + return _node; + } + final DTreeNode nodeA = (DTreeNode) _node; + // Get the two children nodes + final DTree nodeB = nodeA.children_left; + final DTree nodeC = nodeA.children_right; + // Compute the factor of the left and right sub-trees + final int balanceFactor = nodeC.height - nodeB.height; + // If the right node C is 2 higher than left node B + if (balanceFactor > 1) { + assert (!nodeC.isLeaf()); + final DTreeNode nodeCTree = (DTreeNode) nodeC; + final DTree nodeF = nodeCTree.children_left; + final DTree nodeG = nodeCTree.children_right; + nodeCTree.children_left = _node; + nodeCTree.parent = nodeA.parent; + nodeA.parent = new WeakReference(nodeC); + if (nodeC.parent != null) { + final DTreeNode nodeCParent = (DTreeNode) nodeC.parent.get(); + if (nodeCParent.children_left == _node) { + nodeCParent.children_left = nodeC; + } else { + assert (nodeCParent.children_right == _node); + nodeCParent.children_right = nodeC; + } + } else { + this.rootNode = nodeC; + } + assert (!nodeC.isLeaf()); + assert (!nodeA.isLeaf()); + // If the right node C was higher than left node B because of the F node + if (nodeF.height > nodeG.height) { + nodeCTree.children_right = nodeF; + nodeA.children_right = nodeG; + nodeG.parent = new WeakReference(_node); + // Recompute the AABB of node A and C + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); + // Recompute the height of node A and C + nodeA.height = FMath.max(nodeB.height, nodeG.height) + 1; + nodeC.height = FMath.max(nodeA.height, nodeF.height) + 1; + assert (nodeA.height > 0); + assert (nodeC.height > 0); + } else { + // If the right node C was higher than left node B because of node G + nodeCTree.children_right = nodeG; + nodeA.children_right = nodeF; + nodeF.parent = new WeakReference(_node); + // Recompute the AABB of node A and C + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); + // Recompute the height of node A and C + nodeA.height = FMath.max(nodeB.height, nodeF.height) + 1; + nodeC.height = FMath.max(nodeA.height, nodeG.height) + 1; + assert (nodeA.height > 0); + assert (nodeC.height > 0); + } + // Return the new root of the sub-tree + return nodeC; + } + // If the left node B is 2 higher than right node C + if (balanceFactor < -1) { + assert (!nodeB.isLeaf()); + final DTreeNode nodeBTree = (DTreeNode) nodeB; + final DTree nodeF = nodeBTree.children_left; + final DTree nodeG = nodeBTree.children_right; + nodeBTree.children_left = _node; + nodeB.parent = nodeA.parent; + nodeA.parent = new WeakReference(nodeB); + if (nodeB.parent != null) { + final DTreeNode nodeBParent = (DTreeNode) nodeB.parent.get(); + if (nodeBParent.children_left == _node) { + nodeBParent.children_left = nodeB; + } else { + assert (nodeBParent.children_right == _node); + nodeBParent.children_right = nodeB; + } + } else { + this.rootNode = nodeB; + } + assert (!nodeB.isLeaf()); + assert (!nodeA.isLeaf()); + // If the left node B was higher than right node C because of the F node + if (nodeF.height > nodeG.height) { + nodeBTree.children_right = nodeF; + nodeA.children_left = nodeG; + nodeG.parent = new WeakReference(_node); + // Recompute the AABB of node A and B + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); + // Recompute the height of node A and B + nodeA.height = FMath.max(nodeC.height, nodeG.height) + 1; + nodeB.height = FMath.max(nodeA.height, nodeF.height) + 1; + assert (nodeA.height > 0); + assert (nodeB.height > 0); + } else { + // If the left node B was higher than right node C because of node G + nodeBTree.children_right = nodeG; + nodeA.children_left = nodeF; + nodeF.parent = new WeakReference(_node); + // Recompute the AABB of node A and B + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); + // Recompute the height of node A and B + nodeA.height = FMath.max(nodeC.height, nodeF.height) + 1; + nodeB.height = FMath.max(nodeA.height, nodeG.height) + 1; + assert (nodeA.height > 0); + assert (nodeB.height > 0); + } + // Return the new root of the sub-tree + return nodeB; + } + // If the sub-tree is balanced, return the current root node + return _node; + } + + /// Compute the height of the tree + public int computeHeight() { + return computeHeight(this.rootNode); + } + + /// Compute the height of a given node in the tree + private int computeHeight(final DTree _node) { + // If the node is a leaf, its height is zero + if (_node.isLeaf()) { + return 0; + } + final DTreeNode nodeTree = (DTreeNode) _node; + + // Compute the height of the left and right sub-tree + final int leftHeight = computeHeight(nodeTree.children_left); + final int rightHeight = computeHeight(nodeTree.children_right); + // Return the height of the node + return 1 + Math.max(leftHeight, rightHeight); + } + + /// Return the fat AABB corresponding to a given node ID + public AABB getFatAABB(final DTree node) { + return node.aabb; + } + + public int getNodeDataInt_0(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafInt) node).dataInt_0; + } + + public int getNodeDataInt_1(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafInt) node).dataInt_1; + } + + /// Return the data pointer of a given leaf node of the tree + public Object getNodeDataPointer(final DTree node) { + assert (node.isLeaf()); + return ((DTreeLeafData) node).dataPointer; + } + + /// Return the root AABB of the tree + public AABB getRootAABB() { + return getFatAABB(this.rootNode); + } + + /// Initialize the tree + private void init() { + this.rootNode = null; + } + + /// Insert a leaf node in the tree + private void insertLeafNode(final DTree _node) { + // If the tree is empty + if (this.rootNode == null) { + this.rootNode = _node; + return; + } + // Find the best sibling node for the new node + final AABB newNodeAABB = _node.aabb; + DTree currentNode = this.rootNode; + while (!currentNode.isLeaf()) { + final DTreeNode node = (DTreeNode) currentNode; + final DTree leftChild = node.children_left; + final DTree rightChild = node.children_right; + // Compute the merged AABB + final float volumeAABB = currentNode.aabb.getVolume(); + final AABB mergedAABBs = new AABB(); + mergedAABBs.mergeTwoAABBs(currentNode.aabb, newNodeAABB); + final float mergedVolume = mergedAABBs.getVolume(); + // Compute the cost of making the current node the sibbling of the new node + final float costS = 2.0f * mergedVolume; + // Compute the minimum cost of pushing the new node further down the tree (inheritance cost) + final float costI = 2.0f * (mergedVolume - volumeAABB); + // Compute the cost of descending into the left child + float costLeft; + final AABB currentAndLeftAABB = new AABB(); + currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, leftChild.aabb); + if (leftChild.isLeaf()) { // If the left child is a leaf + costLeft = currentAndLeftAABB.getVolume() + costI; + } else { + final float leftChildVolume = leftChild.aabb.getVolume(); + costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume; + } + // Compute the cost of descending into the right child + float costRight; + final AABB currentAndRightAABB = new AABB(); + currentAndRightAABB.mergeTwoAABBs(newNodeAABB, rightChild.aabb); + if (rightChild.isLeaf()) { // If the right child is a leaf + costRight = currentAndRightAABB.getVolume() + costI; + } else { + final float rightChildVolume = rightChild.aabb.getVolume(); + costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume; + } + // If the cost of making the current node a sibbling of the new node is smaller than + // the cost of going down into the left or right child + if (costS < costLeft && costS < costRight) { + break; + } + // It is cheaper to go down into a child of the current node, choose the best child + if (costLeft < costRight) { + currentNode = leftChild; + } else { + currentNode = rightChild; + } + } + final DTree siblingNode = currentNode; + // Create a new parent for the new node and the sibling node + final WeakReference oldParentNode = siblingNode.parent; + final DTreeNode newParentNode = new DTreeNode(); + newParentNode.parent = currentNode.parent; + newParentNode.aabb.mergeTwoAABBs(currentNode.aabb, newNodeAABB); + newParentNode.height = currentNode.height + 1; + // If the sibling node was not the root node + if (oldParentNode != null) { + // replace in parent the child with the new child + final DTreeNode parentNode = (DTreeNode) oldParentNode.get(); + if (parentNode.children_left == siblingNode) { + parentNode.children_left = newParentNode; + } else { + parentNode.children_right = newParentNode; + } + } else { + // If the sibling node was the root node + this.rootNode = newParentNode; + } + // setup the children + newParentNode.children_left = siblingNode; + newParentNode.children_right = _node; + siblingNode.parent = new WeakReference(newParentNode); + _node.parent = new WeakReference(newParentNode); + + // Move up in the tree to change the AABBs that have changed + currentNode = newParentNode; + assert (!currentNode.isLeaf()); + while (currentNode != null) { + // Balance the sub-tree of the current node if it is not balanced + currentNode = balanceSubTreeAtNode(currentNode); + assert (_node.isLeaf()); + assert (!currentNode.isLeaf()); + final DTreeNode nodeDouble = (DTreeNode) currentNode; + final DTree leftChild = nodeDouble.children_left; + final DTree rightChild = nodeDouble.children_right; + assert (leftChild != null); + assert (rightChild != null); + // Recompute the height of the node in the tree + currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1; + assert (currentNode.height > 0); + // Recompute the AABB of the node + currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb); + if (currentNode.parent != null) { + currentNode = currentNode.parent.get(); + } else { + currentNode = null; + } + } + assert (_node.isLeaf()); + } + + /// Ray casting method + public void raycast(final Ray _ray, final CallbackRaycast _callback) { + if (_callback == null) { + Log.error("call with null callback"); + return; + } + float maxFraction = _ray.maxFraction; + // 128 max + final Stack stack = new Stack(); + stack.push(this.rootNode); + // Walk through the tree from the root looking for proxy shapes + // that overlap with the ray AABB + while (stack.size() > 0) { + // Get the next node in the stack + final DTree node = stack.pop(); + // If it is a null node, skip it + if (node == null) { + continue; + } + final Ray rayTemp = new Ray(_ray.point1, _ray.point2, maxFraction); + // Test if the ray intersects with the current node AABB + if (node.aabb.testRayIntersect(rayTemp) == false) { + continue; + } + // If the node is a leaf of the tree + if (node.isLeaf()) { + // Call the callback that will raycast again the broad-phase shape + final float hitFraction = _callback.callback(node, rayTemp); + // If the user returned a hitFraction of zero, it means that + // the raycasting should stop here + if (hitFraction == 0.0f) { + return; + } + // If the user returned a positive fraction + if (hitFraction > 0.0f) { + // We update the maxFraction value and the ray + // AABB using the new maximum fraction + if (hitFraction < maxFraction) { + maxFraction = hitFraction; + } + } + // If the user returned a negative fraction, we continue + // the raycasting as if the proxy shape did not exist + } else { // If the node has children + final DTreeNode tmpNode = (DTreeNode) node; + // Push its children in the stack of nodes to explore + stack.push(tmpNode.children_left); + stack.push(tmpNode.children_right); + } + } + + } + + /// Release a node + private void releaseNode(final DTree _node) { + //this.numberNodes--; + } + + /// Remove a leaf node from the tree + private void removeLeafNode(final DTree _node) { + assert (_node.isLeaf()); + // If we are removing the root node (root node is a leaf in this case) + if (this.rootNode == _node) { + this.rootNode = null; + return; + } + // parent can not be null. + final DTreeNode parentNode = (DTreeNode) _node.parent.get(); + final WeakReference grandParentNodeWeak = parentNode.parent; + DTree siblingNode; + if (parentNode.children_left == _node) { + siblingNode = parentNode.children_right; + } else { + siblingNode = parentNode.children_left; + } + // If the parent of the node to remove is not the root node + if (grandParentNodeWeak == null) { + // If the parent of the node to remove is the root node + // The sibling node becomes the new root node + this.rootNode = siblingNode; + siblingNode.parent = null; + releaseNode(parentNode); + } else { + final DTreeNode grandParentNode = (DTreeNode) grandParentNodeWeak.get(); + // Destroy the parent node + if (grandParentNode.children_left == parentNode) { + grandParentNode.children_left = siblingNode; + } else { + grandParentNode.children_right = siblingNode; + } + siblingNode.parent = parentNode.parent; + releaseNode(parentNode); + // Now, we need to recompute the AABBs of the node on the path back to the root + // and make sure that the tree is still balanced + DTree currentNode = grandParentNode; + while (currentNode != null) { + // Balance the current sub-tree if necessary + currentNode = balanceSubTreeAtNode(currentNode); + assert (!currentNode.isLeaf()); + final DTreeNode currentTreeNode = (DTreeNode) currentNode; + // Get the two children of the current node + final DTree leftChild = currentTreeNode.children_left; + final DTree rightChild = currentTreeNode.children_right; + // Recompute the AABB and the height of the current node + currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb); + currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1; + assert (currentNode.height > 0); + if (currentNode.parent == null) { + currentNode = null; + } else { + currentNode = currentNode.parent.get(); + } + } + } + } + + /// Remove an object from the tree + public void removeObject(final DTree _node) { + assert (_node.isLeaf()); + // Remove the node from the tree + removeLeafNode(_node); + releaseNode(_node); + } + + /// 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"); + return; + } + //Log.error("reportAllShapesOverlappingWithAABB"); + // Create a stack with the nodes to visit + final Stack stack = new Stack(); + // 64 max + stack.push(this.rootNode); + //Log.error(" add stack: " + this.rootNode); + // While there are still nodes to visit + while (stack.size() > 0) { + // Get the next node ID to visit + final DTree nodeIDToVisit = stack.pop(); + // Skip it if it is a null node + if (nodeIDToVisit == null) { + continue; + } + // Get the corresponding node + final DTree nodeToVisit = nodeIDToVisit; + //Log.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 ..."); + } + */ + // Notify the broad-phase about a new potential overlapping pair + _callback.callback(nodeIDToVisit); + } else { + final DTreeNode tmp = (DTreeNode) nodeToVisit; + // If the node is not a leaf + // We need to visit its children + stack.push(tmp.children_left); + stack.push(tmp.children_right); + //Log.error(" add stack: " + tmp.children_left); + //Log.error(" add stack: " + tmp.children_right); + } + } + } + } + + /// Clear all the nodes and reset the tree + public void reset() { + // Initialize the tree + init(); + } + + /// Update the dynamic tree after an object has moved. + /// If the new AABB of the object that has moved is still inside its fat AABB, then + /// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. + /// The method returns true if the object has been reinserted into the tree. The "displacement" + /// argument is the linear velocity of the AABB multiplied by the elapsed time between two + /// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node + /// (this can be useful if the shape AABB has become much smaller than the previous one for instance). + public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement) { + return updateObject(_node, _newAABB, _displacement, false); + } + + 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()); + // If the new AABB is still inside the fat AABB of the node + if (_forceReinsert == false && _node.aabb.contains(_newAABB)) { + return false; + } + // If the new AABB is outside the fat AABB, we remove the corresponding node + removeLeafNode(_node); + // Compute the fat AABB by inflating the AABB with a ant gap + _node.aabb = _newAABB; + + final Vector3f gap = new Vector3f(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); + _node.aabb.getMin().less(gap); + _node.aabb.getMax().add(gap); + // Inflate the fat AABB in direction of the linear motion of the AABB + if (_displacement.x < 0.0f) { + _node.aabb.getMin().setX(_node.aabb.getMin().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x); + } else { + _node.aabb.getMax().setX(_node.aabb.getMax().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x); + } + if (_displacement.y < 0.0f) { + _node.aabb.getMin().setY(_node.aabb.getMin().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y); + } else { + _node.aabb.getMax().setY(_node.aabb.getMax().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y); + } + if (_displacement.z < 0.0f) { + _node.aabb.getMin().setZ(_node.aabb.getMin().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z); + } else { + _node.aabb.getMax().setZ(_node.aabb.getMax().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z); + } + //Log.error(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax()); + //Log.error(" : " + _newAABB.getMin() + " " + _newAABB.getMax()); + if (_node.aabb.contains(_newAABB) == false) { + //Log.critical("ERROR"); + } + assert (_node.aabb.contains(_newAABB)); + // Reinsert the node into the tree + insertLeafNode(_node); + return true; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java b/src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java new file mode 100644 index 0000000..3c4373d --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java @@ -0,0 +1,73 @@ +package org.atriasoft.ephysics.collision.broadphase; + +import java.util.Set; + +import javafx.collections.transformation.SortedList; + +public class PairDTree { + public static int countInSet(final Set values, final PairDTree sample) { + int count = 0; + for (final PairDTree elem : values) { + if (elem.first != sample.first) { + continue; + } + if (elem.second != sample.second) { + continue; + } + count++; + } + return count; + } + + public static int countInSet(final SortedList values, final PairDTree sample) { + int count = 0; + for (final PairDTree elem : values) { + if (elem.first != sample.first) { + continue; + } + if (elem.second != sample.second) { + continue; + } + count++; + } + return count; + } + + public final DTree first; + + public final DTree second; + + public PairDTree(final DTree first, final DTree second) { + if (first.uid < second.uid) { + this.first = first; + this.second = second; + } else { + this.first = second; + this.second = first; + + } + } + + @Override + public boolean equals(final Object obj) { + if (this == obj) { + return true; + } + if (obj == null || getClass() != obj.getClass()) { + return false; + } + final PairDTree tmp = (PairDTree) obj; + if (this.first != tmp.first) { + return false; + } + if (this.second != tmp.second) { + return false; + } + return true; + } + + @Override + public String toString() { + return "PairDTree [first=" + this.first.uid + ", second=" + this.second.uid + "]"; + } +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/CollisionDispatch.java b/src/org/atriasoft/ephysics/collision/narrowphase/CollisionDispatch.java new file mode 100644 index 0000000..dedceb3 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/CollisionDispatch.java @@ -0,0 +1,21 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.shapes.CollisionShapeType; + +/** + * @biref Abstract base class for dispatching the narrow-phase + * collision detection algorithm. Collision dispatching decides which collision + * algorithm to use given two types of proxy collision shapes. + */ +public abstract class CollisionDispatch { + + /// Initialize the collision dispatch configuration + public void init(final CollisionDetection _collisionDetection) { + // Nothing to do ... + } + + /// Select and return the narrow-phase collision detection algorithm to + /// use between two types of collision shapes. + public abstract NarrowPhaseAlgorithm selectAlgorithm(CollisionShapeType _shape1Type, CollisionShapeType _shape2Type); +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java new file mode 100644 index 0000000..ff75532 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java @@ -0,0 +1,355 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; + +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.CollisionShapeInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.shapes.AABB; +import org.atriasoft.ephysics.collision.shapes.CollisionShapeType; +import org.atriasoft.ephysics.collision.shapes.ConcaveShape; +import org.atriasoft.ephysics.collision.shapes.ConvexShape; +import org.atriasoft.ephysics.collision.shapes.TriangleShape; +import org.atriasoft.ephysics.constraint.ContactPointInfo; +import org.atriasoft.ephysics.engine.OverlappingPair; +import org.atriasoft.ephysics.mathematics.Mathematics; +import org.atriasoft.ephysics.mathematics.PairIntVector3f; + +//static boolean sortFunction(SmoothMeshContactInfo&_contact1,SmoothMeshContactInfo&_contact2){return _contact1.contactInfo.penetrationDepth<=_contact2.contactInfo.penetrationDepth;} + +/** + * This class is used to compute the narrow-phase collision detection + * between a concave collision shape and a convex collision shape. The idea is + * to use the GJK collision detection algorithm to compute the collision between + * the convex shape and each of the triangles in the concave shape. + */ +public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm { + /** + * This class is used to encapsulate a callback method for + * collision detection between the triangle of a concave mesh shape + * and a convex shape. + */ + class ConvexVsTriangleCallback implements ConcaveShape.TriangleCallback { + + protected CollisionDetection collisionDetection; //!< Pointer to the collision detection object + protected NarrowPhaseCallback narrowPhaseCallback; //!< Narrow-phase collision callback + protected ConvexShape convexShape; //!< Convex collision shape to test collision with + protected ConcaveShape concaveShape; //!< Concave collision shape + protected ProxyShape convexProxyShape; //!< Proxy shape of the convex collision shape + protected ProxyShape concaveProxyShape; //!< Proxy shape of the concave collision shape + protected OverlappingPair overlappingPair; //!< Broadphase overlapping pair + + // protected static boolean contactsDepthCompare(ContactPointInfo _contact1, ContactPointInfo _contact2); + + /// Set the collision detection pointer + public void setCollisionDetection(final CollisionDetection _collisionDetection) { + this.collisionDetection = _collisionDetection; + } + + /// Set the concave collision shape + public void setConcaveShape(final ConcaveShape _concaveShape) { + this.concaveShape = _concaveShape; + } + + /// Set the convex collision shape to test collision with + public void setConvexShape(final ConvexShape _convexShape) { + this.convexShape = _convexShape; + } + + /// Set the narrow-phase collision callback + public void setNarrowPhaseCallback(final NarrowPhaseCallback _callback) { + this.narrowPhaseCallback = _callback; + } + + /// Set the broadphase overlapping pair + public void setOverlappingPair(final OverlappingPair _overlappingPair) { + this.overlappingPair = _overlappingPair; + } + + /// Set the proxy shapes of the two collision shapes + public void setProxyShapes(final ProxyShape _convexProxyShape, final ProxyShape _concaveProxyShape) { + this.convexProxyShape = _convexProxyShape; + this.concaveProxyShape = _concaveProxyShape; + } + + /// Test collision between a triangle and the convex mesh shape + @Override + public void testTriangle(final Vector3f[] _trianglePoints) { + // Create a triangle collision shape + final float margin = this.concaveShape.getTriangleMargin(); + + final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin); + // Select the collision algorithm to use between the triangle and the convex shape + final NarrowPhaseAlgorithm algo = this.collisionDetection.getCollisionAlgorithm(triangleShape.getType(), this.convexShape.getType()); + // If there is no collision algorithm between those two kinds of shapes + if (algo == null) { + return; + } + // Notify the narrow-phase algorithm about the overlapping pair we are going to test + algo.setCurrentOverlappingPair(this.overlappingPair); + // Create the CollisionShapeInfo objects + final CollisionShapeInfo shapeConvexInfo = new CollisionShapeInfo(this.convexProxyShape, this.convexShape, this.convexProxyShape.getLocalToWorldTransform(), this.overlappingPair, + this.convexProxyShape.getCachedCollisionData()); + + final CollisionShapeInfo shapeConcaveInfo = new CollisionShapeInfo(this.concaveProxyShape, triangleShape, this.concaveProxyShape.getLocalToWorldTransform(), this.overlappingPair, + this.concaveProxyShape.getCachedCollisionData()); + // Use the collision algorithm to test collision between the triangle and the other convex shape + algo.testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback); + } + + } + + /** + * This class is used as a narrow-phase callback to get narrow-phase contacts + * of the concave triangle mesh to temporary store them in order to be used in + * the smooth mesh collision algorithm if this one is enabled. + */ + class SmoothCollisionNarrowPhaseCallback implements NarrowPhaseCallback { + private final List contactPoints; + + // Constructor + public SmoothCollisionNarrowPhaseCallback(final List _contactPoints) { + this.contactPoints = new ArrayList<>(_contactPoints); + } + + /// Called by a narrow-phase collision algorithm when a new contact has been found + @Override + public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) { + final Vector3f[] triangleVertices = new Vector3f[3]; + boolean isFirstShapeTriangle; + // If the collision shape 1 is the triangle + if (_contactInfo.collisionShape1.getType() == CollisionShapeType.TRIANGLE) { + assert (_contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE); + final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape1; + triangleVertices[0] = triangleShape.getVertex(0); + triangleVertices[1] = triangleShape.getVertex(1); + triangleVertices[2] = triangleShape.getVertex(2); + isFirstShapeTriangle = true; + } else { // If the collision shape 2 is the triangle + assert (_contactInfo.collisionShape2.getType() == CollisionShapeType.TRIANGLE); + final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape2; + triangleVertices[0] = triangleShape.getVertex(0); + triangleVertices[1] = triangleShape.getVertex(1); + triangleVertices[2] = triangleShape.getVertex(2); + isFirstShapeTriangle = false; + } + + final SmoothMeshContactInfo smoothContactInfo = new SmoothMeshContactInfo(_contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); + // Add the narrow-phase contact into the list of contact to process for + // smooth mesh collision + this.contactPoints.add(smoothContactInfo); + } + + } + + /** + * This class is used to store data about a contact with a triangle for the smooth + * mesh algorithm. + */ + class SmoothMeshContactInfo { + public ContactPointInfo contactInfo; + public boolean isFirstShapeTriangle; + + public Vector3f[] triangleVertices = new Vector3f[3]; + + public SmoothMeshContactInfo() { + // TODO: add it for List + } + + /// Constructor + public SmoothMeshContactInfo(final ContactPointInfo _contact, final boolean _firstShapeTriangle, final Vector3f _trianglePoint1, final Vector3f _trianglePoint2, + final Vector3f _trianglePoint3) { + this.contactInfo = new ContactPointInfo(_contact); + this.isFirstShapeTriangle = _firstShapeTriangle; + this.triangleVertices[0] = _trianglePoint1; + this.triangleVertices[1] = _trianglePoint2; + this.triangleVertices[2] = _trianglePoint3; + } + } + + /// Constructor + public ConcaveVsConvexAlgorithm(final CollisionDetection collisionDetection) { + super(collisionDetection); + } + + /// Add a triangle vertex into the set of processed triangles + protected void addProcessedVertex(final List _processTriangleVertices, final Vector3f _vertex) { + _processTriangleVertices.add(new PairIntVector3f((int) (_vertex.x * _vertex.y * _vertex.z), _vertex)); + } + + /// Return true if the vertex is in the set of already processed vertices + protected boolean hasVertexBeenProcessed(final List _processTriangleVertices, final Vector3f _vertex) { + /* TODO : List> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good... + int key = int(_vertex.x * _vertex.y * _vertex.z); + auto range = _processTriangleVertices.equal_range(key); + for (auto it = range.first; it != range.second; ++it) { + if ( _vertex.x == it.second.x + && _vertex.y == it.second.y + && _vertex.z == it.second.z) { + return true; + } + } + return false; + */ + // TODO : This is not really the same ... + for (final PairIntVector3f it : _processTriangleVertices) { + if (_vertex.x == it.second.x && _vertex.y == it.second.y && _vertex.z == it.second.z) { + return true; + } + } + return false; + } + + /// Process the concave triangle mesh collision using the smooth mesh collision algorithm + protected void processSmoothMeshCollision(final OverlappingPair _overlappingPair, final List _contactPoints, final NarrowPhaseCallback _callback) { + // Set with the triangle vertices already processed to void further contacts with same triangle + final List processTriangleVertices = new ArrayList<>(); + // Sort the list of narrow-phase contacts according to their penetration depth + _contactPoints.sort(new Comparator() { + @Override + public int compare(final SmoothMeshContactInfo _pair1, final SmoothMeshContactInfo _pair2) { + if (_pair1.contactInfo.penetrationDepth < _pair2.contactInfo.penetrationDepth) { + return -1; + } + if (_pair1.contactInfo.penetrationDepth == _pair2.contactInfo.penetrationDepth) { + return 0; + } + return +1; + } + }); + + // For each contact point (from smaller penetration depth to larger) + for (final SmoothMeshContactInfo info : _contactPoints) { + final Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + // Compute the barycentric coordinates of the point in the triangle + final Float u = 0.0f, v = 0.0f, w = 0.0f; + Mathematics.computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], info.triangleVertices[1], info.triangleVertices[2], contactPoint, u, v, w); + int nbZeros = 0; + final boolean isUZero = Mathematics.ApproxEqual(u, 0.0f, 0.0001f); + final boolean isVZero = Mathematics.ApproxEqual(v, 0.0f, 0.0001f); + final boolean isWZero = Mathematics.ApproxEqual(w, 0.0f, 0.0001f); + if (isUZero) { + nbZeros++; + } + if (isVZero) { + nbZeros++; + } + if (isWZero) { + nbZeros++; + } + // If it is a vertex contact + if (nbZeros == 2) { + final Vector3f contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); + // Check that this triangle vertex has not been processed yet + if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { + // Keep the contact as it is and report it + _callback.notifyContact(_overlappingPair, info.contactInfo); + } + } else if (nbZeros == 1) { + // If it is an edge contact + final Vector3f contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); + final Vector3f contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); + // Check that this triangle edge has not been processed yet + if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { + // Keep the contact as it is and report it + _callback.notifyContact(_overlappingPair, info.contactInfo); + } + } else { + // If it is a face contact + final ContactPointInfo newContactInfo = new ContactPointInfo(info.contactInfo); + ProxyShape firstShape; + ProxyShape secondShape; + if (info.isFirstShapeTriangle) { + firstShape = _overlappingPair.getShape1(); + secondShape = _overlappingPair.getShape2(); + } else { + firstShape = _overlappingPair.getShape2(); + secondShape = _overlappingPair.getShape1(); + } + // We use the triangle normal as the contact normal + final Vector3f a = info.triangleVertices[1].lessNew(info.triangleVertices[0]); + final Vector3f b = info.triangleVertices[2].lessNew(info.triangleVertices[0]); + final Vector3f localNormal = a.cross(b); + newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation().multiply(localNormal); + final Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + final Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform().multiplyNew(firstLocalPoint); + newContactInfo.normal.normalize(); + if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { + newContactInfo.normal.multiply(-1); + } + // We recompute the contact point on the second body with the new normal as described in + // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and + // Dirk Gregorius) to avoid adding torque + final Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().inverseNew(); + if (info.isFirstShapeTriangle) { + final Vector3f newSecondWorldPoint = firstWorldPoint.addNew(newContactInfo.normal); + newContactInfo.localPoint2 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint); + } else { + final Vector3f newSecondWorldPoint = firstWorldPoint.lessNew(newContactInfo.normal); + newContactInfo.localPoint1 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint); + } + // Report the contact + _callback.notifyContact(_overlappingPair, newContactInfo); + } + + // Add the three vertices of the triangle to the set of processed + // triangle vertices + addProcessedVertex(processTriangleVertices, info.triangleVertices[0]); + addProcessedVertex(processTriangleVertices, info.triangleVertices[1]); + addProcessedVertex(processTriangleVertices, info.triangleVertices[2]); + } + + } + + /// Compute a contact info if the two bounding volume collide + @Override + public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final NarrowPhaseCallback _callback) { + ProxyShape convexProxyShape; + ProxyShape concaveProxyShape; + ConvexShape convexShape; + ConcaveShape concaveShape; + // Collision shape 1 is convex, collision shape 2 is concave + if (_shape1Info.collisionShape.isConvex()) { + convexProxyShape = _shape1Info.proxyShape; + convexShape = (ConvexShape) _shape1Info.collisionShape; + concaveProxyShape = _shape2Info.proxyShape; + concaveShape = (ConcaveShape) _shape2Info.collisionShape; + } else { + // Collision shape 2 is convex, collision shape 1 is concave + convexProxyShape = _shape2Info.proxyShape; + convexShape = (ConvexShape) _shape2Info.collisionShape; + concaveProxyShape = _shape1Info.proxyShape; + concaveShape = (ConcaveShape) _shape1Info.collisionShape; + } + // Set the parameters of the callback object + final ConvexVsTriangleCallback convexVsTriangleCallback = new ConvexVsTriangleCallback(); + convexVsTriangleCallback.setCollisionDetection(this.collisionDetection); + convexVsTriangleCallback.setConvexShape(convexShape); + convexVsTriangleCallback.setConcaveShape(concaveShape); + convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape); + convexVsTriangleCallback.setOverlappingPair(_shape1Info.overlappingPair); + // Compute the convex shape AABB in the local-space of the convex shape + final AABB aabb = new AABB(); + convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform()); + // If smooth mesh collision is enabled for the concave mesh + if (concaveShape.getIsSmoothMeshCollisionEnabled()) { + final List contactPoints = new ArrayList<>(); + + final SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback = new SmoothCollisionNarrowPhaseCallback(contactPoints); + convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback); + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape.testAllTriangles(convexVsTriangleCallback, aabb); + // Run the smooth mesh collision algorithm + processSmoothMeshCollision(_shape1Info.overlappingPair, contactPoints, _callback); + } else { + convexVsTriangleCallback.setNarrowPhaseCallback(_callback); + // Call the convex vs triangle callback for each triangle of the concave shape + concaveShape.testAllTriangles(convexVsTriangleCallback, aabb); + } + } +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java b/src/org/atriasoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java new file mode 100644 index 0000000..77404ce --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java @@ -0,0 +1,46 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm; +import org.atriasoft.ephysics.collision.shapes.CollisionShape; +import org.atriasoft.ephysics.collision.shapes.CollisionShapeType; + +/** + * This is the default collision dispatch configuration use in ephysics. + * Collision dispatching decides which collision + * algorithm to use given two types of proxy collision shapes. + */ +public class DefaultCollisionDispatch extends CollisionDispatch { + + //!< Sphere vs Sphere collision algorithm + protected SphereVsSphereAlgorithm sphereVsSphereAlgorithm; + //!< Concave vs Convex collision algorithm + protected ConcaveVsConvexAlgorithm concaveVsConvexAlgorithm; + //!< GJK Algorithm + protected GJKAlgorithm GJKAlgorithm; + + @Override + public void init(final CollisionDetection _collisionDetection) { + // Initialize the collision algorithms + this.sphereVsSphereAlgorithm = new SphereVsSphereAlgorithm(_collisionDetection); + this.GJKAlgorithm = new GJKAlgorithm(_collisionDetection); + this.concaveVsConvexAlgorithm = new ConcaveVsConvexAlgorithm(_collisionDetection); + } + + @Override + public NarrowPhaseAlgorithm selectAlgorithm(final CollisionShapeType _type1, final CollisionShapeType _type2) { + // Sphere vs Sphere algorithm + if (_type1 == CollisionShapeType.SPHERE && _type2 == CollisionShapeType.SPHERE) { + return this.sphereVsSphereAlgorithm; + } else if ((!CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) || (!CollisionShape.isConvex(_type2) && CollisionShape.isConvex(_type1))) { + // Concave vs Convex algorithm + return this.concaveVsConvexAlgorithm; + } else if (CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) { + // Convex vs Convex algorithm (GJK algorithm) + return this.GJKAlgorithm; + } else { + return null; + } + } + +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java new file mode 100644 index 0000000..116c3e9 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java @@ -0,0 +1,423 @@ +package org.atriasoft.ephysics.collision.narrowphase.EPA; + +import java.util.Comparator; +import java.util.SortedSet; +import java.util.TreeSet; + +import org.atriasoft.ephysics.collision.CollisionShapeInfo; +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback; +import org.atriasoft.ephysics.collision.narrowphase.GJK.GJKAlgorithm; +import org.atriasoft.ephysics.collision.narrowphase.GJK.Simplex; +import org.atriasoft.ephysics.collision.shapes.CacheData; +import org.atriasoft.ephysics.collision.shapes.ConvexShape; +import org.atriasoft.ephysics.constraint.ContactPointInfo; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +/** + * Class EPAAlgorithm + * This class is the implementation of the Expanding Polytope Algorithm (EPA). + * The EPA algorithm computes the penetration depth and contact points between + * two enlarged objects (with margin) where the original objects (without margin) + * intersect. The penetration depth of a pair of intersecting objects A and B is + * the length of a point on the boundary of the Minkowski sum (A-B) closest to the + * origin. The goal of the EPA algorithm is to start with an initial simplex polytope + * that contains the origin and expend it in order to find the point on the boundary + * of (A-B) that is closest to the origin. An initial simplex that contains origin + * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex + * polytope to find the correct penetration depth. The implementation of the EPA + * algorithm is based on the book "Collision Detection in 3D Environments". + */ +public class EPAAlgorithm { + /// Add a triangle face in the candidate triangle heap + private void addFaceCandidate(final TriangleEPA _triangle, final SortedSet _heap, final float _upperBoundSquarePenDepth) { + ////Log.info("addFaceCandidate: " + _triangle.get(0) + ", " + _triangle.get(1) + ", " + _triangle.get(2) + " " + _upperBoundSquarePenDepth); + // 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()); + if (_triangle.isClosestPointInternalToTriangle() && _triangle.getDistSquare() <= _upperBoundSquarePenDepth) { + // Add the triangle face to the list of candidates + _heap.add(_triangle); + ////Log.info("add in heap:"); + //int iii = 0; + //for (final TriangleEPA elem : _heap) { + // ////Log.info(" [" + iii + "] " + elem.getDistSquare()); + // ++iii; + //} + } + } + + // Compute the penetration depth with the EPA algorithm. + /// This method computes the penetration depth and contact points between two + /// enlarged objects (with margin) where the original objects (without margin) + /// intersect. An initial simplex that contains origin has been computed with + /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find + /// the correct penetration depth + public void computePenetrationDepthAndContactPoints(final Simplex _simplex, final CollisionShapeInfo _shape1Info, final Transform3D _transform1, final CollisionShapeInfo _shape2Info, + final Transform3D _transform2, final Vector3f _vector, final NarrowPhaseCallback _narrowPhaseCallback) { + ////Log.info("computePenetrationDepthAndContactPoints()"); + assert (_shape1Info.collisionShape.isConvex()); + assert (_shape2Info.collisionShape.isConvex()); + final ConvexShape shape1 = (ConvexShape) (_shape1Info.collisionShape); + final ConvexShape shape2 = (ConvexShape) (_shape2Info.collisionShape); + final CacheData shape1CachedCollisionData = (CacheData) _shape1Info.cachedCollisionData; + final CacheData shape2CachedCollisionData = (CacheData) _shape2Info.cachedCollisionData; + final Vector3f suppPointsA[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates + final Vector3f suppPointsB[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates + final Vector3f points[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Current points + final TrianglesStore triangleStore = new TrianglesStore(); // Store the triangles + + //https://docs.oracle.com/en/java/javase/11/docs/api/java.base/java/util/SortedSet.html + // https://stackoverflow.com/questions/38066291/how-to-define-comparator-on-sortedset-like-treeset + final SortedSet triangleHeap = new TreeSet<>(new Comparator() { + @Override + public int compare(final TriangleEPA _face1, final TriangleEPA _face2) { + final float d1 = _face1.getDistSquare(); + final float d2 = _face2.getDistSquare(); + if (d1 < d2) { + return -1; + } + if (d1 > d2) { + return 1; + } + return 0; + } + }); + // 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().multiplyNew(_transform2); + // Matrix that transform a direction from local + // space of body 1 into local space of body 2 + final Quaternion rotateToBody2 = _transform2.getOrientation().inverseNew().multiplyNew(_transform1.getOrientation()); + // Get the simplex computed previously by the GJK algorithm + int nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points); + // Compute the tolerance + final float tolerance = Constant.FLOAT_EPSILON * _simplex.getMaxLengthSquareOfAPoint(); + // Clear the storing of triangles + triangleStore.clear(); + // 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); + switch (nbVertices) { + case 1: + // Only one point in the simplex (which should be the origin). + // We have a touching contact with zero penetration depth. + // We drop that kind of contact. Therefore, we return false + return; + case 2: { + // The simplex returned by GJK is a line segment d containing the origin. + // We add two additional support points to ruct a hexahedron (two tetrahedron + // glued together with triangle faces. The idea is to compute three different vectors + // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively + // rotated of 120 degree around the d segment. The the three new points to + // ruct the polytope are the three support points in those three directions + // v1, v2 and v3. + // Direction of the segment + final Vector3f d = points[1].lessNew(points[0]).safeNormalizeNew(); + // Choose the coordinate axis from the minimal absolute component of the vector d + final int minAxis = d.abs().getMinAxis(); + // Compute sin(60) + final float sin60 = FMath.sqrt(3.0f) * 0.5f; + // Create a rotation quaternion to rotate the vector v1 to get the vectors + // v2 and v3 + final Quaternion rotationQuat = new Quaternion(d.x * sin60, d.y * sin60, d.z * sin60, 0.5f); + // Compute the vector v1, v2, v3 + final Vector3f v1 = d.cross(new Vector3f(minAxis == 0 ? 1.0f : 0.0f, minAxis == 1 ? 1.0f : 0.0f, minAxis == 2 ? 1.0f : 0.0f)); + final Vector3f v2 = rotationQuat.multiply(v1); + final Vector3f v3 = rotationQuat.multiply(v2); + // Compute the support point in the direction of v1 + suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); + suppPointsB[2] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v1.multiplyNew(-1)), shape2CachedCollisionData)); + points[2] = suppPointsA[2].lessNew(suppPointsB[2]); + // Compute the support point in the direction of v2 + suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); + suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v2.multiplyNew(-1)), shape2CachedCollisionData)); + points[3] = suppPointsA[3].lessNew(suppPointsB[3]); + // Compute the support point in the direction of v3 + suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); + suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v3.multiplyNew(-1)), shape2CachedCollisionData)); + points[4] = suppPointsA[4].lessNew(suppPointsB[4]); + // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the + // tetrahedron that contains the origin in order that the initial polytope of the + // EPA algorithm is a tetrahedron, which is simpler to deal with. + // If the origin is in the tetrahedron of points 0, 2, 3, 4 + if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) { + // We use the point 4 instead of point 1 for the initial tetrahedron + suppPointsA[1] = suppPointsA[4]; + suppPointsB[1] = suppPointsB[4]; + points[1] = points[4]; + } + // If the origin is in the tetrahedron of points 1, 2, 3, 4 + else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { + // We use the point 4 instead of point 0 for the initial tetrahedron + suppPointsA[0] = suppPointsA[4]; + suppPointsB[0] = suppPointsB[4]; + points[0] = points[4]; + } else { + // The origin is not in the initial polytope + return; + } + // The polytope contains now 4 vertices + nbVertices = 4; + } + case 4: { + // The simplex computed by the GJK algorithm is a tetrahedron. Here we check + // if this tetrahedron contains the origin. If it is the case, we keep it and + // otherwise we remove the wrong vertex of the tetrahedron and go in the case + // where the GJK algorithm compute a simplex of three vertices. + // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise) + final int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]); + // If the origin is in the tetrahedron + if (badVertex == 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)"); + final TriangleEPA face0 = triangleStore.newTriangle(points, 0, 1, 2); + ////Log.error("befor call: (points, 0, 3, 1)"); + final TriangleEPA face1 = triangleStore.newTriangle(points, 0, 3, 1); + ////Log.error("befor call: (points, 0, 2, 3)"); + final TriangleEPA face2 = triangleStore.newTriangle(points, 0, 2, 3); + ////Log.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 + && face3.getDistSquare() > 0.0)) { + return; + } + // Associate the edges of neighbouring triangle faces + TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2)); + TriangleEPA.link(new EdgeEPA(face0, 1), new EdgeEPA(face3, 2)); + TriangleEPA.link(new EdgeEPA(face0, 2), new EdgeEPA(face2, 0)); + TriangleEPA.link(new EdgeEPA(face1, 0), new EdgeEPA(face2, 2)); + TriangleEPA.link(new EdgeEPA(face1, 1), new EdgeEPA(face3, 0)); + TriangleEPA.link(new EdgeEPA(face2, 1), new EdgeEPA(face3, 1)); + // Add the triangle faces in the candidate heap + addFaceCandidate(face0, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face1, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face2, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face3, triangleHeap, Float.MAX_VALUE); + break; + } + // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) + // Remove the wrong vertex and continue to the next case with the + // three remaining vertices + if (badVertex < 4) { + suppPointsA[badVertex - 1] = suppPointsA[3]; + suppPointsB[badVertex - 1] = suppPointsB[3]; + points[badVertex - 1] = points[3]; + } + // We have removed the wrong vertex + nbVertices = 3; + } + case 3: { + // The GJK algorithm returned a triangle that contains the origin. + // We need two new vertices to create two tetrahedron. The two new + // vertices are the support points in the "n" and "-n" direction + // where "n" is the normal of the triangle. Then, we use only the + // tetrahedron that contains the origin. + // Compute the normal of the triangle + final Vector3f v1 = points[1].lessNew(points[0]); + final Vector3f v2 = points[2].lessNew(points[0]); + final Vector3f n = v1.cross(v2); + ////Log.info(">>>>>>>>>>>>>>>>>>"); + ////Log.info(" v1 = " + v1); + ////Log.info(" v2 = " + v2); + ////Log.info(" n = " + n); + // Compute the two new vertices to obtain a hexahedron + suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData); + suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiplyNew(-1)), shape2CachedCollisionData)); + points[3] = suppPointsA[3].lessNew(suppPointsB[3]); + ////Log.info(" suppPointsA[3]= " + suppPointsA[3]); + ////Log.info(" suppPointsB[3]= " + suppPointsB[3]); + ////Log.info(" points[3] = " + points[3]); + suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiplyNew(-1), shape1CachedCollisionData); + suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData)); + points[4] = suppPointsA[4].lessNew(suppPointsB[4]); + ////Log.info(" suppPointsA[4]= " + suppPointsA[4]); + ////Log.info(" suppPointsB[4]= " + suppPointsB[4]); + ////Log.info(" points[4]= " + points[4]); + TriangleEPA face0 = null; + TriangleEPA face1 = null; + TriangleEPA face2 = null; + TriangleEPA face3 = null; + // If the origin is in the first tetrahedron + if (isOriginInTetrahedron(points[0], points[1], points[2], points[3]) == 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)"); + face0 = triangleStore.newTriangle(points, 0, 1, 2); + ////Log.error("befor call: (points, 0, 2, 1)"); + face1 = triangleStore.newTriangle(points, 0, 3, 1); + ////Log.error("befor call: (points, 0, 2, 3)"); + face2 = triangleStore.newTriangle(points, 0, 2, 3); + ////Log.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)"); + face0 = triangleStore.newTriangle(points, 0, 1, 2); + ////Log.error("befor call: (points, 0, 4, 1)"); + face1 = triangleStore.newTriangle(points, 0, 4, 1); + ////Log.error("befor call: (points, 0, 2, 4)"); + face2 = triangleStore.newTriangle(points, 0, 2, 4); + ////Log.error("befor call: (points, 1, 4, 2)"); + face3 = triangleStore.newTriangle(points, 1, 4, 2); + } else { + return; + } + // If the ructed tetrahedron is not correct + if (!(face0 != null && face1 != null && face2 != null && face3 != null && face0.getDistSquare() > 0.0f && face1.getDistSquare() > 0.0f && face2.getDistSquare() > 0.0f + && face3.getDistSquare() > 0.0f)) { + return; + } + // Associate the edges of neighbouring triangle faces + TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2)); + TriangleEPA.link(new EdgeEPA(face0, 1), new EdgeEPA(face3, 2)); + TriangleEPA.link(new EdgeEPA(face0, 2), new EdgeEPA(face2, 0)); + TriangleEPA.link(new EdgeEPA(face1, 0), new EdgeEPA(face2, 2)); + TriangleEPA.link(new EdgeEPA(face1, 1), new EdgeEPA(face3, 0)); + TriangleEPA.link(new EdgeEPA(face2, 1), new EdgeEPA(face3, 1)); + // Add the triangle faces in the candidate heap + addFaceCandidate(face0, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face1, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face2, triangleHeap, Float.MAX_VALUE); + addFaceCandidate(face3, triangleHeap, Float.MAX_VALUE); + nbVertices = 4; + } + break; + } + // At this point, we have a polytope that contains the origin. Therefore, we + // can run the EPA algorithm. + if (triangleHeap.size() == 0) { + return; + } + TriangleEPA triangle = null; + float upperBoundSquarePenDepth = Float.MAX_VALUE; + do { + triangle = triangleHeap.first(); + triangleHeap.remove(triangle); + ////Log.info("rm from heap:"); + int iii = 0; + for (final TriangleEPA elem : triangleHeap) { + ////Log.info(" [" + iii + "] " + elem.getDistSquare()); + ++iii; + } + // If the candidate face in the heap is not obsolete + if (!triangle.getIsObsolete()) { + // If we have reached the maximum number of support points + if (nbVertices == EdgeEPA.MAX_SUPPORT_POINTS) { + assert (false); + break; + } + // Compute the support point of the Minkowski + // difference (A-B) in the closest point direction + suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData); + suppPointsB[nbVertices] = body2Tobody1 + .multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiplyNew(-1)), shape2CachedCollisionData)); + points[nbVertices] = suppPointsA[nbVertices].lessNew(suppPointsB[nbVertices]); + final int indexNewVertex = nbVertices; + 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); + if (wDotv < 0.0f) { + ////Log.error("depth penetration error " + wDotv); + continue; + } + assert (wDotv >= 0.0f); + final float wDotVSquare = wDotv * wDotv / triangle.getDistSquare(); + if (wDotVSquare < upperBoundSquarePenDepth) { + upperBoundSquarePenDepth = wDotVSquare; + } + // Compute the error + final float error = wDotv - triangle.getDistSquare(); + if (error <= FMath.max(tolerance, GJKAlgorithm.REL_ERROR_SQUARE * wDotv) || points[indexNewVertex].isEqual(points[triangle.get(0)]) + || points[indexNewVertex].isEqual(points[triangle.get(1)]) || points[indexNewVertex].isEqual(points[triangle.get(2)])) { + break; + } + // Now, we compute the silhouette cast by the new vertex. The current triangle + // face will not be in the convex hull. We start the local recursive silhouette + // algorithm from the current triangle face. + int i = triangleStore.getNbTriangles(); + if (!triangle.computeSilhouette(points, indexNewVertex, triangleStore)) { + break; + } + // Add all the new triangle faces computed with the silhouette algorithm + // to the candidates list of faces of the current polytope + while (i != triangleStore.getNbTriangles()) { + final TriangleEPA newTriangle = triangleStore.get(i); + addFaceCandidate(newTriangle, triangleHeap, upperBoundSquarePenDepth); + i++; + } + } + } while (triangleHeap.size() > 0 && triangleHeap.first().getDistSquare() <= upperBoundSquarePenDepth); + // Compute the contact info + final Vector3f tmp = _transform1.getOrientation().multiply(triangle.getClosestPoint()); + _vector.set(tmp); + final Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA); + final Vector3f pBLocal = body2Tobody1.inverseNew().multiply(triangle.computeClosestPointOfObject(suppPointsB)); + final Vector3f normal = _vector.safeNormalizeNew(); + final float penetrationDepth = _vector.length(); + assert (penetrationDepth >= 0.0f); + if (normal.length2() < Constant.FLOAT_EPSILON) { + return; + } + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, normal, penetrationDepth, + pALocal, pBLocal); + _narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo); + } + + /// Initalize the algorithm + public void init() { + + } + + // Decide if the origin is in the tetrahedron. + /// 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 + ")"); + // Check vertex 1 + final Vector3f normal1 = _p2.lessNew(_p1).cross(_p3.lessNew(_p1)); + if ((normal1.dot(_p1) > 0.0f) == (normal1.dot(_p4) > 0.0)) { + ////Log.error(" ==> 4"); + return 4; + } + // Check vertex 2 + final Vector3f normal2 = _p4.lessNew(_p2).cross(_p3.lessNew(_p2)); + if ((normal2.dot(_p2) > 0.0f) == (normal2.dot(_p1) > 0.0)) { + ////Log.error(" ==> 1"); + return 1; + } + // Check vertex 3 + final Vector3f normal3 = _p4.lessNew(_p3).cross(_p1.lessNew(_p3)); + if ((normal3.dot(_p3) > 0.0f) == (normal3.dot(_p2) > 0.0)) { + ////Log.error(" ==> 2"); + return 2; + } + // Check vertex 4 + final Vector3f normal4 = _p2.lessNew(_p4).cross(_p1.lessNew(_p4)); + if ((normal4.dot(_p4) > 0.0f) == (normal4.dot(_p3) > 0.0)) { + ////Log.error(" ==> 3"); + return 3; + } + ////Log.error(" ==> 0"); + // The origin is in the tetrahedron, we return 0 + return 0; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java new file mode 100644 index 0000000..5396e82 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java @@ -0,0 +1,138 @@ +package org.atriasoft.ephysics.collision.narrowphase.EPA; + +import org.atriasoft.etk.math.Vector3f; + +/** + * Class EPAAlgorithm + * This class is the implementation of the Expanding Polytope Algorithm (EPA). + * The EPA algorithm computes the penetration depth and contact points between + * two enlarged objects (with margin) where the original objects (without margin) + * intersect. The penetration depth of a pair of intersecting objects A and B is + * the length of a point on the boundary of the Minkowski sum (A-B) closest to the + * origin. The goal of the EPA algorithm is to start with an initial simplex polytope + * that contains the origin and expend it in order to find the point on the boundary + * of (A-B) that is closest to the origin. An initial simplex that contains origin + * has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex + * polytope to find the correct penetration depth. The implementation of the EPA + * algorithm is based on the book "Collision Detection in 3D Environments". + */ +public class EdgeEPA implements Cloneable { + /// Maximum number of support points of the polytope + static public final int MAX_SUPPORT_POINTS = 100; + /// Maximum number of facets of the polytope + static public final int MAX_FACETS = 200; + + // Return the index of the next counter-clockwise edge of the ownver triangle + static public int indexOfNextCounterClockwiseEdge(final int iii) { + return (iii + 1) % 3; + } + + // Return the index of the previous counter-clockwise edge of the ownver triangle + static public int indexOfPreviousCounterClockwiseEdge(final int iii) { + return (iii + 2) % 3; + } + + /// Pointer to the triangle that contains this edge + private TriangleEPA ownerTriangle; + /// Index of the edge in the triangle (between 0 and 2). + /// The edge with index i connect triangle vertices i and (i+1 % 3) + private int index; + + /// Constructor + public EdgeEPA() { + this.ownerTriangle = null; + }; + + /// Copy-ructor + public EdgeEPA(final EdgeEPA obj) { + this.ownerTriangle = obj.ownerTriangle; + this.index = obj.index; + } + + /// Constructor + public EdgeEPA(final TriangleEPA ownerTriangle, final int index) { + this.ownerTriangle = ownerTriangle; + this.index = index; + assert (index >= 0 && index < 3); + } + + @Override + protected EdgeEPA clone() throws CloneNotSupportedException { + return new EdgeEPA(this); + } + + /// Execute the recursive silhouette algorithm from this edge + public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) { + //Log.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()); + final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); + // If the triangle has been created + if (triangle != null) { + TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this); + return true; + } + return false; + } else { + // The current triangle is visible and therefore obsolete + this.ownerTriangle.setIsObsolete(true); + final int backup = triangleStore.getNbTriangles(); + if (!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { + this.ownerTriangle.setIsObsolete(false); + //Log.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) { + TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this); + return true; + } + return false; + } else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { + this.ownerTriangle.setIsObsolete(false); + triangleStore.resize(backup); + //Log.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); + return true; + } + return false; + } + } + } + return true; + } + + /// Return the index of the edge in the triangle + public int getIndex() { + return this.index; + } + + /// Return the pointer to the owner triangle + public TriangleEPA getOwnerTriangle() { + return this.ownerTriangle; + } + + /// Return index of the source vertex of the edge + public int getSourceVertexIndex() { + //return this.ownerTriangle[this.index]; + return this.ownerTriangle.get(this.index); + } + + /// Return the index of the target vertex of the edge + public int getTargetVertexIndex() { + return this.ownerTriangle.get(indexOfNextCounterClockwiseEdge(this.index)); + //return this.ownerTriangle[indexOfNextCounterClockwiseEdge(this.index)]; + } + + /// Assignment + public EdgeEPA set(final EdgeEPA obj) { + this.ownerTriangle = obj.ownerTriangle; + this.index = obj.index; + return this; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java new file mode 100644 index 0000000..925b518 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java @@ -0,0 +1,223 @@ +package org.atriasoft.ephysics.collision.narrowphase.EPA; + +import org.atriasoft.etk.math.Vector3f; + +/** + * Class TriangleEPA + * This class represents a triangle face of the current polytope in the EPA algorithm. + */ +public class TriangleEPA { + /// Make an half link of an edge with another one from another triangle. An half-link + /// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an + /// adjacent edge of "edge0" but not the opposite. The opposite edge connection will + /// be made later. + public static void halfLink(final EdgeEPA _edge0, final EdgeEPA _edge1) { + assert (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()); + _edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1; + } + + public static boolean link(final EdgeEPA _edge0, final EdgeEPA _edge1) { + if (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()) { + _edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1; + _edge1.getOwnerTriangle().adjacentEdges[_edge1.getIndex()] = _edge0; + return true; + } + return false; + } + + private final int[] indicesVertices = new int[3]; //!< Indices of the vertices y_i of the triangle + private final EdgeEPA[] adjacentEdges = new EdgeEPA[3]; //!< Three adjacent edges of the triangle (edges of other triangles) + private boolean isObsolete; //!< True if the triangle face is visible from the new support point + private float determinant; //!< Determinant + private Vector3f closestPoint; //!< Point v closest to the origin on the affine hull of the triangle + private float lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + + private float lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2 + + private float distSquare; //!< Square distance of the point closest point v to the origin + + /// Constructor + /* + public TriangleEPA() { + this.adjacentEdges[0] = new EdgeEPA(this, 0); + this.adjacentEdges[1] = new EdgeEPA(this, 1); + this.adjacentEdges[2] = new EdgeEPA(this, 2); + this.closestPoint = new Vector3f(); + } + */ + + /// Constructor + public TriangleEPA(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) { + this.isObsolete = false; + this.indicesVertices[0] = _indexVertex1; + this.indicesVertices[1] = _indexVertex2; + this.indicesVertices[2] = _indexVertex3; + this.adjacentEdges[0] = new EdgeEPA(); + this.adjacentEdges[1] = new EdgeEPA(); + this.adjacentEdges[2] = new EdgeEPA(); + this.closestPoint = new Vector3f(); + } + + /// Private copy-ructor + /* + public TriangleEPA(final TriangleEPA _triangle) { + this.indicesVertices[0] = _triangle.indicesVertices[0]; + this.indicesVertices[1] = _triangle.indicesVertices[1]; + this.indicesVertices[2] = _triangle.indicesVertices[2]; + this.adjacentEdges[0] = _triangle.adjacentEdges[0]; + this.adjacentEdges[1] = _triangle.adjacentEdges[1]; + this.adjacentEdges[2] = _triangle.adjacentEdges[2]; + this.isObsolete = _triangle.isObsolete; + this.determinant = _triangle.determinant; + this.closestPoint = _triangle.closestPoint; + this.lambda1 = _triangle.lambda1; + this.lambda2 = _triangle.lambda2; + this.distSquare = _triangle.distSquare; + } + */ + + /// Compute the point v closest to the origin of this triangle + public boolean computeClosestPoint(final Vector3f[] _vertices) { + final Vector3f p0 = _vertices[this.indicesVertices[0]]; + final Vector3f v1 = _vertices[this.indicesVertices[1]].lessNew(p0); + final Vector3f v2 = _vertices[this.indicesVertices[2]].lessNew(p0); + final float v1Dotv1 = v1.dot(v1); + final float v1Dotv2 = v1.dot(v2); + final float v2Dotv2 = v2.dot(v2); + final float p0Dotv1 = p0.dot(v1); + final float p0Dotv2 = p0.dot(v2); + // Compute determinant + this.determinant = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2; + // Compute lambda values + this.lambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2; + this.lambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1; + // If the determinant is positive + if (this.determinant > 0.0f) { + // Compute the closest point v + this.closestPoint = v1.multiplyNew(this.lambda1).add(v2.multiplyNew(this.lambda2)).multiply(1.0f / this.determinant).add(p0); + // Compute the square distance of closest point to the origin + this.distSquare = this.closestPoint.dot(this.closestPoint); + return true; + } + return false; + } + + /// Compute the point of an object closest to the origin + public Vector3f computeClosestPointOfObject(final Vector3f[] _supportPointsOfObject) { + final Vector3f p0 = _supportPointsOfObject[this.indicesVertices[0]].clone(); + final Vector3f tmp1 = _supportPointsOfObject[this.indicesVertices[1]].lessNew(p0).multiply(this.lambda1); + final Vector3f tmp2 = _supportPointsOfObject[this.indicesVertices[2]].lessNew(p0).multiply(this.lambda2); + return p0.add(tmp1.add(tmp2).multiply(1.0f / this.determinant)); + } + + // Execute the recursive silhouette algorithm from this triangle face. + /// The parameter "vertices" is an array that contains the vertices of the current polytope and the + /// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the + /// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore, + /// the triangle faces that are visible from the new vertex must be removed from the polytope and we + /// need to add triangle faces where each face contains the new vertex and an edge of the silhouette. + /// The silhouette is the connected set of edges that are part of the border between faces that + /// are seen and faces that are not seen from the new vertex. This method starts from the nearest + /// face from the new vertex, computes the silhouette and create the new faces from the new vertex in + /// order that we always have a convex polytope. The faces visible from the new vertex are set + /// obselete and will not be idered as being a candidate face in the future. + public boolean computeSilhouette(final Vector3f[] _vertices, final int _indexNewVertex, final TrianglesStore _triangleStore) { + final int first = _triangleStore.getNbTriangles(); + // Mark the current triangle as obsolete because it + setIsObsolete(true); + // Execute recursively the silhouette algorithm for the adjacent edges of neighboring + // triangles of the current triangle + final boolean result = this.adjacentEdges[0].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) + && this.adjacentEdges[1].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) && this.adjacentEdges[2].computeSilhouette(_vertices, _indexNewVertex, _triangleStore); + if (result) { + int i, j; + // For each triangle face that contains the new vertex and an edge of the silhouette + for (i = first, j = _triangleStore.getNbTriangles() - 1; i != _triangleStore.getNbTriangles(); j = i++) { + final TriangleEPA triangle = _triangleStore.get(i); + halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1)); + if (!link(new EdgeEPA(triangle, 0), new EdgeEPA(_triangleStore.get(j), 2))) { + return false; + } + } + } + return result; + } + + /// Access operator + public int get(final int _pos) { + assert (_pos >= 0 && _pos < 3); + return this.indicesVertices[_pos]; + } + /// Link an edge with another one. It means that the current edge of a triangle will + /// be associated with the edge of another triangle in order that both triangles + /// are neighbour aint both edges). + + /// Return an adjacent edge of the triangle + public EdgeEPA getAdjacentEdge(final int _index) { + assert (_index >= 0 && _index < 3); + return this.adjacentEdges[_index]; + } + + /// Return the point closest to the origin + public Vector3f getClosestPoint() { + return this.closestPoint; + } + + /// Return the square distance of the closest point to origin + public float getDistSquare() { + return this.distSquare; + } + + /// Return true if the triangle face is obsolete + public boolean getIsObsolete() { + return this.isObsolete; + } + + // Return true if the closest point on affine hull is inside the triangle + public boolean isClosestPointInternalToTriangle() { + return (this.lambda1 >= 0.0f && this.lambda2 >= 0.0 && (this.lambda1 + this.lambda2) <= this.determinant); + } + + /// Return true if the triangle is visible from a given vertex + public boolean isVisibleFromVertex(final Vector3f[] _vertices, final int _index) { + final Vector3f closestToVert = _vertices[_index].lessNew(this.closestPoint); + return (this.closestPoint.dot(closestToVert) > 0.0f); + } + + /// Constructor + public void set(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) { + this.isObsolete = false; + this.indicesVertices[0] = _indexVertex1; + this.indicesVertices[1] = _indexVertex2; + this.indicesVertices[2] = _indexVertex3; + } + + /// Private assignment operator + public TriangleEPA set(final TriangleEPA _triangle) { + this.indicesVertices[0] = _triangle.indicesVertices[0]; + this.indicesVertices[1] = _triangle.indicesVertices[1]; + this.indicesVertices[2] = _triangle.indicesVertices[2]; + this.adjacentEdges[0] = _triangle.adjacentEdges[0]; + this.adjacentEdges[1] = _triangle.adjacentEdges[1]; + this.adjacentEdges[2] = _triangle.adjacentEdges[2]; + this.isObsolete = _triangle.isObsolete; + this.determinant = _triangle.determinant; + this.closestPoint = _triangle.closestPoint; + this.lambda1 = _triangle.lambda1; + this.lambda2 = _triangle.lambda2; + this.distSquare = _triangle.distSquare; + return this; + } + + /// Set an adjacent edge of the triangle + public void setAdjacentEdge(final int _index, final EdgeEPA _edge) { + assert (_index >= 0 && _index < 3); + this.adjacentEdges[_index] = _edge; + } + + /// Set the isObsolete value + public void setIsObsolete(final boolean _isObsolete) { + this.isObsolete = _isObsolete; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java new file mode 100644 index 0000000..478650e --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java @@ -0,0 +1,70 @@ +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; + +public class TrianglesStore { + private static int MAX_TRIANGLES = 200; + + public static void shrinkTo(final List list, final int newSize) { + final int size = list.size(); + if (newSize >= size) { + return; + } + for (int i = newSize; i < size; i++) { + list.remove(list.size() - 1); + } + } + + private final List triangles = new ArrayList<>(); + + /// Clear all the storage + public void clear() { + this.triangles.clear(); + } + + /// Access operator + public TriangleEPA get(final int _id) { + return this.triangles.get(_id); + } + + /// Return the number of triangles + public int getNbTriangles() { + return this.triangles.size(); + } + + /// Return the last triangle + public TriangleEPA last() { + return this.triangles.get(this.triangles.size() - 1); + } + + /// Create a new triangle + public TriangleEPA newTriangle(final Vector3f[] _vertices, final int _v0, final int _v1, final int _v2) { + //Log.info("newTriangle: " + _v0 + ", " + _v1 + ", " + _v2); + // If we have not reached the maximum number of triangles + if (this.triangles.size() < MAX_TRIANGLES) { + + final TriangleEPA tmp = new TriangleEPA(_v0, _v1, _v2); + if (!tmp.computeClosestPoint(_vertices)) { + return null; + } + this.triangles.add(tmp); + //Log.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2)); + return tmp; + } + // We are at the limit (internal) + return null; + + } + + /// Set the number of triangles + public void resize(final int _backup) { + if (_backup > this.triangles.size()) { + Log.error("RESIZE BIGGER : " + _backup + " > " + this.triangles.size()); + } + shrinkTo(this.triangles, _backup); + } +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java new file mode 100644 index 0000000..6df7bc9 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java @@ -0,0 +1,435 @@ +package org.atriasoft.ephysics.collision.narrowphase.GJK; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.CollisionShapeInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseAlgorithm; +import org.atriasoft.ephysics.collision.narrowphase.NarrowPhaseCallback; +import org.atriasoft.ephysics.collision.narrowphase.EPA.EPAAlgorithm; +import org.atriasoft.ephysics.collision.shapes.CacheData; +import org.atriasoft.ephysics.collision.shapes.ConvexShape; +import org.atriasoft.ephysics.constraint.ContactPointInfo; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class implements a narrow-phase collision detection algorithm. This + * algorithm uses the ISA-GJK algorithm and the EPA algorithm. This + * implementation is based on the implementation discussed in the book + * "Collision Detection in Interactive 3D Environments" by Gino van den Bergen. + * This method implements the Hybrid Technique for calculating the + * penetration depth. The two objects are enlarged with a small margin. If + * the object intersects in their margins, the penetration depth is quickly + * computed using the GJK algorithm on the original objects (without margin). + * If the original objects (without margin) intersect, we run again the GJK + * algorithm on the enlarged objects (with margin) to compute simplex + * polytope that contains the origin and give it to the EPA (Expanding + * Polytope Algorithm) to compute the correct penetration depth between the + * enlarged objects. + */ +public class GJKAlgorithm extends NarrowPhaseAlgorithm { + public static final float REL_ERROR = 0.001f; + + public static final float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; + public static final int MAX_ITERATIONS_GJK_RAYCAST = 32; + private final EPAAlgorithm algoEPA; //!< EPA Algorithm + /// This method runs the GJK algorithm on the two enlarged objects (with margin) + /// to compute a simplex polytope that contains the origin. The two objects are + /// assumed to intersect in the original objects (without margin). Therefore such + /// a polytope must exist. Then, we give that polytope to the EPA algorithm to + /// compute the correct penetration depth and contact points of the enlarged objects. + + public GJKAlgorithm(final CollisionDetection collisionDetection) { + super(collisionDetection); + this.algoEPA = new EPAAlgorithm(); + this.algoEPA.init(); + } + + private void computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2, + final NarrowPhaseCallback narrowPhaseCallback, final Vector3f v) { + //Log.info("computePenetrationDepthForEnlargedObjects..."); + final Simplex simplex = new Simplex(); + float distSquare = Float.MAX_VALUE; + float prevDistSquare; + assert (shape1Info.collisionShape.isConvex()); + assert (shape2Info.collisionShape.isConvex()); + final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape); + final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape); + final Object shape1CachedCollisionData = shape1Info.cachedCollisionData; + final Object shape2CachedCollisionData = shape2Info.cachedCollisionData; + //Log.info(" transform1=" + transform1); + //Log.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().multiplyNew(transform2); + // Matrix that transform a direction from local space of body 1 into local space of body 2 + final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiply(transform1.getOrientation().getMatrix()); + //Log.info(" body2ToBody1=" + body2ToBody1); + //Log.info(" rotateToBody2=" + rotateToBody2); + //Log.info(" v=" + v); + do { + // Compute the support points for the enlarged object A and B + final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiplyNew(-1), (CacheData) shape1CachedCollisionData); + //Log.info(" suppA=" + suppA); + final Vector3f suppB = body2ToBody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiplyNew(v), (CacheData) shape2CachedCollisionData)); + //Log.info(" suppB=" + suppB); + // Compute the support point for the Minkowski difference A-B + final Vector3f w = suppA.lessNew(suppB); + final float vDotw = v.dot(w); + //Log.info(" vDotw=" + vDotw); + // If the enlarge objects do not intersect + if (vDotw > 0.0f) { + //Log.info(" ==> ret 1"); + // No intersection, we return + return; + } + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); + if (simplex.isAffinelyDependent()) { + //Log.info(" ==> ret 2"); + return; + } + if (!simplex.computeClosestPoint(v)) { + //Log.info(" ==> ret 3"); + return; + } + // Store and update the square distance + prevDistSquare = distSquare; + //Log.info(" distSquare=" + distSquare); + distSquare = v.length2(); + //Log.info(" distSquare=" + distSquare); + if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { + //Log.info(" ==> ret 4"); + return; + } + } 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"); + this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback); + } + + /// Ray casting algorithm agains a convex collision shape using the GJK Algorithm + /// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in + /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection". + public boolean raycast(final Ray ray, final ProxyShape proxyShape, final RaycastInfo raycastInfo) { + assert (proxyShape.getCollisionShape().isConvex()); + final ConvexShape shape = (ConvexShape) (proxyShape.getCollisionShape()); + final Object shapeCachedCollisionData = proxyShape.getCachedCollisionData(); + Vector3f suppA; // Current lower bound point on the ray (starting at ray's origin) + Vector3f suppB; // Support point on the collision shape + final float machineEpsilonSquare = Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON; + final float epsilon = 0.0001f; + // Convert the ray origin and direction into the local-space of the collision shape + final Vector3f rayDirection = ray.point2.lessNew(ray.point1); + // If the points of the segment are two close, return no hit + if (rayDirection.length2() < machineEpsilonSquare) { + return false; + } + Vector3f w; + // Create a simplex set + final Simplex simplex = new Simplex(); + + Vector3f n = new Vector3f(0.0f, 0.0f, 0.0f); + float lambda = 0.0f; + suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) + suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, (CacheData) shapeCachedCollisionData); + final Vector3f v = suppA.lessNew(suppB); + float vDotW, vDotR; + float distSquare = v.length2(); + int nbIterations = 0; + // GJK Algorithm loop + while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) { + // Compute the support points + suppB = shape.getLocalSupportPointWithoutMargin(v, (CacheData) shapeCachedCollisionData); + w = suppA.lessNew(suppB); + vDotW = v.dot(w); + if (vDotW > 0.0f) { + vDotR = v.dot(rayDirection); + if (vDotR >= -machineEpsilonSquare) { + return false; + } else { + // We have found a better lower bound for the hit point aint the ray + lambda = lambda - vDotW / vDotR; + suppA = rayDirection.multiplyNew(lambda).add(ray.point1); + w = suppA.lessNew(suppB); + n = v; + } + } + // Add the new support point to the simplex + if (!simplex.isPointInSimplex(w)) { + simplex.addPoint(w, suppA, suppB); + } + // Compute the closest point + if (simplex.computeClosestPoint(v)) { + distSquare = v.length2(); + } else { + distSquare = 0.0f; + } + // If the current lower bound distance is larger than the maximum raycasting distance + if (lambda > ray.maxFraction) { + return false; + } + nbIterations++; + } + // If the origin was inside the shape, we return no hit + if (lambda < Constant.FLOAT_EPSILON) { + return false; + } + // Compute the closet points of both objects (without the margins) + final Vector3f pointA = new Vector3f(); + final Vector3f pointB = new Vector3f(); + simplex.computeClosestPointsOfAandB(pointA, pointB); + // A raycast hit has been found, we fill in the raycast info + raycastInfo.hitFraction = lambda; + raycastInfo.worldPoint = pointB; + raycastInfo.body = proxyShape.getBody(); + raycastInfo.proxyShape = proxyShape; + if (n.length2() >= machineEpsilonSquare) { + // The normal vector is valid + raycastInfo.worldNormal = n; + } else { + // Degenerated normal vector, we return a zero normal vector + raycastInfo.worldNormal = new Vector3f(0.0f, 0.0f, 0.0f); + } + return true; + } + + @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); + Vector3f suppA = new Vector3f(); // Support point of object A + Vector3f suppB = new Vector3f(); // Support point of object B + Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B + Vector3f pA = new Vector3f(); // Closest point of object A + Vector3f pB = new Vector3f(); // Closest point of object B + float vDotw; + float prevDistSquare; + assert (shape1Info.collisionShape.isConvex()); + assert (shape2Info.collisionShape.isConvex()); + final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape); + final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape); + final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData; + final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData; + // Get the local-space to world-space transforms + final Transform3D transform1 = shape1Info.shapeToWorldTransform.clone(); + final Transform3D transform2 = shape2Info.shapeToWorldTransform.clone(); + // 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().multiplyNew(transform2); + // Matrix that transform a direction from local + // space of body 1 into local space of body 2 + final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiplyNew(transform1.getOrientation().getMatrix()); + // Initialize the margin (sum of margins of both objects) + final float margin = shape1.getMargin() + shape2.getMargin(); + final float marginSquare = margin * margin; + assert (margin > 0.0f); + // Create a simplex set + final Simplex simplex = new Simplex(); + // Get the previous point V (last cached separating axis) + final Vector3f cacheSeparatingAxis = this.currentOverlappingPair.getCachedSeparatingAxis().clone(); + // 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); + + do { + //Log.error("------------------"); + // Compute the support points for original objects (without margins) A and B + suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.multiplyNew(-1.0f), shape1CachedCollisionData); + suppB = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiplyNew(cacheSeparatingAxis), shape2CachedCollisionData)); + // Compute the support point for the Minkowski difference A-B + w = suppA.lessNew(suppB); + vDotw = cacheSeparatingAxis.dot(w); + //Log.error(" suppA=" + suppA); + //Log.error(" suppB=" + suppB); + //Log.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 + this.currentOverlappingPair.setCachedSeparatingAxis(cacheSeparatingAxis); + // No intersection, we return + return; + } + // If the objects intersect only in the margins + if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { + //Log.error("11111111 "); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + // Project those two points on the margins to have the closest points of both + // object with the margins + final float dist = FMath.sqrt(distSquare); + assert (dist > 0.0f); + pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist)); + pB = body2Tobody1.inverseNew().multiplyNew(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB)); + // Compute the contact info + final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1)); + final float penetrationDepth = margin - dist; + // Reject the contact if the penetration depth is negative (due too numerical errors) + if (penetrationDepth <= 0.0f) { + return; + } + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, + pA, pB); + callback.notifyContact(shape1Info.overlappingPair, contactInfo); + // There is an intersection, therefore we return + return; + } + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); + // If the simplex is affinely dependent + if (simplex.isAffinelyDependent()) { + //Log.error("222222 "); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + // Project those two points on the margins to have the closest points of both + // object with the margins + final float dist = FMath.sqrt(distSquare); + assert (dist > 0.0f); + pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist)); + pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB)); + // Compute the contact info + final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1)); + final float penetrationDepth = margin - dist; + + // Reject the contact if the penetration depth is negative (due too numerical errors) + if (penetrationDepth <= 0.0f) { + //Log.info("penetration depth " + penetrationDepth); + return; + } + + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, + pA, pB); + callback.notifyContact(shape1Info.overlappingPair, contactInfo); + // There is an intersection, therefore we return + return; + } + // Compute the point of the simplex closest to the origin + // If the computation of the closest point fail + if (!simplex.computeClosestPoint(cacheSeparatingAxis)) { + //Log.error("3333333333 "); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + // Project those two points on the margins to have the closest points of both + // object with the margins + final float dist = FMath.sqrt(distSquare); + assert (dist > 0.0f); + pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist)); + pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB)); + // Compute the contact info + final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1)); + final float penetrationDepth = margin - dist; + + // Reject the contact if the penetration depth is negative (due too numerical errors) + if (penetrationDepth <= 0.0f) { + return; + } + + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, + pA, pB); + callback.notifyContact(shape1Info.overlappingPair, contactInfo); + // There is an intersection, therefore we return + return; + } + // Store and update the squared distance of the closest point + prevDistSquare = distSquare; + distSquare = cacheSeparatingAxis.length2(); + // If the distance to the closest point doesn't improve a lot + if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { + //Log.error("444444444 "); + simplex.backupClosestPointInSimplex(cacheSeparatingAxis); + + // Get the new squared distance + distSquare = cacheSeparatingAxis.length2(); + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + // Project those two points on the margins to have the closest points of both + // object with the margins + final float dist = FMath.sqrt(distSquare); + assert (dist > 0.0f); + pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist)); + pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB)); + // Compute the contact info + final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1)); + final float penetrationDepth = margin - dist; + + // Reject the contact if the penetration depth is negative (due too numerical errors) + if (penetrationDepth <= 0.0f) { + return; + } + + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, + pA, pB); + callback.notifyContact(shape1Info.overlappingPair, contactInfo); + // There is an intersection, therefore we return + return; + } + } while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); + // The objects (without margins) intersect. Therefore, we run the GJK algorithm + // again but on the enlarged objects to compute a simplex polytope that contains + // the origin. Then, we give that simplex polytope to the EPA algorithm to compute + // the correct penetration depth and contact points between the enlarged objects. + computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, transform2, callback, cacheSeparatingAxis); + } + + /// Use the GJK Algorithm to find if a point is inside a convex collision shape + public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) { + Vector3f suppA = new Vector3f(); // Support point of object A + Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B + float prevDistSquare; + assert (proxyShape.getCollisionShape().isConvex()); + final ConvexShape shape = (ConvexShape) (proxyShape.getCollisionShape()); + final CacheData shapeCachedCollisionData = (CacheData) proxyShape.getCachedCollisionData(); + // Support point of object B (object B is a single point) + final Vector3f suppB = new Vector3f(localPoint); + // Create a simplex set + final Simplex simplex = new Simplex(); + + // Initial supporting direction + final Vector3f v = new Vector3f(1, 1, 1); + // Initialize the upper bound for the square distance + float distSquare = Float.MAX_VALUE; + do { + // Compute the support points for original objects (without margins) A and B + suppA = shape.getLocalSupportPointWithoutMargin(v.multiplyNew(-1), shapeCachedCollisionData); + // Compute the support point for the Minkowski difference A-B + w = suppA.lessNew(suppB); + // Add the new support point to the simplex + simplex.addPoint(w, suppA, suppB); + // If the simplex is affinely dependent + if (simplex.isAffinelyDependent()) { + return false; + } + // Compute the point of the simplex closest to the origin + // If the computation of the closest point fail + if (!simplex.computeClosestPoint(v)) { + return false; + } + // Store and update the squared distance of the closest point + prevDistSquare = distSquare; + distSquare = v.length2(); + // If the distance to the closest point doesn't improve a lot + if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) { + return false; + } + } while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint()); + // The point is inside the collision shape + return true; + } +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java new file mode 100644 index 0000000..b948221 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java @@ -0,0 +1,444 @@ +package org.atriasoft.ephysics.collision.narrowphase.GJK; + +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a simplex which is a set of 3D points. This + * class is used in the GJK algorithm. This implementation is based on + * the implementation discussed in the book "Collision Detection in 3D + * Environments". This class implements the Johnson's algorithm for + * computing the point of a simplex that is closest to the origin and also + * the smallest simplex needed to represent that closest point. + */ +public class Simplex { + + private class Array2f { + private final float[] data; + private final int sizeX; + private final int sizeY; + + public Array2f(final int sizeX, final int sizeY) { + this.sizeX = sizeX; + this.sizeY = sizeY; + this.data = new float[sizeX * sizeY]; + } + + public float get(final int xxx, final int yyy) { + return this.data[yyy * this.sizeX + xxx]; + } + + public int getSizeX() { + return this.sizeX; + } + + public int getSizeY() { + return this.sizeY; + } + + public void set(final int xxx, final int yyy, final float data) { + this.data[yyy * this.sizeX + xxx] = data; + } + } + + private class Array2Vector3f { + private final Vector3f[] data; + private final int sizeX; + private final int sizeY; + + public Array2Vector3f(final int sizeX, final int sizeY) { + this.sizeX = sizeX; + this.sizeY = sizeY; + this.data = new Vector3f[sizeX * sizeY]; + } + + public Vector3f get(final int xxx, final int yyy) { + return this.data[yyy * this.sizeX + xxx]; + } + + public int getSizeX() { + return this.sizeX; + } + + public int getSizeY() { + return this.sizeY; + } + + public void set(final int xxx, final int yyy, final Vector3f data) { + this.data[yyy * this.sizeX + xxx] = data; + } + } + + /// Current points + private final Vector3f[] points = new Vector3f[4]; + + /// pointsLengthSquare[i] = (points[i].length)^2 + private final float[] pointsLengthSquare = new float[4]; + + /// Maximum length of pointsLengthSquare[i] + private float maxLengthSquare; + + /// Support points of object A in local coordinates + private final Vector3f[] suppPointsA = new Vector3f[4]; + + /// Support points of object B in local coordinates + private final Vector3f[] suppPointsB = new Vector3f[4]; + + /// diff[i][j] contains points[i] - points[j] + private final Array2Vector3f diffLength = new Array2Vector3f(4, 4); + + /// Cached determinant values + private final Array2f det = new Array2f(16, 4); + + /// norm[i][j] = (diff[i][j].length())^2 + private final Array2f normalSquare = new Array2f(4, 4); + + /// 4 bits that identify the current points of the simplex + /// For instance, 0101 means that points[1] and points[3] are in the simplex + private int bitsCurrentSimplex = 0; + + /// Number between 1 and 4 that identify the last found support point + private int lastFound = 0; + + /// Position of the last found support point (lastFoundBit = 0x1 << lastFound) + private int lastFoundBit = 0; + + /// allint = bitsCurrentSimplex | lastFoundBit; + private int allBits = 0; + + // -------------------- Methods -------------------- // + + /// Constructor + public Simplex() { + + } + + /// Add a new support point of (A-B) into the simplex. + /// suppPointA : support point of object A in a direction -v + /// suppPointB : support point of object B in a direction v + /// 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 + ")"); + this.lastFound = 0; + this.lastFoundBit = 0x1; + + // Look for the bit corresponding to one of the four point that is not in + // the current simplex + while (overlap(this.bitsCurrentSimplex, this.lastFoundBit)) { + this.lastFound++; + this.lastFoundBit <<= 1; + } + //Log.warning(" this.lastFound " + this.lastFound); + //Log.warning(" this.lastFoundBit " + this.lastFoundBit); + + assert (this.lastFound < 4); + + // Add the point into the 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); + + // Update the cached values + updateCache(); + + // Compute the cached determinant values + computeDeterminants(); + + // Add the support points of objects A and B + this.suppPointsA[this.lastFound] = suppPointA; + this.suppPointsB[this.lastFound] = suppPointB; + } + + /// Backup the closest point + public void backupClosestPointInSimplex(final Vector3f point) { + float minDistSquare = Float.MAX_VALUE; + for (int bit = this.allBits; bit != 0x0; bit--) { + if (isSubset(bit, this.allBits) && isProperSubset(bit)) { + final Vector3f u = computeClosestPointForSubset(bit); + final float distSquare = u.dot(u); + if (distSquare < minDistSquare) { + minDistSquare = distSquare; + this.bitsCurrentSimplex = bit; + point.set(u); + } + } + } + } + + /// Compute the closest point to the origin of the current simplex. + /// This method executes the Jonhnson's algorithm for computing the point + /// "v" of simplex that is closest to the origin. The method returns true + /// if a closest point has been found. + public boolean computeClosestPoint(final Vector3f vvv) { + // For each possible simplex set + for (int subset = this.bitsCurrentSimplex; subset != 0x0; subset--) { + // If the simplex is a subset of the current simplex and is valid for the Johnson's + // algorithm test + if (isSubset(subset, this.bitsCurrentSimplex) && isValidSubset(subset | this.lastFoundBit)) { + this.bitsCurrentSimplex = subset | this.lastFoundBit; // Add the last added point to the current simplex + vvv.set(computeClosestPointForSubset(this.bitsCurrentSimplex)); // Compute the closest point in the simplex + return true; + } + } + + // If the simplex that contains only the last added point is valid for the Johnson's algorithm test + if (isValidSubset(this.lastFoundBit)) { + this.bitsCurrentSimplex = this.lastFoundBit; // Set the current simplex to the set that contains only the last added point + this.maxLengthSquare = this.pointsLengthSquare[this.lastFound]; // Update the maximum square length + vvv.set(this.points[this.lastFound]); // The closest point of the simplex "v" is the last added point + return true; + } + + // The algorithm failed to found a point + return false; + } + + /// Return the closest point "v" in the convex hull of a subset of points + private Vector3f computeClosestPointForSubset(final int subset) { + final Vector3f vvv = new Vector3f(0.0f, 0.0f, 0.0f); // Closet point v = sum(lambda_i * points[i]) + this.maxLengthSquare = 0.0f; + float deltaX = 0.0f; // deltaX = sum of all det[subset][i] + // For each four point in the possible simplex set + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // deltaX = sum of all det[subset][i] + deltaX += this.det.get(subset, iii); + if (this.maxLengthSquare < this.pointsLengthSquare[iii]) { + this.maxLengthSquare = this.pointsLengthSquare[iii]; + } + // Closest point v = sum(lambda_i * points[i]) + vvv.add(this.points[iii].multiplyNew(this.det.get(subset, iii))); + } + } + assert (deltaX > 0.0f); + // Return the closet point "v" in the convex hull for the given subset + return vvv.multiply(1.0f / deltaX); + } + + /// Compute the closest points "pA" and "pB" of object A and B. + /// The points are computed as follows : + /// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A + /// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B + /// with lambda_i = deltaX_i / deltaX + public void computeClosestPointsOfAandB(final Vector3f pA, final Vector3f pB) { + float deltaX = 0.0f; + pA.set(0.0f, 0.0f, 0.0f); + pB.set(0.0f, 0.0f, 0.0f); + // For each four points in the possible simplex set + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + // If the current point is part of the simplex + if (overlap(this.bitsCurrentSimplex, bit)) { + deltaX += this.det.get(this.bitsCurrentSimplex, iii); + pA.add(this.suppPointsA[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii))); + pB.add(this.suppPointsB[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii))); + } + } + + assert (deltaX > 0.0f); + final float factor = 1.0f / deltaX; + pA.multiply(factor); + pB.multiply(factor); + } + + /// 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)); + // If the current simplex is not empty + if (!isEmpty()) { + // For each possible four points in the simplex set + for (int iii = 0, bitI = 0x1; iii < 4; iii++, bitI <<= 1) { + // If the current point is in the simplex + if (overlap(this.bitsCurrentSimplex, bitI)) { + final int bit2 = bitI | this.lastFoundBit; + float tmpp = this.diffLength.get(this.lastFound, iii).dot(this.points[this.lastFound]); + this.det.set(bit2, iii, tmpp); + tmpp = this.diffLength.get(iii, this.lastFound).dot(this.points[iii]); + this.det.set(bit2, this.lastFound, tmpp); + for (int jjj = 0, bitJ = 0x1; jjj < iii; jjj++, bitJ <<= 1) { + if (overlap(this.bitsCurrentSimplex, bitJ)) { + final int bit3 = bitJ | bit2; + int kkk = this.normalSquare.get(iii, jjj) < this.normalSquare.get(this.lastFound, jjj) ? iii : this.lastFound; + float tmp2 = this.det.get(bit2, iii) * this.diffLength.get(kkk, jjj).dot(this.points[iii]) + + this.det.get(bit2, this.lastFound) * this.diffLength.get(kkk, jjj).dot(this.points[this.lastFound]); + this.det.set(bit3, jjj, tmp2); + kkk = this.normalSquare.get(jjj, iii) < this.normalSquare.get(this.lastFound, iii) ? jjj : this.lastFound; + tmp2 = this.det.get(bitJ | this.lastFoundBit, jjj) * this.diffLength.get(kkk, iii).dot(this.points[jjj]) + + this.det.get(bitJ | this.lastFoundBit, this.lastFound) * this.diffLength.get(kkk, iii).dot(this.points[this.lastFound]); + this.det.set(bit3, iii, tmp2); + kkk = this.normalSquare.get(iii, this.lastFound) < this.normalSquare.get(jjj, this.lastFound) ? iii : jjj; + tmp2 = this.det.get(bitJ | bitI, jjj) * this.diffLength.get(kkk, this.lastFound).dot(this.points[jjj]) + + this.det.get(bitJ | bitI, iii) * this.diffLength.get(kkk, this.lastFound).dot(this.points[iii]); + this.det.set(bit3, this.lastFound, tmp2); + } + } + } + } + + if (this.allBits == 0xf) { + int kkk; + + kkk = this.normalSquare.get(1, 0) < this.normalSquare.get(2, 0) ? (this.normalSquare.get(1, 0) < this.normalSquare.get(3, 0) ? 1 : 3) + : (this.normalSquare.get(2, 0) < this.normalSquare.get(3, 0) ? 2 : 3); + float tmp3 = this.det.get(0xe, 1) * this.diffLength.get(kkk, 0).dot(this.points[1]) + this.det.get(0xe, 2) * this.diffLength.get(kkk, 0).dot(this.points[2]) + + this.det.get(0xe, 3) * this.diffLength.get(kkk, 0).dot(this.points[3]); + this.det.set(0xf, 0, tmp3); + + kkk = this.normalSquare.get(0, 1) < this.normalSquare.get(2, 1) ? (this.normalSquare.get(0, 1) < this.normalSquare.get(3, 1) ? 0 : 3) + : (this.normalSquare.get(2, 1) < this.normalSquare.get(3, 1) ? 2 : 3); + tmp3 = this.det.get(0xd, 0) * this.diffLength.get(kkk, 1).dot(this.points[0]) + this.det.get(0xd, 2) * this.diffLength.get(kkk, 1).dot(this.points[2]) + + this.det.get(0xd, 3) * this.diffLength.get(kkk, 1).dot(this.points[3]); + this.det.set(0xf, 1, tmp3); + + kkk = this.normalSquare.get(0, 2) < this.normalSquare.get(1, 2) ? (this.normalSquare.get(0, 2) < this.normalSquare.get(3, 2) ? 0 : 3) + : (this.normalSquare.get(1, 2) < this.normalSquare.get(3, 2) ? 1 : 3); + tmp3 = this.det.get(0xb, 0) * this.diffLength.get(kkk, 2).dot(this.points[0]) + this.det.get(0xb, 1) * this.diffLength.get(kkk, 2).dot(this.points[1]) + + this.det.get(0xb, 3) * this.diffLength.get(kkk, 2).dot(this.points[3]); + this.det.set(0xf, 2, tmp3); + + kkk = this.normalSquare.get(0, 3) < this.normalSquare.get(1, 3) ? (this.normalSquare.get(0, 3) < this.normalSquare.get(2, 3) ? 0 : 2) + : (this.normalSquare.get(1, 3) < this.normalSquare.get(2, 3) ? 1 : 2); + tmp3 = this.det.get(0x7, 0) * this.diffLength.get(kkk, 3).dot(this.points[0]) + this.det.get(0x7, 1) * this.diffLength.get(kkk, 3).dot(this.points[1]) + + this.det.get(0x7, 2) * this.diffLength.get(kkk, 3).dot(this.points[2]); + this.det.set(0xf, 3, tmp3); + } + } + } + + /// Return the maximum squared length of a point + public float getMaxLengthSquareOfAPoint() { + return this.maxLengthSquare; + } + + /// Return the points of the simplex + public int getSimplex(final Vector3f[] suppPointsA, final Vector3f[] suppPointsB, final Vector3f[] points) { + int nbVertices = 0; + // For each four point in the possible simplex + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + // If the current point is in the simplex + if (overlap(this.bitsCurrentSimplex, bit)) { + // Store the points + suppPointsA[nbVertices] = this.suppPointsA[nbVertices].clone(); + suppPointsB[nbVertices] = this.suppPointsB[nbVertices].clone(); + points[nbVertices] = this.points[nbVertices].clone(); + nbVertices++; + } + } + // Return the number of points in the simplex + return nbVertices; + } + + /// Return true if the set is affinely dependent + /// A set if affinely dependent if a point of the set + /// is an affine combination of other points in the set + public boolean isAffinelyDependent() { + float sum = 0.0f; + // For each four point of the possible simplex set + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + if (overlap(this.allBits, bit)) { + sum += this.det.get(this.allBits, iii); + } + } + return (sum <= 0.0f); + } + + /// Return true if the simplex is empty + public boolean isEmpty() { + return (this.bitsCurrentSimplex == 0x0); + } + + /// Return true if the simplex contains 4 points + public boolean isFull() { + return (this.bitsCurrentSimplex == 0xf); + } + + /// Return true if the point is in the simplex + public boolean isPointInSimplex(final Vector3f point) { + // For each four possible points in the simplex + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + // Check if the current point is in the simplex + if (overlap(this.allBits, bit) && point == this.points[iii]) { + return true; + } + } + return false; + } + + /// Return true if the subset is a proper subset. + /// A proper subset X is a subset where for all point "y_i" in X, we have + /// detX_i value bigger than zero + private boolean isProperSubset(final int subset) { + // For each four point of the possible simplex set + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + if (overlap(subset, bit) && this.det.get(subset, iii) <= 0.0f) { + return false; + } + } + return true; + } + + /// Return true if the bits of "b" is a subset of the bits of "a" + private boolean isSubset(final int a, final int b) { + return ((a & b) == a); + } + + /// Return true if the subset is a valid one for the closest point computation. + /// A subset X is valid if : + /// 1. delta(X)_i > 0 for each "i" in I_x and + /// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ + private boolean isValidSubset(final int subset) { + // For each four point in the possible simplex set + for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) { + if (overlap(this.allBits, bit)) { + // If the current point is in the subset + if (overlap(subset, bit)) { + // If one delta(X)_i is smaller or equal to zero + if (this.det.get(subset, iii) <= 0.0f) { + // The subset is not valid + return false; + } + } + // If the point is not in the subset and the value delta(X U {y_j})_j + // is bigger than zero + else if (this.det.get(subset | bit, iii) > 0.0f) { + // The subset is not valid + return false; + } + } + } + return true; + } + + /// Return true if some bits of "a" overlap with bits of "b" + private boolean overlap(final int a, final int b) { + return ((a & b) != 0x0); + } + + /// Update the cached values used during the GJK algorithm + private void updateCache() { + //Log.warning("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); + + // If the current points is in the simplex + if (overlap(this.bitsCurrentSimplex, bit)) { + //Log.warning("simplex: ==> overlap"); + // Compute the distance between two points in the possible simplex set + final Vector3f tmp = this.points[iii].lessNew(this.points[this.lastFound]); + this.diffLength.set(iii, this.lastFound, tmp); + final Vector3f tmp2 = tmp.multiplyNew(-1); + this.diffLength.set(this.lastFound, iii, tmp2); + // Compute the squared length of the vector + // distances from points in the possible simplex set + final float normal = tmp.dot(tmp); + this.normalSquare.set(iii, this.lastFound, normal); + this.normalSquare.set(this.lastFound, iii, normal); + } + } + } + +} diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java new file mode 100644 index 0000000..5796a56 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java @@ -0,0 +1,32 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.CollisionShapeInfo; +import org.atriasoft.ephysics.engine.OverlappingPair; + +/** + * @breif It is the base class for a narrow-phase collision + * detection algorithm. The goal of the narrow phase algorithm is to + * compute information about the contact between two proxy shapes. + */ +public abstract class NarrowPhaseAlgorithm { + + protected CollisionDetection collisionDetection; //!< Pointer to the collision detection object + protected OverlappingPair currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision + + /// Constructor + public NarrowPhaseAlgorithm(final CollisionDetection collisionDetection) { + this.currentOverlappingPair = null; + this.collisionDetection = null; + this.collisionDetection = collisionDetection; + } + + /// Set the current overlapping pair of bodies + public void setCurrentOverlappingPair(final OverlappingPair overlappingPair) { + this.currentOverlappingPair = overlappingPair; + } + + /// Compute a contact info if the two bounding volume collide + public abstract void testCollision(final CollisionShapeInfo _shape1Info, CollisionShapeInfo _shape2Info, NarrowPhaseCallback _narrowPhaseCallback); + +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java b/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java new file mode 100644 index 0000000..86c4ed4 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java @@ -0,0 +1,14 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import org.atriasoft.ephysics.constraint.ContactPointInfo; +import org.atriasoft.ephysics.engine.OverlappingPair; + +/** + * It is the base class for a narrow-phase collision + * callback class. + */ +public interface NarrowPhaseCallback { + /// Called by a narrow-phase collision algorithm when a new contact has been found + void notifyContact(OverlappingPair _overlappingPair, ContactPointInfo _contactInfo); + +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java b/src/org/atriasoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java new file mode 100644 index 0000000..a703378 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java @@ -0,0 +1,50 @@ +package org.atriasoft.ephysics.collision.narrowphase; + +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.CollisionShapeInfo; +import org.atriasoft.ephysics.collision.shapes.SphereShape; +import org.atriasoft.ephysics.constraint.ContactPointInfo; + +/** + * It is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ +public class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm { + + public SphereVsSphereAlgorithm(final CollisionDetection collisionDetection) { + super(collisionDetection); + } + + @Override + public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final NarrowPhaseCallback _narrowPhaseCallback) { + // Get the sphere collision shapes + final SphereShape sphereShape1 = (SphereShape) _shape1Info.collisionShape; + final SphereShape sphereShape2 = (SphereShape) _shape2Info.collisionShape; + // Get the local-space to world-space transforms + final Transform3D transform1 = _shape1Info.shapeToWorldTransform; + final Transform3D transform2 = _shape2Info.shapeToWorldTransform; + // Compute the distance between the centers + final Vector3f vectorBetweenCenters = transform2.getPosition().lessNew(transform1.getPosition()); + final float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); + // Compute the sum of the radius + final float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius(); + // If the sphere collision shapes intersect + if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { + final Vector3f centerSphere2InBody1LocalSpace = transform1.inverseNew().multiply(transform2.getPosition()); + final Vector3f centerSphere1InBody2LocalSpace = transform2.inverseNew().multiply(transform1.getPosition()); + final Vector3f intersectionOnBody1 = centerSphere2InBody1LocalSpace.safeNormalizeNew().multiply(sphereShape1.getRadius()); + final Vector3f intersectionOnBody2 = centerSphere1InBody2LocalSpace.safeNormalizeNew().multiply(sphereShape2.getRadius()); + final float penetrationDepth = sumRadius - FMath.sqrt(squaredDistanceBetweenCenters); + + // Create the contact info object + final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, + vectorBetweenCenters.safeNormalizeNew(), penetrationDepth, intersectionOnBody1, intersectionOnBody2); + // Notify about the new contact + _narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo); + } + } +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/AABB.java b/src/org/atriasoft/ephysics/collision/shapes/AABB.java new file mode 100644 index 0000000..8bb0397 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/AABB.java @@ -0,0 +1,322 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a bounding volume of type "Axis Aligned Bounding Box". It's a box where all the edges are + * always aligned with the world coordinate system. The AABB is defined by the this.inimum and this.aximum world coordinates of + * the three axis. + * + * @author Jason Sorensen + */ +public class AABB { + + /** + * @brief Create and return an AABB for a triangle + * @param[in] _trianglePoints List of 3 point od a triangle + * @return An AABB box + */ + public static AABB createAABBForTriangle(final Vector3f[] _trianglePoints) { + final Vector3f minCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z); + final Vector3f maxCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z); + if (_trianglePoints[1].x < minCoords.x) { + minCoords.setX(_trianglePoints[1].x); + } + if (_trianglePoints[1].y < minCoords.y) { + minCoords.setY(_trianglePoints[1].y); + } + if (_trianglePoints[1].z < minCoords.z) { + minCoords.setZ(_trianglePoints[1].z); + } + if (_trianglePoints[2].x < minCoords.x) { + minCoords.setX(_trianglePoints[2].x); + } + if (_trianglePoints[2].y < minCoords.y) { + minCoords.setY(_trianglePoints[2].y); + } + if (_trianglePoints[2].z < minCoords.z) { + minCoords.setZ(_trianglePoints[2].z); + } + if (_trianglePoints[1].x > maxCoords.x) { + maxCoords.setX(_trianglePoints[1].x); + } + if (_trianglePoints[1].y > maxCoords.y) { + maxCoords.setY(_trianglePoints[1].y); + } + if (_trianglePoints[1].z > maxCoords.z) { + maxCoords.setZ(_trianglePoints[1].z); + } + if (_trianglePoints[2].x > maxCoords.x) { + maxCoords.setX(_trianglePoints[2].x); + } + if (_trianglePoints[2].y > maxCoords.y) { + maxCoords.setY(_trianglePoints[2].y); + } + if (_trianglePoints[2].z > maxCoords.z) { + maxCoords.setZ(_trianglePoints[2].z); + } + return new AABB(minCoords, maxCoords); + } + + // Maximum world coordinates of the AABB on the x,y and z axis + private final Vector3f maxCoordinates; + + // Minimum world coordinates of the AABB on the x,y and z axis + private final Vector3f minCoordinates; + + /** + * @brief default contructor + */ + public AABB() { + this.maxCoordinates = new Vector3f(); + this.minCoordinates = new Vector3f(); + } + + /** + * @brief contructor Whit sizes + * @param[in] _minCoordinates Minimum coordinates + * @param[in] _maxCoordinates Maximum coordinates + */ + public AABB(final Vector3f minCoordinates, final Vector3f maxCoordinates) { + this.maxCoordinates = maxCoordinates; + this.minCoordinates = minCoordinates; + } + + /** + * @brief Return true if the current AABB contains the AABB given in parameter + * @param[in] _aabb AABB box that is contains in the current. + * @return true The parameter in contained inside + */ + public boolean contains(final AABB _aabb) { + boolean isInside = true; + isInside = isInside && this.minCoordinates.x <= _aabb.minCoordinates.x; + isInside = isInside && this.minCoordinates.y <= _aabb.minCoordinates.y; + isInside = isInside && this.minCoordinates.z <= _aabb.minCoordinates.z; + isInside = isInside && this.maxCoordinates.x >= _aabb.maxCoordinates.x; + isInside = isInside && this.maxCoordinates.y >= _aabb.maxCoordinates.y; + isInside = isInside && this.maxCoordinates.z >= _aabb.maxCoordinates.z; + return isInside; + } + + /** + * @brief Return true if a point is inside the AABB + * @param[in] _point Point to check. + * @return true The point in contained inside + */ + public boolean contains(final Vector3f _point) { + return _point.x >= this.minCoordinates.x - Constant.FLOAT_EPSILON && _point.x <= this.maxCoordinates.x + Constant.FLOAT_EPSILON && _point.y >= this.minCoordinates.y - Constant.FLOAT_EPSILON + && _point.y <= this.maxCoordinates.y + Constant.FLOAT_EPSILON && _point.z >= this.minCoordinates.z - Constant.FLOAT_EPSILON + && _point.z <= this.maxCoordinates.z + Constant.FLOAT_EPSILON; + } + + /** + * @brief Get the center point of the AABB box + * @return The 3D position of the center + */ + public Vector3f getCenter() { + return new Vector3f(this.minCoordinates).add(this.maxCoordinates).multiply(0.5f); + } + + /** + * @brief Get the size of the AABB in the three dimension x, y and z + * @return the AABB 3D size + */ + public Vector3f getExtent() { + return this.maxCoordinates.lessNew(this.minCoordinates); + } + + /** + * @brief Return the maximum coordinates of the AABB + * @return The 3d maximum coordonates + */ + public Vector3f getMax() { + return this.maxCoordinates; + } + + /** + * @brief Get the minimum coordinates of the AABB + * @return The 3d minimum coordonates + */ + public Vector3f getMin() { + return this.minCoordinates; + } + + /** + * @brief Get the volume of the AABB + * @return The 3D volume. + */ + public float getVolume() { + final Vector3f diff = this.maxCoordinates.lessNew(this.minCoordinates); + return (diff.x * diff.y * diff.z); + } + + /** + * @brief Inflate each side of the AABB by a given size + * @param[in] _dx Inflate X size + * @param[in] _dy Inflate Y size + * @param[in] _dz Inflate Z size + */ + public void inflate(final float _dx, final float _dy, final float _dz) { + this.maxCoordinates.add(new Vector3f(_dx, _dy, _dz)); + this.minCoordinates.less(new Vector3f(_dx, _dy, _dz)); + } + + /** + * @brief Replace the current AABB with a new AABB that is the union of two AABBs in parameters + * @param[in] _aabb1 first AABB box to merge with _aabb2. + * @param[in] _aabb2 second AABB box to merge with _aabb1. + */ + public void mergeTwoAABBs(final AABB _aabb1, final AABB _aabb2) { + this.minCoordinates.setX(FMath.min(_aabb1.minCoordinates.x, _aabb2.minCoordinates.x)); + this.minCoordinates.setY(FMath.min(_aabb1.minCoordinates.y, _aabb2.minCoordinates.y)); + this.minCoordinates.setZ(FMath.min(_aabb1.minCoordinates.z, _aabb2.minCoordinates.z)); + this.maxCoordinates.setX(FMath.max(_aabb1.maxCoordinates.x, _aabb2.maxCoordinates.x)); + this.maxCoordinates.setY(FMath.max(_aabb1.maxCoordinates.y, _aabb2.maxCoordinates.y)); + this.maxCoordinates.setZ(FMath.max(_aabb1.maxCoordinates.z, _aabb2.maxCoordinates.z)); + } + + /** + * @brief Merge the AABB in parameter with the current one + * @param[in] _aabb Other AABB box to merge. + */ + public void mergeWithAABB(final AABB _aabb) { + this.minCoordinates.setX(FMath.min(this.minCoordinates.x, _aabb.minCoordinates.x)); + this.minCoordinates.setY(FMath.min(this.minCoordinates.y, _aabb.minCoordinates.y)); + this.minCoordinates.setZ(FMath.min(this.minCoordinates.z, _aabb.minCoordinates.z)); + this.maxCoordinates.setX(FMath.max(this.maxCoordinates.x, _aabb.maxCoordinates.x)); + this.maxCoordinates.setY(FMath.max(this.maxCoordinates.y, _aabb.maxCoordinates.y)); + this.maxCoordinates.setZ(FMath.max(this.maxCoordinates.z, _aabb.maxCoordinates.z)); + } + + /** + * @brief Set the maximum coordinates of the AABB + * @param[in] _max The 3d maximum coordonates + */ + public void setMax(final Vector3f max) { + this.maxCoordinates.set(max); + } + + /** + * @brief Set the minimum coordinates of the AABB + * @param[in] _min The 3d minimum coordonates + */ + public void setMin(final Vector3f min) { + this.minCoordinates.set(min); + } + + /** + * @brief Return true if the current AABB is overlapping with the AABB in argument + * Two AABBs overlap if they overlap in the three x, y and z axis at the same time + * @param[in] _aabb Other AABB box to check. + * @return true Collision detected + * @return false Not collide + */ + public boolean testCollision(final AABB aabb) { + if (this == aabb) { + ///Log.info("test : AABB ==> same object "); + return true; + } + //Log.info("test : " + this + " && " + aabb); + if (this.maxCoordinates.getX() < aabb.minCoordinates.getX() || aabb.maxCoordinates.getX() < this.minCoordinates.getX()) { + return false; + } + if (this.maxCoordinates.getZ() < aabb.minCoordinates.getZ() || aabb.maxCoordinates.getZ() < this.minCoordinates.getZ()) { + return false; + } + if (this.maxCoordinates.getY() < aabb.minCoordinates.getY() || aabb.maxCoordinates.getY() < this.minCoordinates.getY()) { + return false; + } + //Log.info("detect collision "); + return true; + } + + /** + * @brief check if the AABB of a triangle intersects the AABB + * @param[in] _trianglePoints List of 3 point od a triangle + * @return true The triangle is contained in the Box + */ + public boolean testCollisionTriangleAABB(final Vector3f[] _trianglePoints) { + if (FMath.min(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) > this.maxCoordinates.x) { + return false; + } + if (FMath.min(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) > this.maxCoordinates.y) { + return false; + } + if (FMath.min(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) > this.maxCoordinates.z) { + return false; + } + if (FMath.max(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) < this.minCoordinates.x) { + return false; + } + if (FMath.max(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) < this.minCoordinates.y) { + return false; + } + if (FMath.max(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) < this.minCoordinates.z) { + return false; + } + return true; + } + + /* + * @brief check if the ray intersects the AABB + * This method use the line vs AABB raycasting technique described in + * Real-time Collision Detection by Christer Ericson. + * @param[in] _ray Ray to test + * @return true The raytest intersect the AABB box + */ + public boolean testRayIntersect(final Ray ray) { + final Vector3f point2 = ray.point2.lessNew(ray.point1).multiply(ray.maxFraction).add(ray.point1); + final Vector3f e = this.maxCoordinates.lessNew(this.minCoordinates); + final Vector3f d = point2.lessNew(ray.point1); + final Vector3f m = ray.point1.addNew(point2).less(this.minCoordinates).less(this.maxCoordinates); + // Test if the AABB face normals are separating axis + float adx = FMath.abs(d.x); + if (FMath.abs(m.x) > e.x + adx) { + return false; + } + float ady = FMath.abs(d.y); + if (FMath.abs(m.y) > e.y + ady) { + return false; + } + float adz = FMath.abs(d.z); + if (FMath.abs(m.z) > e.z + adz) { + return false; + } + // Add in an epsilon term to counteract arithmetic errors when segment is + // (near) parallel to a coordinate axis (see text for detail) + final float epsilon = 0.00001f; + adx += epsilon; + ady += epsilon; + adz += epsilon; + // Test if the cross products between face normals and ray direction are + // separating axis + if (FMath.abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) { + return false; + } + if (FMath.abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) { + return false; + } + if (FMath.abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) { + return false; + } + // No separating axis has been found + return true; + } + + @Override + public String toString() { + return "AABB [min=" + this.minCoordinates + ", max=" + this.maxCoordinates + "]"; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java b/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java new file mode 100644 index 0000000..0a5572a --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/BoxShape.java @@ -0,0 +1,170 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a 3D box shape. Those axis are unit length. The three extents are half-widths of the box along + * the three axis x, y, z local axis. The "transform" of the corresponding rigid body will give an orientation and a + * position to the box. This collision shape uses an extra margin distance around it for collision detection purpose. + * The default margin is 4cm (if your units are meters, which is recommended). In case, you want to simulate small + * objects (smaller than the margin distance), you might want to reduce the margin by specifying your own margin + * distance using the "margin" parameter in the constructor of the box shape. Otherwise, it is recommended to use the + * default margin distance by not using the "margin" parameter in the constructor. + * + * @author Jason Sorensen + */ +public class BoxShape extends ConvexShape { + + // Extent sizes of the box in the x, y and z direction + private final Vector3f extent; + + // Copy-constructor + public BoxShape(final BoxShape shape) { + super(CollisionShapeType.BOX, shape.margin); + this.extent = shape.extent.clone(); + } + + // Constructor + public BoxShape(final Vector3f extent) { + this(extent, Defaults.OBJECT_MARGIN); + } + + public BoxShape(final Vector3f extent, final float margin) { + super(CollisionShapeType.BOX, margin); + assert (extent.getX() > 0.0f && extent.getX() > margin); + assert (extent.getY() > 0.0f && extent.getY() > margin); + assert (extent.getZ() > 0.0f && extent.getZ() > margin); + this.extent = extent.lessNew(margin); + } + + @Override + public BoxShape clone() { + return new BoxShape(this); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f tensor, final float _mass) { + final float factor = (1.0f / 3.0f) * _mass; + final Vector3f realExtent = this.extent.addNew(new Vector3f(this.margin, this.margin, this.margin)); + final float xSquare = realExtent.x * realExtent.x; + final float ySquare = realExtent.y * realExtent.y; + final float zSquare = realExtent.z * realExtent.z; + tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare)); + } + + public Vector3f getExtent() { + return (new Vector3f(this.margin, this.margin, this.margin)).add(this.extent); + } + + @Override + public void getLocalBounds(final Vector3f _min, final Vector3f _max) { + // Maximum bounds + _max.set(this.extent.x + this.margin, this.extent.y + this.margin, this.extent.z + this.margin); + // Minimum bounds + _min.set(-_max.x, -_max.y, -_max.z); + } + + /* + protected size_t getSizeInBytes() { + return sizeof(BoxShape); + } + */ + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + //Log.error("getLocalSupportPointWithoutMargin(" + _direction); + //Log.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); + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f rayDirection = _ray.point2.less(_ray.point1); + float tMin = Float.MIN_VALUE; + float tMax = Float.MAX_VALUE; + Vector3f normalDirection = new Vector3f(0, 0, 0); + Vector3f currentNormal = new Vector3f(0, 0, 0); + // For each of the three slabs + for (int iii = 0; iii < 3; ++iii) { + // If ray is parallel to the slab + if (FMath.abs(rayDirection.get(iii)) < Constant.FLOAT_EPSILON) { + // If the ray's origin is not inside the slab, there is no hit + if (_ray.point1.get(iii) > this.extent.get(iii) || _ray.point1.get(iii) < -this.extent.get(iii)) { + return false; + } + } else { + // Compute the intersection of the ray with the near and far plane of the slab + final float oneOverD = 1.0f / rayDirection.get(iii); + float t1 = (-this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD; + float t2 = (this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD; + currentNormal.x = (iii == 0) ? -this.extent.get(iii) : 0.0f; + currentNormal.y = (iii == 1) ? -this.extent.get(iii) : 0.0f; + currentNormal.z = (iii == 2) ? -this.extent.get(iii) : 0.0f; + // Swap t1 and t2 if need so that t1 is intersection with near plane and + // t2 with far plane + if (t1 > t2) { + final float ttt = t2; + t2 = t1; + t1 = ttt; + currentNormal = currentNormal.multiply(-1); + } + // Compute the intersection of the of slab intersection interval with previous slabs + if (t1 > tMin) { + tMin = t1; + normalDirection = currentNormal; + } + tMax = FMath.min(tMax, t2); + // If tMin is larger than the maximum raycasting fraction, we return no hit + if (tMin > _ray.maxFraction) { + return false; + } + // If the slabs intersection is empty, there is no hit + if (tMin > tMax) { + return false; + } + } + } + // If tMin is negative, we return no hit + if (tMin < 0.0f || tMin > _ray.maxFraction) { + return false; + } + if (normalDirection.isZero()) { + return false; + } + // The ray longersects the three slabs, we compute the hit point + final Vector3f localHitPoint = rayDirection.multiplyNew(tMin).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = tMin; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.worldNormal = normalDirection; + return true; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.extent.divide(this.scaling).multiply(_scaling); + super.setLocalScaling(_scaling); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + return (_localPoint.x < this.extent.x && _localPoint.x > -this.extent.x && _localPoint.y < this.extent.y && _localPoint.y > -this.extent.y && _localPoint.z < this.extent.z + && _localPoint.z > -this.extent.z); + } + +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/CacheData.java b/src/org/atriasoft/ephysics/collision/shapes/CacheData.java new file mode 100644 index 0000000..b147dab --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/CacheData.java @@ -0,0 +1,10 @@ +/** @file + * @author Edouard DUPIN + * @copyright 2020-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +public class CacheData { + public Object data = null; +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/CapsuleShape.java b/src/org/atriasoft/ephysics/collision/shapes/CapsuleShape.java new file mode 100644 index 0000000..da2926e --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/CapsuleShape.java @@ -0,0 +1,284 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a capsule collision shape that is defined around the Y axis. A capsule shape can be seen as the + * convex hull of two spheres. The capsule shape is defined by its radius (radius of the two spheres of the capsule) and + * its height (distance between the centers of the two spheres). This collision shape does not have an explicit object + * margin distance. The margin is implicitly the radius and height of the shape. Therefore, no need to specify an object + * margin for a capsule shape. + * + * @author Jason Sorensen + */ +public class CapsuleShape extends ConvexShape { + // Half height of the capsule (height = distance between the centers of the two spheres) + private float halfHeight; + + // Copy-constructor + public CapsuleShape(final CapsuleShape shape) { + super(CollisionShapeType.CAPSULE, shape.margin); + this.halfHeight = shape.halfHeight; + } + + // Constructor + public CapsuleShape(final float radius, final float height) { + // TODO: Should radius really be the margin for a capsule? Seems like a bug. + super(CollisionShapeType.CAPSULE, radius); + this.halfHeight = height * 0.5f; + } + + @Override + public CapsuleShape clone() { + return new CapsuleShape(this); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 + final float height = this.halfHeight + this.halfHeight; + final float radiusSquare = this.margin * this.margin; + final float heightSquare = height * height; + final float radiusSquareDouble = radiusSquare + radiusSquare; + final float factor1 = 2.0f * this.margin / (4.0f * this.margin + 3.0f * height); + final float factor2 = 3.0f * height / (4.0f * this.margin + 3.0f * height); + final float sum1 = 0.4f * radiusSquareDouble; + final float sum2 = 0.75f * height * this.margin + 0.5f * heightSquare; + final float sum3 = 0.25f * radiusSquare + 1.0f / 12.0f * heightSquare; + final float IxxAndzz = factor1 * _mass * (sum1 + sum2) + factor2 * _mass * sum3; + final float Iyy = factor1 * _mass * sum1 + factor2 * _mass * 0.25f * radiusSquareDouble; + _tensor.set(IxxAndzz, 0.0f, 0.0f, 0.0f, Iyy, 0.0f, 0.0f, 0.0f, IxxAndzz); + } + + // Return the height of the capsule + public float getHeight() { + return this.halfHeight + this.halfHeight; + } + + @Override + public void getLocalBounds(final Vector3f min, final Vector3f max) { + // Maximum bounds + max.setX(this.margin); + max.setY(this.halfHeight + this.margin); + max.setZ(this.margin); + // Minimum bounds + min.setX(-max.x); + min.setY(-max.y); + min.setZ(-max.z); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + // Support point top sphere + final float dotProductTop = this.halfHeight * _direction.y; + // Support point bottom sphere + final float dotProductBottom = -this.halfHeight * _direction.y; + // Return the point with the maximum dot product + if (dotProductTop > dotProductBottom) { + return new Vector3f(0, this.halfHeight, 0); + } + return new Vector3f(0, -this.halfHeight, 0); + } + + // Get the radius of the capsule + public float getRadius() { + return this.margin; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f n = _ray.point2.lessNew(_ray.point1); + final float epsilon = 0.01f; + final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f); + final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f); + final Vector3f d = q.lessNew(p); + final Vector3f m = _ray.point1.lessNew(p); + float t; + final float mDotD = m.dot(d); + final float nDotD = n.dot(d); + final float dDotD = d.dot(d); + // Test if the segment is outside the cylinder + final float vec1DotD = _ray.point1.lessNew(new Vector3f(0.0f, -this.halfHeight - this.margin, 0.0f)).dot(d); + if (vec1DotD < 0.0f && vec1DotD + nDotD < 0.0f) { + return false; + } + final float ddotDExtraCaps = 2.0f * this.margin * d.y; + if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) { + return false; + } + final float nDotN = n.dot(n); + final float mDotN = m.dot(n); + final float a = dDotD * nDotN - nDotD * nDotD; + final float k = m.dot(m) - this.margin * this.margin; + final float c = dDotD * k - mDotD * mDotD; + // If the ray is parallel to the capsule axis + if (FMath.abs(a) < epsilon) { + // If the origin is outside the surface of the capusle's cylinder, we return no hit + if (c > 0.0f) { + return false; + } + // Here we know that the segment longersect an endcap of the capsule + // If the ray longersects with the "p" endcap of the capsule + if (mDotD < 0.0f) { + // Check longersection between the ray and the "p" sphere endcap of the capsule + final Vector3f hitLocalPoint = new Vector3f(); + final Float hitFraction = 0.0f; + if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) { + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldPoint = hitLocalPoint; + final Vector3f normalDirection = hitLocalPoint.lessNew(p); + _raycastInfo.worldNormal = normalDirection; + return true; + } + return false; + } else if (mDotD > dDotD) { // If the ray longersects with the "q" endcap of the cylinder + // Check longersection between the ray and the "q" sphere endcap of the capsule + final Vector3f hitLocalPoint = new Vector3f(); + final Float hitFraction = 0.0f; + if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) { + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldPoint = hitLocalPoint; + final Vector3f normalDirection = hitLocalPoint.lessNew(q); + _raycastInfo.worldNormal = normalDirection; + return true; + } + return false; + } else { + // If the origin is inside the cylinder, we return no hit + return false; + } + } + final float b = dDotD * mDotN - nDotD * mDotD; + final float discriminant = b * b - a * c; + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < 0.0f) { + return false; + } + // Compute the smallest root (first longersection along the ray) + final float t0 = t = (-b - FMath.sqrt(discriminant)) / a; + // If the longersection is outside the finite cylinder of the capsule on "p" endcap side + final float value = mDotD + t * nDotD; + if (value < 0.0f) { + // Check longersection between the ray and the "p" sphere endcap of the capsule + final Vector3f hitLocalPoint = new Vector3f(); + final Float hitFraction = 0.0f; + if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) { + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldPoint = hitLocalPoint; + final Vector3f normalDirection = hitLocalPoint.lessNew(p); + _raycastInfo.worldNormal = normalDirection; + return true; + } + return false; + } else if (value > dDotD) { // If the longersection is outside the finite cylinder on the "q" side + // Check longersection between the ray and the "q" sphere endcap of the capsule + final Vector3f hitLocalPoint = new Vector3f(); + final Float hitFraction = 0.0f; + if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) { + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldPoint = hitLocalPoint; + final Vector3f normalDirection = hitLocalPoint.lessNew(q); + _raycastInfo.worldNormal = normalDirection; + return true; + } + return false; + } + t = t0; + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + final Vector3f v = localHitPoint.lessNew(p); + final Vector3f w = d.multiplyNew(v.dot(d) / d.length2()); + final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w)).safeNormalize(); + _raycastInfo.worldNormal = normalDirection; + return true; + } + + /** + * @brief Raycasting method between a ray one of the two spheres end cap of the capsule + */ + protected boolean raycastWithSphereEndCap(final Vector3f _point1, final Vector3f _point2, final Vector3f _sphereCenter, final float _maxFraction, Vector3f _hitLocalPoint, Float _hitFraction) { + final Vector3f m = _point1.lessNew(_sphereCenter); + final float c = m.dot(m) - this.margin * this.margin; + // If the origin of the ray is inside the sphere, we return no longersection + if (c < 0.0f) { + return false; + } + final Vector3f rayDirection = _point2.lessNew(_point1); + final float b = m.dot(rayDirection); + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere, there is no longersection + if (b > 0.0f) { + return false; + } + final float raySquareLength = rayDirection.length2(); + // Compute the discriminant of the quadratic equation + final float discriminant = b * b - raySquareLength * c; + // If the discriminant is negative or the ray length is very small, there is no longersection + if (discriminant < 0.0f || raySquareLength < Constant.FLOAT_EPSILON) { + return false; + } + // Compute the solution "t" closest to the origin + float t = -b - FMath.sqrt(discriminant); + assert (t >= 0.0f); + // If the hit point is withing the segment ray fraction + if (t < _maxFraction * raySquareLength) { + // Compute the longersection information + t /= raySquareLength; + _hitFraction = t; + _hitLocalPoint = rayDirection.multiplyNew(t).add(_point1); + return true; + } + return false; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y; + this.margin = (this.margin / this.scaling.x) * _scaling.x; + super.setLocalScaling(_scaling); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + final float diffYCenterSphere1 = _localPoint.y - this.halfHeight; + final float diffYCenterSphere2 = _localPoint.y + this.halfHeight; + final float xSquare = _localPoint.x * _localPoint.x; + final float zSquare = _localPoint.z * _localPoint.z; + final float squareRadius = this.margin * this.margin; + // Return true if the point is inside the cylinder or one of the two spheres of the capsule + return ((xSquare + zSquare) < squareRadius && _localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight) + || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; + } + +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/CollisionShape.java b/src/org/atriasoft/ephysics/collision/shapes/CollisionShape.java new file mode 100644 index 0000000..3406a7c --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/CollisionShape.java @@ -0,0 +1,126 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; + +/** + * This abstract class represents the collision shape associated with a + * body that is used during the narrow-phase collision detection. + */ +public abstract class CollisionShape { + /** + * @brief Get the maximum number of contact + * @return The maximum number of contact manifolds in an overlapping pair given two shape types + */ + public static int computeNbMaxContactManifolds(final CollisionShapeType _shapeType1, final CollisionShapeType _shapeType2) { + // If both shapes are convex + if (isConvex(_shapeType1) && isConvex(_shapeType2)) { + return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE; + } + // If there is at least one concave shape + return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE; + } + + /** + * @brief Check if the shape is convex + * @param[in] _shapeType shape type + * @return true If the collision shape is convex + * @return false If it is concave + */ + public static boolean isConvex(final CollisionShapeType _shapeType) { + return _shapeType != CollisionShapeType.CONCAVE_MESH && _shapeType != CollisionShapeType.HEIGHTFIELD; + } + + protected CollisionShapeType type; //!< Type of the collision shape + protected Vector3f scaling; //!< Scaling vector of the collision shape + /// Constructor + + public CollisionShape(final CollisionShapeType type) { + this.type = type; + this.scaling = new Vector3f(1.0f, 1.0f, 1.0f); + } + + /** + * @brief Update the AABB of a body using its collision shape + * @param[out] _aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates + * @param[in] _transform Transform3D used to compute the AABB of the collision shape + */ + public void computeAABB(final AABB _aabb, final Transform3D _transform) { + // Get the local bounds in x,y and z direction + final Vector3f minBounds = new Vector3f(0, 0, 0); + final Vector3f maxBounds = new Vector3f(0, 0, 0); + getLocalBounds(minBounds, maxBounds); + // Rotate the local bounds according to the orientation of the body + final Matrix3f worldAxis = _transform.getOrientation().getMatrix().getAbsolute(); + final Vector3f worldMinBounds = new Vector3f(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds)); + final Vector3f worldMaxBounds = new Vector3f(worldAxis.getColumn(0).dot(maxBounds), worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(2).dot(maxBounds)); + // Compute the minimum and maximum coordinates of the rotated extents + final Vector3f minCoordinates = _transform.getPosition().addNew(worldMinBounds); + final Vector3f maxCoordinates = _transform.getPosition().addNew(worldMaxBounds); + // Update the AABB with the new minimum and maximum coordinates + _aabb.setMin(minCoordinates); + _aabb.setMax(maxCoordinates); + } + + /** + * @brief Compute the local inertia tensor of the sphere + * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates + * @param[in] _mass Mass to use to compute the inertia tensor of the collision shape + */ + public abstract void computeLocalInertiaTensor(Matrix3f _tensor, float _mass); + + /** + * @brief Get the local bounds of the shape in x, y and z directions. + * This method is used to compute the AABB of the box + * @param _min The minimum bounds of the shape in local-space coordinates + * @param _max The maximum bounds of the shape in local-space coordinates + */ + public abstract void getLocalBounds(Vector3f _min, Vector3f _max); + + /// Return the scaling vector of the collision shape + public Vector3f getScaling() { + return this.scaling; + } + + /** + * @brief Get the type of the collision shapes + * @return The type of the collision shape (box, sphere, cylinder, ...) + */ + public CollisionShapeType getType() { + return this.type; + } + + /** + * @brief Check if the shape is convex + * @return true If the collision shape is convex + * @return false If it is concave + */ + public abstract boolean isConvex(); + + /// Raycast method with feedback information + public abstract boolean raycast(Ray ray, RaycastInfo raycastInfo, ProxyShape proxyShape); + + /** + * @brief Set the scaling vector of the collision shape + */ + public void setLocalScaling(final Vector3f _scaling) { + this.scaling = _scaling; + } + + /// Return true if a point is inside the collision shape + public abstract boolean testPointInside(Vector3f worldPoint, ProxyShape proxyShape); +}; diff --git a/src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java b/src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java new file mode 100644 index 0000000..709af01 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java @@ -0,0 +1,58 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +/** + * Type of the collision shape + * + * @author Jason Sorensen + */ +public enum CollisionShapeType { + TRIANGLE(0), + BOX(1), + SPHERE(2), + CONE(3), + CYLINDER(4), + CAPSULE(5), + CONVEX_MESH(6), + CONCAVE_MESH(7), + HEIGHTFIELD(8); + + public static final int NB_COLLISION_SHAPE_TYPES = 9; + + public static CollisionShapeType getType(final int value) { + switch (value) { + case 0: + return TRIANGLE; + case 1: + return BOX; + case 2: + return SPHERE; + case 3: + return CONE; + case 4: + return CYLINDER; + case 5: + return CAPSULE; + case 6: + return CONVEX_MESH; + case 7: + return CONCAVE_MESH; + case 8: + return HEIGHTFIELD; + } + return null; + } + + public final int value; + + private CollisionShapeType(final int value) { + this.value = value; + } +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java new file mode 100644 index 0000000..40ce4ad --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java @@ -0,0 +1,210 @@ +package org.atriasoft.ephysics.collision.shapes; + +import java.util.ArrayList; +import java.util.List; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.Triangle; +import org.atriasoft.ephysics.collision.TriangleMesh; +import org.atriasoft.ephysics.collision.TriangleVertexArray; +import org.atriasoft.ephysics.collision.broadphase.CallbackOverlapping; +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; + +/** + * Represents a static concave mesh shape. Note that collision detection + * with a concave mesh shape can be very expensive. You should use only use + * this shape for a static mesh. + */ +public class ConcaveMeshShape extends ConcaveShape { + class ConcaveMeshRaycastCallback { + private final List hitAABBNodes = new ArrayList<>(); + private final DynamicAABBTree dynamicAABBTree; + private final ConcaveMeshShape concaveMeshShape; + private final ProxyShape proxyShape; + private final RaycastInfo raycastInfo; + private final Ray ray; + private boolean isHit; + + // Constructor + ConcaveMeshRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final ConcaveMeshShape _concaveMeshShape, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final Ray _ray) { + this.dynamicAABBTree = _dynamicAABBTree; + this.concaveMeshShape = _concaveMeshShape; + this.proxyShape = _proxyShape; + this.raycastInfo = _raycastInfo; + this.ray = _ray; + this.isHit = false; + + } + + /// Return true if a raycast hit has been found + public boolean getIsHit() { + return this.isHit; + } + + /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree + public float operator__parenthese(final DTree _node, final Ray _ray) { + // Add the id of the hit AABB node longo + this.hitAABBNodes.add(_node); + return _ray.maxFraction; + } + + /// Raycast all collision shapes that have been collected + public void raycastTriangles() { + float smallestHitFraction = this.ray.maxFraction; + for (final DTree it : this.hitAABBNodes) { + // Get the node data (triangle index and mesh subpart index) + final int data_0 = this.dynamicAABBTree.getNodeDataInt_0(it); + final int data_1 = this.dynamicAABBTree.getNodeDataInt_1(it); + // Get the triangle vertices for this node from the concave mesh shape + final Vector3f[] trianglePoints = new Vector3f[3]; + this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints); + // Create a triangle collision shape + final float margin = this.concaveMeshShape.getTriangleMargin(); + + 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(); + final boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape); + // If the ray hit the collision shape + if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { + assert (raycastInfo.hitFraction >= 0.0f); + this.raycastInfo.body = raycastInfo.body; + this.raycastInfo.proxyShape = raycastInfo.proxyShape; + this.raycastInfo.hitFraction = raycastInfo.hitFraction; + this.raycastInfo.worldPoint = raycastInfo.worldPoint; + this.raycastInfo.worldNormal = raycastInfo.worldNormal; + this.raycastInfo.meshSubpart = data_0; + this.raycastInfo.triangleIndex = data_1; + smallestHitFraction = raycastInfo.hitFraction; + this.isHit = true; + } + } + } + + }; + + protected TriangleMesh triangleMesh; //!< Triangle mesh + + protected DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles + + public ConcaveMeshShape(final TriangleMesh _triangleMesh) { + super(CollisionShapeType.CONCAVE_MESH); + this.triangleMesh = _triangleMesh; + this.raycastTestType = TriangleRaycastSide.FRONT; + initBVHTree(); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + _tensor.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass); + } + + @Override + public void getLocalBounds(final Vector3f _min, final Vector3f _max) { + // Get the AABB of the whole tree + final AABB treeAABB = this.dynamicAABBTree.getRootAABB(); + _min.set(treeAABB.getMin()); + _max.set(treeAABB.getMax()); + } + + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + protected void getTriangleVerticesWithIndexPointer(final int _subPart, final int _triangleIndex, final Vector3f[] _outTriangleVertices) { + 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 ..."); + } + final Triangle trianglePoints = triangleVertexArray.getTriangle(_triangleIndex); + _outTriangleVertices[0] = trianglePoints.get(0).multiplyNew(this.scaling); + _outTriangleVertices[1] = trianglePoints.get(1).multiplyNew(this.scaling); + _outTriangleVertices[2] = trianglePoints.get(2).multiplyNew(this.scaling); + } + + /// Insert all the triangles longo the dynamic AABB tree + protected void initBVHTree() { + // TODO : Try to randomly add the triangles into the tree to obtain a better tree + // For each sub-part of the mesh + for (int subPart = 0; subPart < this.triangleMesh.getNbSubparts(); subPart++) { + // Get the triangle vertex array of the current sub-part + final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart); + // For each triangle of the concave mesh + for (int iii = 0; iii < triangleVertexArray.getNbTriangles(); ++iii) { + final Triangle trianglePoints = triangleVertexArray.getTriangle(iii); + final Vector3f[] trianglePoints2 = new Vector3f[3]; + trianglePoints2[0] = trianglePoints.get(0); + trianglePoints2[1] = trianglePoints.get(1); + trianglePoints2[2] = trianglePoints.get(2); + // Create the AABB for the triangle + final AABB aabb = AABB.createAABBForTriangle(trianglePoints2); + aabb.inflate(this.triangleMargin, this.triangleMargin, this.triangleMargin); + // Add the AABB with the index of the triangle longo the dynamic AABB tree + this.dynamicAABBTree.addObject(aabb, subPart, iii); + } + } + } + + @Override + public boolean isConvex() { + return false; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + // Create the callback object that will compute ray casting against triangles + final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(this.dynamicAABBTree, this, _proxyShape, _raycastInfo, _ray); + // Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray. + // The raycastCallback object will then compute ray casting against the triangles + // in the hit AABBs. + this.dynamicAABBTree.raycast(_ray, new CallbackRaycast() { + + @Override + public float callback(final DTree _node, final Ray _ray) { + return raycastCallback.operator__parenthese(_node, _ray); + } + }); + raycastCallback.raycastTriangles(); + return raycastCallback.getIsHit(); + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + super.setLocalScaling(_scaling); + this.dynamicAABBTree.reset(); + initBVHTree(); + } + + @Override + public void testAllTriangles(final ConcaveShape.TriangleCallback _callback, final AABB _localAABB) { + // Ask the Dynamic AABB Tree to report all the triangles that are overlapping + // with the AABB of the convex shape. + final ConcaveMeshShape self = this; + this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, new CallbackOverlapping() { + @Override + public void callback(final DTree _node) { + // Get the node data (triangle index and mesh subpart index) + final int data_0 = self.dynamicAABBTree.getNodeDataInt_0(_node); + final int data_1 = self.dynamicAABBTree.getNodeDataInt_1(_node); + // Get the triangle vertices for this node from the concave mesh shape + final Vector3f[] trianglePoints = new Vector3f[3]; + getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints); + // Call the callback to test narrow-phase collision with this triangle + _callback.testTriangle(trianglePoints); + } + }); + } +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConcaveShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConcaveShape.java new file mode 100644 index 0000000..643d678 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/ConcaveShape.java @@ -0,0 +1,88 @@ +/** @file + * Original ReactPhysics3D C++ library by Daniel Chappuis This code is re-licensed with permission from ReactPhysics3D author. + * @author Daniel CHAPPUIS + * @author Edouard DUPIN + * @copyright 2010-2016, Daniel Chappuis + * @copyright 2017-now, Edouard DUPIN + * @license MPL v2.0 (see license file) + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide; + +/** + * This abstract class represents a concave collision shape associated with a + * body that is used during the narrow-phase collision detection. + */ +public abstract class ConcaveShape extends CollisionShape { + + /** + * It is used to encapsulate a callback method for + * a single triangle of a ConcaveMesh. + */ + public interface TriangleCallback { + /// Report a triangle + public void testTriangle(Vector3f[] _trianglePoints); + } + + boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled + + protected float triangleMargin; //!< Margin use for collision detection for each triangle + + protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) + + /// Return true if the collision shape is convex, false if it is concave + public boolean isConvex; + + public ConcaveShape(final CollisionShapeType _type) { + super(_type); + this.isSmoothMeshCollisionEnabled = false; + this.triangleMargin = 0; + this.raycastTestType = TriangleRaycastSide.FRONT; + } + + /// Return true if the smooth mesh collision is enabled + public boolean getIsSmoothMeshCollisionEnabled() { + return this.isSmoothMeshCollisionEnabled; + } + + /// Return the raycast test type (front, back, front-back) + public TriangleRaycastSide getRaycastTestType() { + return this.raycastTestType; + } + + /// Return the triangle margin + public float getTriangleMargin() { + return this.triangleMargin; + } + + /** + * Enable/disable the smooth mesh collision algorithm + * + * Smooth mesh collision is used to avoid collisions against some longernal edges of the triangle mesh. + * If it is enabled, collsions with the mesh will be smoother but collisions computation is a bit more expensive. + */ + public void setIsSmoothMeshCollisionEnabled(final boolean _isEnabled) { + this.isSmoothMeshCollisionEnabled = _isEnabled; + } + + /** + * Set the raycast test type (front, back, front-back) + * @param testType Raycast test type for the triangle (front, back, front-back) + */ + public void setRaycastTestType(final TriangleRaycastSide _testType) { + this.raycastTestType = _testType; + } + + /// Use a callback method on all triangles of the concave shape inside a given AABB + public abstract void testAllTriangles(TriangleCallback _callback, AABB _localAABB); + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + return false; + } + +}; diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConeShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConeShape.java new file mode 100644 index 0000000..9ab61f6 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/ConeShape.java @@ -0,0 +1,255 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a cone collision shape centered at the + * origin and alligned with the Y axis. The cone is defined + * by its height and by the radius of its base. The center of the + * cone is at the half of the height. The "transform" of the + * corresponding rigid body gives an orientation and a position + * to the cone. This collision shape uses an extra margin distance around + * it for collision detection purpose. The default margin is 4cm (if your + * units are meters, which is recommended). In case, you want to simulate small + * objects (smaller than the margin distance), you might want to reduce the margin + * by specifying your own margin distance using the "margin" parameter in the + * ructor of the cone shape. Otherwise, it is recommended to use the + * default margin distance by not using the "margin" parameter in the ructor. + */ +public class ConeShape extends ConvexShape { + protected float radius; //!< Radius of the base + protected float halfHeight; //!< Half height of the cone + + protected float sinTheta; //!< sine of the semi angle at the apex point + + /** + * Constructor + * @param _radius Radius of the cone (in meters) + * @param _height Height of the cone (in meters) + * @param _margin Collision margin (in meters) around the collision shape + */ + public ConeShape(final float _radius, final float _height) { + this(_radius, _height, Defaults.OBJECT_MARGIN); + } + + public ConeShape(final float _radius, final float _height, final float _margin) { + super(CollisionShapeType.CONE, _margin); + this.radius = _radius; + this.halfHeight = _height * 0.5f; + // Compute the sine of the semi-angle at the apex point + this.sinTheta = this.radius / (FMath.sqrt(this.radius * this.radius + _height * _height)); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + final float rSquare = this.radius * this.radius; + final float diagXZ = 0.15f * _mass * (rSquare + this.halfHeight); + _tensor.set(diagXZ, 0.0f, 0.0f, 0.0f, 0.3f * _mass * rSquare, 0.0f, 0.0f, 0.0f, diagXZ); + } + + /** + * Return the height + * @return Height of the cone (in meters) + */ + public float getHeight() { + return 2.0f * this.halfHeight; + } + + @Override + public void getLocalBounds(final Vector3f min, final Vector3f max) { + // Maximum bounds + max.setX(this.radius + this.margin); + max.setY(this.halfHeight + this.margin); + max.setZ(this.radius + this.margin); + // Minimum bounds + min.setX(-max.x); + min.setY(-max.y); + min.setZ(-max.z); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + final Vector3f v = _direction; + final float sinThetaTimesLengthV = this.sinTheta * v.length(); + Vector3f supportPoint; + if (v.y > sinThetaTimesLengthV) { + supportPoint = new Vector3f(0.0f, this.halfHeight, 0.0f); + } else { + final float projectedLength = FMath.sqrt(v.x * v.x + v.z * v.z); + if (projectedLength > Constant.FLOAT_EPSILON) { + final float d = this.radius / projectedLength; + supportPoint = new Vector3f(v.x * d, -this.halfHeight, v.z * d); + } else { + supportPoint = new Vector3f(0.0f, -this.halfHeight, 0.0f); + } + } + return supportPoint; + } + + /** + * Return the radius + * @return Radius of the cone (in meters) + */ + public float getRadius() { + return this.radius; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f r = _ray.point2.lessNew(_ray.point1); + final float epsilon = 0.00001f; + final Vector3f V = new Vector3f(0, this.halfHeight, 0); + final Vector3f centerBase = new Vector3f(0, -this.halfHeight, 0); + final Vector3f axis = new Vector3f(0, -1.0f, 0); + final float heightSquare = 4.0f * this.halfHeight * this.halfHeight; + final float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius); + final float factor = 1.0f - cosThetaSquare; + final Vector3f delta = _ray.point1.lessNew(V); + final float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - cosThetaSquare * delta.z * delta.z; + final float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z; + final float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z; + final float tHit[] = { -1.0f, -1.0f, -1.0f }; + final Vector3f[] localHitPoint = new Vector3f[3]; + final Vector3f[] localNormal = new Vector3f[3]; + // If c2 is different from zero + if (FMath.abs(c2) > Constant.FLOAT_EPSILON) { + final float gamma = c1 * c1 - c0 * c2; + // If there is no real roots in the quadratic equation + if (gamma < 0.0f) { + return false; + } else if (gamma > 0.0f) { // The equation has two real roots + // Compute two longersections + final float sqrRoot = FMath.sqrt(gamma); + tHit[0] = (-c1 - sqrRoot) / c2; + tHit[1] = (-c1 + sqrRoot) / c2; + } else { // If the equation has a single real root + // Compute the longersection + tHit[0] = -c1 / c2; + } + } else // If c2 == 0 + if (FMath.abs(c1) > Constant.FLOAT_EPSILON) { + // If c2 = 0 and c1 != 0 + tHit[0] = -c0 / (2.0f * c1); + } else { + // If c2 = c1 = 0 + // If c0 is different from zero, no solution and if c0 = 0, we have a + // degenerate case, the whole ray is contained in the cone side + // but we return no hit in this case + return false; + } + // If the origin of the ray is inside the cone, we return no hit + if (testPointInside(_ray.point1, null)) { + return false; + } + localHitPoint[0] = r.multiplyNew(tHit[0]).add(_ray.point1); + localHitPoint[1] = r.multiplyNew(tHit[1]).add(_ray.point1); + // Only keep hit points in one side of the double cone (the cone we are longerested in) + if (axis.dot(localHitPoint[0].lessNew(V)) < 0.0f) { + tHit[0] = -1.0f; + } + if (axis.dot(localHitPoint[1].lessNew(V)) < 0.0f) { + tHit[1] = -1.0f; + } + // Only keep hit points that are within the correct height of the cone + if (localHitPoint[0].y < -this.halfHeight) { + tHit[0] = -1.0f; + } + if (localHitPoint[1].y < -this.halfHeight) { + tHit[1] = -1.0f; + } + // If the ray is in direction of the base plane of the cone + if (r.y > epsilon) { + // Compute the longersection with the base plane of the cone + tHit[2] = (-_ray.point1.y - this.halfHeight) / (r.y); + // Only keep this longersection if it is inside the cone radius + localHitPoint[2] = r.multiplyNew(tHit[2]).add(_ray.point1); + if ((localHitPoint[2].lessNew(centerBase)).length2() > this.radius * this.radius) { + tHit[2] = -1.0f; + } + // Compute the normal direction + localNormal[2] = axis; + } + // Find the smallest positive t value + int hitIndex = -1; + float t = Float.MAX_VALUE; + for (int i = 0; i < 3; i++) { + if (tHit[i] < 0.0f) { + continue; + } + if (tHit[i] < t) { + hitIndex = i; + t = tHit[hitIndex]; + } + } + if (hitIndex < 0) { + return false; + } + if (t > _ray.maxFraction) { + return false; + } + // Compute the normal direction for hit against side of the cone + if (hitIndex != 2) { + final float h = 2.0f * this.halfHeight; + final float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + localHitPoint[hitIndex].z * localHitPoint[hitIndex].z); + final float rOverH = this.radius / h; + final float value2 = 1.0f + rOverH * rOverH; + final float factor22 = 1.0f / FMath.sqrt(value1 * value2); + final float x = localHitPoint[hitIndex].x * factor22; + final float z = localHitPoint[hitIndex].z * factor22; + localNormal[hitIndex].setX(x); + localNormal[hitIndex].setY(FMath.sqrt(x * x + z * z) * rOverH); + localNormal[hitIndex].setZ(z); + } + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint[hitIndex]; + _raycastInfo.worldNormal = localNormal[hitIndex]; + return true; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y; + this.radius = (this.radius / this.scaling.x) * _scaling.x; + super.setLocalScaling(_scaling); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + final float radiusHeight = this.radius * (-_localPoint.y + this.halfHeight) / (this.halfHeight * 2.0f); + return (_localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight) && (_localPoint.x * _localPoint.x + _localPoint.z * _localPoint.z < radiusHeight * radiusHeight); + } + +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConvexMeshShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConvexMeshShape.java new file mode 100644 index 0000000..cac50ac --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/ConvexMeshShape.java @@ -0,0 +1,343 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.collision.shapes; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.TriangleVertexArray; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.ephysics.mathematics.SetInteger; + +/** + * It represents a convex mesh shape. In order to create a convex mesh shape, you + * need to indicate the local-space position of the mesh vertices. You do it either by + * passing a vertices array to the ructor or using the addVertex method. Make sure + * that the set of vertices that you use to create the shape are indeed part of a convex + * mesh. The center of mass of the shape will be at the origin of the local-space geometry + * that you use to create the mesh. The method used for collision detection with a convex + * mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh. + * Therefore, you should try not to use too many vertices. However, it is possible to speed + * up the collision detection by using the edges information of your mesh. The running time + * of the collision detection that uses the edges is almost O(1) ant time at the cost + * of additional memory used to store the vertices. You can indicate edges information + * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method + * in order to use the edges information for collision detection. + */ +public class ConvexMeshShape extends ConvexShape { + protected List vertices = new ArrayList<>(); //!< Array with the vertices of the mesh + protected int numberVertices = 0; //!< Number of vertices in the mesh + protected Vector3f minBounds = new Vector3f(); //!< Mesh minimum bounds in the three local x, y and z directions + protected Vector3f maxBounds = new Vector3f(); //!< Mesh maximum bounds in the three local x, y and z directions + protected boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster + + protected Map edgesAdjacencyList = new HashMap<>(); //!< Adjacency list representing the edges of the mesh + + /** + * Constructor. + * If you use this ructor, you will need to set the vertices manually one by one using the addVertex method. + */ + public ConvexMeshShape() { + this(Defaults.OBJECT_MARGIN); + } + + public ConvexMeshShape(final float _margin) { + super(CollisionShapeType.CONVEX_MESH, _margin); + this.minBounds = new Vector3f(0, 0, 0); + this.maxBounds = new Vector3f(0, 0, 0); + this.numberVertices = 0; + this.isEdgesInformationUsed = false; + } + + /** + * Constructor to initialize with an array of 3D vertices. + * This method creates an longernal copy of the input vertices. + * @param _arrayVertices Array with the vertices of the convex mesh + * @param _nbVertices Number of vertices in the convex mesh + * @param _stride Stride between the beginning of two elements in the vertices array + * @param _margin Collision margin (in meters) around the collision shape + */ + public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride) { + this(_arrayVertices, _nbVertices, _stride, Defaults.OBJECT_MARGIN); + } + + public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride, final float _margin) { + super(CollisionShapeType.CONVEX_MESH, _margin); + this.numberVertices = _nbVertices; + this.minBounds = new Vector3f(0, 0, 0); + this.maxBounds = new Vector3f(0, 0, 0); + this.isEdgesInformationUsed = false; + int offset = 0; + // Copy all the vertices longo the longernal array + for (long iii = 0; iii < this.numberVertices; iii++) { + this.vertices.add(new Vector3f(_arrayVertices[offset], _arrayVertices[offset + 1], _arrayVertices[offset + 2])); + offset += _stride; + } + // Recalculate the bounds of the mesh + recalculateBounds(); + } + + /** + * Constructor to initialize with a triangle mesh + * This method creates an internal copy of the input vertices. + * @param _triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh + * @param _isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory) + * @param _margin Collision margin (in meters) around the collision shape + */ + public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray) { + this(_triangleVertexArray, true); + } + + public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed) { + this(_triangleVertexArray, true, Defaults.OBJECT_MARGIN); + } + + public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed, final float _margin) { + super(CollisionShapeType.CONVEX_MESH, _margin); + this.minBounds = new Vector3f(0, 0, 0); + this.maxBounds = new Vector3f(0, 0, 0); + this.isEdgesInformationUsed = _isEdgesInformationUsed; + // For each vertex of the mesh + for (final Vector3f it : _triangleVertexArray.getVertices()) { + this.vertices.add(it.multiplyNew(this.scaling)); + } + // If we need to use the edges information of the mesh + if (this.isEdgesInformationUsed) { + // For each triangle of the mesh + for (int iii = 0; iii < _triangleVertexArray.getNbTriangles(); iii++) { + final int vertexIndex[] = { 0, 0, 0 }; + vertexIndex[0] = _triangleVertexArray.getIndices()[iii * 3]; + vertexIndex[1] = _triangleVertexArray.getIndices()[iii * 3 + 1]; + vertexIndex[2] = _triangleVertexArray.getIndices()[iii * 3 + 2]; + // Add information about the edges + addEdge(vertexIndex[0], vertexIndex[1]); + addEdge(vertexIndex[0], vertexIndex[2]); + addEdge(vertexIndex[1], vertexIndex[2]); + } + } + this.numberVertices = this.vertices.size(); + recalculateBounds(); + } + + /** + * Add an edge longo the convex mesh by specifying the two vertex indices of the edge. + * @note that the vertex indices start at zero and need to correspond to the order of + * the vertices in the vertices array in the ructor or the order of the calls + * of the addVertex methods that you use to add vertices longo the convex mesh. + * @param _v1 Index of the first vertex of the edge to add + * @param _v2 Index of the second vertex of the edge to add + */ + public void addEdge(final int _v1, final int _v2) { + // If the entry for vertex v1 does not exist in the adjacency list + if (!this.edgesAdjacencyList.containsKey(_v1)) { + this.edgesAdjacencyList.put(_v1, new SetInteger()); + } + // If the entry for vertex v2 does not exist in the adjacency list + if (!this.edgesAdjacencyList.containsKey(_v2)) { + this.edgesAdjacencyList.put(_v2, new SetInteger()); + } + // Add the edge in the adjacency list + this.edgesAdjacencyList.get(_v1).add(_v2); + this.edgesAdjacencyList.get(_v2).add(_v1); + } + + /** + * Add a vertex longo the convex mesh + * @param vertex Vertex to be added + */ + public void addVertex(final Vector3f _vertex) { + // Add the vertex in to vertices array + this.vertices.add(_vertex); + this.numberVertices++; + // Update the bounds of the mesh + if (_vertex.x * this.scaling.x > this.maxBounds.x) { + this.maxBounds.setX(_vertex.x * this.scaling.x); + } + if (_vertex.x * this.scaling.x < this.minBounds.x) { + this.minBounds.setX(_vertex.x * this.scaling.x); + } + if (_vertex.y * this.scaling.y > this.maxBounds.y) { + this.maxBounds.setY(_vertex.y * this.scaling.y); + } + if (_vertex.y * this.scaling.y < this.minBounds.y) { + this.minBounds.setY(_vertex.y * this.scaling.y); + } + if (_vertex.z * this.scaling.z > this.maxBounds.z) { + this.maxBounds.setZ(_vertex.z * this.scaling.z); + } + if (_vertex.z * this.scaling.z < this.minBounds.z) { + this.minBounds.setZ(_vertex.z * this.scaling.z); + } + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + final float factor = (1.0f / 3.0f) * _mass; + final Vector3f realExtent = this.maxBounds.lessNew(this.minBounds).multiply(0.5f); + assert (realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0); + final float xSquare = realExtent.x * realExtent.x; + final float ySquare = realExtent.y * realExtent.y; + final float zSquare = realExtent.z * realExtent.z; + _tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare)); + } + + @Override + public void getLocalBounds(final Vector3f _min, final Vector3f _max) { + _min.set(this.minBounds); + _max.set(this.maxBounds); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + assert (this.numberVertices == this.vertices.size()); + assert (_cachedCollisionData != null); + // Allocate memory for the cached collision data if not allocated yet + if (_cachedCollisionData.data == null) { + // TODO the data is nort set outside ==> find how ... !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + _cachedCollisionData.data = 0; + } + // If the edges information is used to speed up the collision detection + if (this.isEdgesInformationUsed) { + assert (this.edgesAdjacencyList.size() == this.numberVertices); + int maxVertex = (Integer) (_cachedCollisionData.data); + float maxDotProduct = _direction.dot(this.vertices.get(maxVertex)); + boolean isOptimal; + // Perform hill-climbing (local search) + do { + isOptimal = true; + assert (this.edgesAdjacencyList.get(maxVertex).size() > 0); + // For all neighbors of the current vertex + for (final Integer it : this.edgesAdjacencyList.get(maxVertex).getRaw()) { + // Compute the dot product + final float dotProduct = _direction.dot(this.vertices.get(it)); + // If the current vertex is a better vertex (larger dot product) + if (dotProduct > maxDotProduct) { + maxVertex = it; + maxDotProduct = dotProduct; + isOptimal = false; + } + } + } while (!isOptimal); + // Cache the support vertex + _cachedCollisionData.data = maxVertex; + // Return the support vertex + return this.vertices.get(maxVertex).multiplyNew(this.scaling); + } else { + // If the edges information is not used + double maxDotProduct = Float.MIN_VALUE; + int indexMaxDotProduct = 0; + // For each vertex of the mesh + for (int i = 0; i < this.numberVertices; i++) { + // Compute the dot product of the current vertex + final double dotProduct = _direction.dot(this.vertices.get(i)); + // If the current dot product is larger than the maximum one + if (dotProduct > maxDotProduct) { + indexMaxDotProduct = i; + maxDotProduct = dotProduct; + } + } + assert (maxDotProduct >= 0.0f); + // Return the vertex with the largest dot product in the support direction + return this.vertices.get(indexMaxDotProduct).multiplyNew(this.scaling); + } + } + + /** + * Return true if the edges information is used to speed up the collision detection + * @return True if the edges information is used and false otherwise + */ + public boolean isEdgesInformationUsed() { + return this.isEdgesInformationUsed; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().raycast(_ray, _proxyShape, _raycastInfo); + } + + /// Recompute the bounds of the mesh + protected void recalculateBounds() { + // TODO : Only works if the local origin is inside the mesh + // => Make it more robust (init with first vertex of mesh instead) + this.minBounds.setZero(); + this.maxBounds.setZero(); + // For each vertex of the mesh + for (int i = 0; i < this.numberVertices; i++) { + if (this.vertices.get(i).x > this.maxBounds.x) { + this.maxBounds.setX(this.vertices.get(i).x); + } + if (this.vertices.get(i).x < this.minBounds.x) { + this.minBounds.setX(this.vertices.get(i).x); + } + if (this.vertices.get(i).y > this.maxBounds.y) { + this.maxBounds.setY(this.vertices.get(i).y); + } + if (this.vertices.get(i).y < this.minBounds.y) { + this.minBounds.setY(this.vertices.get(i).y); + } + if (this.vertices.get(i).z > this.maxBounds.z) { + this.maxBounds.setZ(this.vertices.get(i).z); + } + if (this.vertices.get(i).z < this.minBounds.z) { + this.minBounds.setZ(this.vertices.get(i).z); + } + } + // Apply the local scaling factor + this.maxBounds.multiply(this.scaling); + this.minBounds.multiply(this.scaling); + // Add the object margin to the bounds + this.maxBounds.add(this.margin); + this.minBounds.less(this.margin); + } + + /** + * Set the variable to know if the edges information is used to speed up the + * collision detection + * @param isEdgesUsed True if you want to use the edges information to speed up the collision detection with the convex mesh shape + */ + public void setIsEdgesInformationUsed(final boolean _isEdgesUsed) { + this.isEdgesInformationUsed = _isEdgesUsed; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + super.setLocalScaling(_scaling); + recalculateBounds(); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + // Use the GJK algorithm to test if the point is inside the convex mesh + return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().testPointInside(_localPoint, _proxyShape); + } +}; diff --git a/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java b/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java new file mode 100644 index 0000000..69cff0a --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java @@ -0,0 +1,56 @@ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.Vector3f; + +public abstract class ConvexShape extends CollisionShape { + protected float margin; //!< Margin used for the GJK collision detection algorithm + /// Constructor + + public ConvexShape(final CollisionShapeType _type, final float _margin) { + super(_type); + this.margin = _margin; + } + + // 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); + // Get the support point without margin + final Vector3f supportPoint = getLocalSupportPointWithoutMargin(_direction, _cachedCollisionData); + ////Log.error(" -> supportPoint = " + supportPoint); + ////Log.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.FLOAT_EPSILON=" + FMath.floatToString(Constant.FLOAT_EPSILON)); + if (_direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) { + unitVec = _direction.safeNormalizeNew(); + ////Log.error(" -> unitVec= " + unitVec); + } + supportPoint.add(unitVec.multiplyNew(this.margin)); + } + ////Log.error(" -> ==> supportPoint = " + supportPoint); + return supportPoint; + } + + /// Return a local support point in a given direction without the object margin + public abstract Vector3f getLocalSupportPointWithoutMargin(Vector3f _direction, CacheData _cachedCollisionData); + + /** + * @brief Get the current object margin + * @return The margin (in meters) around the collision shape + */ + public float getMargin() { + return this.margin; + } + + @Override + public boolean isConvex() { + return true; + } + + @Override + public abstract boolean testPointInside(Vector3f _worldPoint, ProxyShape _proxyShape); +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/CylinderShape.java b/src/org/atriasoft/ephysics/collision/shapes/CylinderShape.java new file mode 100644 index 0000000..20bbf04 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/CylinderShape.java @@ -0,0 +1,297 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a cylinder collision shape around the Y axis and centered at the origin. The cylinder is + * defined by its height and the radius of its base. The "transform" of the corresponding rigid body gives an + * orientation and a position to the cylinder. This collision shape uses an extra margin distance around it for + * collision detection purpose. The default margin is 4cm (if your units are meters, which is recommended). In case, you + * want to simulate small objects (smaller than the margin distance), you might want to reduce the margin by specifying + * your own margin distance using the "margin" parameter in the constructor of the cylinder shape. Otherwise, it is + * recommended to use the default margin distance by not using the "margin" parameter in the constructor. + * + * @author Jason Sorensen + */ +/** + * It represents a cylinder collision shape around the Y axis + * and centered at the origin. The cylinder is defined by its height + * and the radius of its base. The "transform" of the corresponding + * rigid body gives an orientation and a position to the cylinder. + * This collision shape uses an extra margin distance around it for collision + * detection purpose. The default margin is 4cm (if your units are meters, + * which is recommended). In case, you want to simulate small objects + * (smaller than the margin distance), you might want to reduce the margin by + * specifying your own margin distance using the "margin" parameter in the + * ructor of the cylinder shape. Otherwise, it is recommended to use the + * default margin distance by not using the "margin" parameter in the ructor. + */ +public class CylinderShape extends ConvexShape { + protected float radius; //!< Radius of the base + protected float halfHeight; //!< Half height of the cylinder + + /** + * Contructor + * @param radius Radius of the cylinder (in meters) + * @param height Height of the cylinder (in meters) + * @param margin Collision margin (in meters) around the collision shape + */ + public CylinderShape(final float _radius, final float _height) { + this(_radius, _height, Defaults.OBJECT_MARGIN); + } + + public CylinderShape(final float _radius, final float _height, final float _margin) { + super(CollisionShapeType.CYLINDER, _margin); + this.radius = _radius; + this.halfHeight = _height / 2.0f; + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + final float height = 2.0f * this.halfHeight; + final float diag = (1.0f / 12.0f) * _mass * (3.0f * this.radius * this.radius + height * height); + _tensor.set(diag, 0.0f, 0.0f, 0.0f, 0.5f * _mass * this.radius * this.radius, 0.0f, 0.0f, 0.0f, diag); + } + + /** + * Get the Shape height + * @return Height of the cylinder (in meters) + */ + public float getHeight() { + return this.halfHeight + this.halfHeight; + } + + @Override + public void getLocalBounds(final Vector3f min, final Vector3f max) { + // Maximum bounds + max.setX(this.radius + this.margin); + max.setY(this.halfHeight + this.margin); + max.setZ(this.radius + this.margin); + // Minimum bounds + min.setX(-max.x); + min.setY(-max.y); + min.setZ(-max.z); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + final Vector3f supportPoint = new Vector3f(0.0f, 0.0f, 0.0f); + final float uDotv = _direction.y; + final Vector3f w = new Vector3f(_direction.x, 0.0f, _direction.z); + final float lengthW = FMath.sqrt(_direction.x * _direction.x + _direction.z * _direction.z); + if (lengthW > Constant.FLOAT_EPSILON) { + if (uDotv < 0.0f) { + supportPoint.setY(-this.halfHeight); + } else { + supportPoint.setY(this.halfHeight); + } + supportPoint.add(w.multiply(this.radius / lengthW)); + } else if (uDotv < 0.0f) { + supportPoint.setY(-this.halfHeight); + } else { + supportPoint.setY(this.halfHeight); + } + return supportPoint; + } + + /** + * Get the Shape radius + * @return Radius of the cylinder (in meters) + */ + public float getRadius() { + return this.radius; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f n = _ray.point2.lessNew(_ray.point1); + final float epsilon = 0.01f; + final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f); + final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f); + final Vector3f d = q.lessNew(p); + final Vector3f m = _ray.point1.lessNew(p); + float t; + final float mDotD = m.dot(d); + final float nDotD = n.dot(d); + final float dDotD = d.dot(d); + // Test if the segment is outside the cylinder + if (mDotD < 0.0f && mDotD + nDotD < 0.0f) { + return false; + } + + if (mDotD > dDotD && mDotD + nDotD > dDotD) { + return false; + } + final float nDotN = n.dot(n); + final float mDotN = m.dot(n); + final float a = dDotD * nDotN - nDotD * nDotD; + final float k = m.dot(m) - this.radius * this.radius; + final float c = dDotD * k - mDotD * mDotD; + // If the ray is parallel to the cylinder axis + if (FMath.abs(a) < epsilon) { + // If the origin is outside the surface of the cylinder, we return no hit + if (c > 0.0f) { + return false; + } + // Here we know that the segment longersect an endcap of the cylinder + // If the ray longersects with the "p" endcap of the cylinder + if (mDotD < 0.0f) { + t = -mDotN / nDotN; + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + final Vector3f normalDirection = new Vector3f(0.0f, -1.0f, 0.0f); + _raycastInfo.worldNormal = normalDirection; + return true; + } + // If the ray longersects with the "q" endcap of the cylinder + if (mDotD > dDotD) { + t = (nDotD - mDotN) / nDotN; + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0); + return true; + } + // If the origin is inside the cylinder, we return no hit + return false; + } + final float b = dDotD * mDotN - nDotD * mDotD; + final float discriminant = b * b - a * c; + // If the discriminant is negative, no real roots and therfore, no hit + if (discriminant < 0.0f) { + return false; + } + // Compute the smallest root (first longersection along the ray) + final float t0 = t = (-b - FMath.sqrt(discriminant)) / a; + // If the longersection is outside the cylinder on "p" endcap side + final float value = mDotD + t * nDotD; + if (value < 0.0f) { + // If the ray is pointing away from the "p" endcap, we return no hit + if (nDotD <= 0.0f) { + return false; + } + // Compute the longersection against the "p" endcap (longersection agains whole plane) + t = -mDotD / nDotD; + // Keep the longersection if the it is inside the cylinder radius + if (k + t * (2.0f * mDotN + t) > 0.0f) { + return false; + } + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.worldNormal = new Vector3f(0, -1.0f, 0); + return true; + } + // If the longersection is outside the cylinder on the "q" side + if (value > dDotD) { + // If the ray is pointing away from the "q" endcap, we return no hit + if (nDotD >= 0.0f) { + return false; + } + // Compute the longersection against the "q" endcap (longersection against whole plane) + t = (dDotD - mDotD) / nDotD; + // Keep the longersection if it is inside the cylinder radius + if (k + dDotD - 2.0f * mDotD + t * (2.0f * (mDotN - nDotD) + t) > 0.0f) { + return false; + } + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0); + return true; + } + t = t0; + // If the longersection is behind the origin of the ray or beyond the maximum + // raycasting distance, we return no hit + if (t < 0.0f || t > _ray.maxFraction) { + return false; + } + // Compute the hit information + final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1); + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = localHitPoint; + final Vector3f v = localHitPoint.lessNew(p); + final Vector3f w = d.multiplyNew(v.dot(d) / d.length2()); + final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w)); + _raycastInfo.worldNormal = normalDirection; + return true; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y; + this.radius = (this.radius / this.scaling.x) * _scaling.x; + super.setLocalScaling(_scaling); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + return ((_localPoint.x * _localPoint.x + _localPoint.z * _localPoint.z) < this.radius * this.radius && _localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight); + } +} \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/shapes/HeightFieldShape.java b/src/org/atriasoft/ephysics/collision/shapes/HeightFieldShape.java new file mode 100644 index 0000000..290d931 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/HeightFieldShape.java @@ -0,0 +1,305 @@ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.mathematics.Ray; + +/** + * This class represents a static height field that can be used to represent + * a terrain. The height field is made of a grid with rows and columns with a + * height value at each grid point. Note that the height values are not copied longo the shape + * but are shared instead. The height values can be of type longeger, float or double. + * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of + * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means + * that for instance, if the minimum height value is -200 and the maximum value is 400, the final + * minimum height of the field in the simulation will be -300 and the maximum height will be 300. + */ +public class HeightFieldShape extends ConcaveShape { + + /** + * This class is used for testing AABB and triangle overlap for raycasting + */ + public class TriangleOverlapCallback implements TriangleCallback { + protected Ray ray; + protected ProxyShape proxyShape; + protected RaycastInfo raycastInfo; + protected boolean isHit; + protected float smallestHitFraction; + protected HeightFieldShape heightFieldShape; + + public TriangleOverlapCallback(final Ray _ray, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final HeightFieldShape _heightFieldShape) { + this.ray = _ray; + this.proxyShape = _proxyShape; + this.raycastInfo = _raycastInfo; + this.heightFieldShape = _heightFieldShape; + this.isHit = false; + this.smallestHitFraction = this.ray.maxFraction; + } + + public boolean getIsHit() { + return this.isHit; + } + + /// Raycast test between a ray and a triangle of the heightfield + @Override + public void testTriangle(final Vector3f[] _trianglePoints) { + // Create a triangle collision shape + final float margin = this.heightFieldShape.getTriangleMargin(); + final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin); + triangleShape.setRaycastTestType(this.heightFieldShape.getRaycastTestType()); + // Ray casting test against the collision shape + final RaycastInfo raycastInfo = new RaycastInfo(); + final boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape); + // If the ray hit the collision shape + if (isTriangleHit && raycastInfo.hitFraction <= this.smallestHitFraction) { + assert (raycastInfo.hitFraction >= 0.0f); + this.raycastInfo.body = raycastInfo.body; + this.raycastInfo.proxyShape = raycastInfo.proxyShape; + this.raycastInfo.hitFraction = raycastInfo.hitFraction; + this.raycastInfo.worldPoint = raycastInfo.worldPoint; + this.raycastInfo.worldNormal = raycastInfo.worldNormal; + this.raycastInfo.meshSubpart = -1; + this.raycastInfo.triangleIndex = -1; + this.smallestHitFraction = raycastInfo.hitFraction; + this.isHit = true; + } + } + }; + + protected int numberColumns; //!< Number of columns in the grid of the height field + protected int numberRows; //!< Number of rows in the grid of the height field + + protected float width; //!< Height field width + protected float length; //!< Height field length + protected float minHeight; //!< Minimum height of the height field + protected float maxHeight; //!< Maximum height of the height field + protected int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z) + protected float[] heightFieldData; //!< Array of data with all the height values of the height field + protected AABB AABB; //!< Local AABB of the height field (without scaling) + + /** + * Contructor + * @param nbGridColumns Number of columns in the grid of the height field + * @param nbGridRows Number of rows in the grid of the height field + * @param minHeight Minimum height value of the height field + * @param maxHeight Maximum height value of the height field + * @param heightFieldData Pointer to the first height value data (note that values are shared and not copied) + * @param dataType Data type for the height values (long, float, double) + * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z) + * @param longegerHeightScale Scaling factor used to scale the height values (only when height values type is longeger) + */ + public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData) { + this(_nbGridColumns, _nbGridRows, _minHeight, _maxHeight, _heightFieldData, 1); + } + + /// Insert all the triangles longo the dynamic AABB tree + //protected void initBVHTree(); + + public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData, final int _upAxis) { + super(CollisionShapeType.HEIGHTFIELD); + this.numberColumns = _nbGridColumns; + this.numberRows = _nbGridRows; + this.width = _nbGridColumns - 1; + this.length = _nbGridRows - 1; + this.minHeight = _minHeight; + this.maxHeight = _maxHeight; + this.upAxis = _upAxis; + assert (_nbGridColumns >= 2); + assert (_nbGridRows >= 2); + assert (this.width >= 1); + assert (this.length >= 1); + assert (_minHeight <= _maxHeight); + assert (_upAxis == 0 || _upAxis == 1 || _upAxis == 2); + this.heightFieldData = _heightFieldData; + final float halfHeight = (this.maxHeight - this.minHeight) * 0.5f; + assert (halfHeight >= 0); + // Compute the local AABB of the height field + if (this.upAxis == 0) { + this.AABB.setMin(new Vector3f(-halfHeight, -this.width * 0.5f, -this.length * 0.5f)); + this.AABB.setMax(new Vector3f(halfHeight, this.width * 0.5f, this.length * 0.5f)); + } else if (this.upAxis == 1) { + this.AABB.setMin(new Vector3f(-this.width * 0.5f, -halfHeight, -this.length * 0.5f)); + this.AABB.setMax(new Vector3f(this.width * 0.5f, halfHeight, this.length * 0.5f)); + } else if (this.upAxis == 2) { + this.AABB.setMin(new Vector3f(-this.width * 0.5f, -this.length * 0.5f, -halfHeight)); + this.AABB.setMax(new Vector3f(this.width * 0.5f, this.length * 0.5f, halfHeight)); + } + } + + /// Return the closest inside longeger grid value of a given floating grid value + protected int computeIntegerGridValue(final float _value) { + return (int) ((_value < 0.0f) ? _value - 0.5f : _value + 0.5f); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + // Default inertia tensor + // Note that this is not very realistic for a concave triangle mesh. + // However, in most cases, it will only be used static bodies and therefore, + // the inertia tensor is not used. + _tensor.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass); + } + + /// Compute the min/max grid coords corresponding to the longersection of the AABB of the height field and the AABB to collide + protected void computeMinMaxGridCoordinates(final Vector3f _minCoords, final Vector3f _maxCoords, final AABB _aabbToCollide) { + // Clamp the min/max coords of the AABB to collide inside the height field AABB + Vector3f minPoint = FMath.max(_aabbToCollide.getMin(), this.AABB.getMin()); + minPoint = FMath.min(minPoint, this.AABB.getMax()); + Vector3f maxPoint = FMath.min(_aabbToCollide.getMax(), this.AABB.getMax()); + maxPoint = FMath.max(maxPoint, this.AABB.getMin()); + // Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] + // and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... this.width/2] + // and [-this.length/2 ... this.length/2] + final Vector3f translateVec = this.AABB.getExtent().multiplyNew(0.5f); + minPoint.add(translateVec); + maxPoint.add(translateVec); + // Convert the floating min/max coords of the AABB longo closest longeger + // grid values (note that we use the closest grid coordinate that is out + // of the AABB) + _minCoords.set(computeIntegerGridValue(minPoint.x) - 1, computeIntegerGridValue(minPoint.y) - 1, computeIntegerGridValue(minPoint.z) - 1); + _maxCoords.set(computeIntegerGridValue(maxPoint.x) + 1, computeIntegerGridValue(maxPoint.y) + 1, computeIntegerGridValue(maxPoint.z) + 1); + } + + /// Return the height of a given (x,y) point in the height field + protected float getHeightAt(final int _xxx, final int _yyy) { + return this.heightFieldData[_yyy * this.numberColumns + _xxx]; + } + + @Override + public void getLocalBounds(final Vector3f _min, final Vector3f _max) { + _min.set(this.AABB.getMin().multiplyNew(this.scaling)); + _max.set(this.AABB.getMax().multiplyNew(this.scaling)); + } + + /// Return the number of columns in the height field + public long getNbColumns() { + return this.numberColumns; + } + + /// Return the number of rows in the height field + public long getNbRows() { + return this.numberRows; + } + + /// 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 long _subPart, + final long _triangleIndex, + Vector3f* _outTriangleVertices) ; + */ + /// Return the vertex (local-coordinates) of the height field at a given (x,y) position + protected Vector3f getVertexAt(final int _xxx, final int _yyy) { + // Get the height value + final float height = getHeightAt(_xxx, _yyy); + // Height values origin + final float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight; + Vector3f vertex = null; + switch (this.upAxis) { + case 0: + vertex = new Vector3f(heightOrigin + height, -this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy); + break; + case 1: + vertex = new Vector3f(-this.width * 0.5f + _xxx, heightOrigin + height, -this.length * 0.5f + _yyy); + break; + case 2: + vertex = new Vector3f(-this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy, heightOrigin + height); + break; + default: + assert (false); + } + assert (this.AABB.contains(vertex)); + return vertex.multiply(this.scaling); + } + + @Override + public boolean isConvex() { + return false; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + // TODO : Implement raycasting without using an AABB for the ray + // but using a dynamic AABB tree or octree instead + + final TriangleOverlapCallback triangleCallback = new TriangleOverlapCallback(_ray, _proxyShape, _raycastInfo, this); + // Compute the AABB for the ray + final Vector3f rayEnd = _ray.point2.lessNew(_ray.point1).multiply(_ray.maxFraction).add(_ray.point1); + + final AABB rayAABB = new AABB(FMath.min(_ray.point1, rayEnd), FMath.max(_ray.point1, rayEnd)); + testAllTriangles(triangleCallback, rayAABB); + return triangleCallback.getIsHit(); + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + super.setLocalScaling(_scaling); + } + + @Override + public void testAllTriangles(final TriangleCallback _callback, final AABB _localAABB) { + + // Compute the non-scaled AABB + final Vector3f inverseScaling = new Vector3f(1.0f / this.scaling.x, 1.0f / this.scaling.y, 1.0f / this.scaling.z); + + final AABB aabb = new AABB(_localAABB.getMin().multiplyNew(inverseScaling), _localAABB.getMax().multiplyNew(inverseScaling)); + // Compute the longeger grid coordinates inside the area we need to test for collision + final Vector3f minGridCoords = new Vector3f(); + final Vector3f maxGridCoords = new Vector3f(); + computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb); + // Compute the starting and ending coords of the sub-grid according to the up axis + int iMin = 0; + int iMax = 0; + int jMin = 0; + int jMax = 0; + switch (this.upAxis) { + case 0: + iMin = FMath.clamp((int) minGridCoords.y, 0, this.numberColumns - 1); + iMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberColumns - 1); + jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1); + jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1); + break; + case 1: + iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1); + iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1); + jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1); + jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1); + break; + case 2: + iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1); + iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1); + jMin = FMath.clamp((int) minGridCoords.y, 0, this.numberRows - 1); + jMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberRows - 1); + break; + } + assert (iMin >= 0 && iMin < this.numberColumns); + assert (iMax >= 0 && iMax < this.numberColumns); + assert (jMin >= 0 && jMin < this.numberRows); + assert (jMax >= 0 && jMax < this.numberRows); + // For each sub-grid points (except the last ones one each dimension) + for (int i = iMin; i < iMax; i++) { + for (int j = jMin; j < jMax; j++) { + // Compute the four point of the current quad + final Vector3f p1 = getVertexAt(i, j); + final Vector3f p2 = getVertexAt(i, j + 1); + final Vector3f p3 = getVertexAt(i + 1, j); + final Vector3f p4 = getVertexAt(i + 1, j + 1); + // Generate the first triangle for the current grid rectangle + final Vector3f[] trianglePoints = { p1, p2, p3 }; + // Test collision against the first triangle + _callback.testTriangle(trianglePoints); + // Generate the second triangle for the current grid rectangle + trianglePoints[0] = p3; + trianglePoints[1] = p2; + trianglePoints[2] = p4; + // Test collision against the second triangle + _callback.testTriangle(trianglePoints); + } + } + } + +} diff --git a/src/org/atriasoft/ephysics/collision/shapes/SphereShape.java b/src/org/atriasoft/ephysics/collision/shapes/SphereShape.java new file mode 100644 index 0000000..c9a27a9 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/SphereShape.java @@ -0,0 +1,142 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +/** + * Represents a sphere collision shape that is centered + * at the origin and defined by its radius. This collision shape does not + * have an explicit object margin distance. The margin is implicitly the + * radius of the sphere. Therefore, no need to specify an object margin + * for a sphere shape. + */ +public class SphereShape extends ConvexShape { + /** + * Constructor + * @param radius Radius of the sphere (in meters) + */ + public SphereShape(final float _radius) { + super(CollisionShapeType.SPHERE, _radius); + } + + @Override + public void computeAABB(final AABB _aabb, final Transform3D _transform) { + + // Get the local extents in x,y and z direction + final Vector3f extents = new Vector3f(this.margin, this.margin, this.margin); + // Update the AABB with the new minimum and maximum coordinates + _aabb.setMin(_transform.getPosition().lessNew(extents)); + _aabb.setMax(_transform.getPosition().addNew(extents)); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + final float diag = 0.4f * _mass * this.margin * this.margin; + _tensor.set(diag, 0.0f, 0.0f, 0.0f, diag, 0.0f, 0.0f, 0.0f, diag); + } + + @Override + public void getLocalBounds(final Vector3f min, final Vector3f max) { + // Maximum bounds + max.setX(this.margin); + max.setY(this.margin); + max.setZ(this.margin); + // Minimum bounds + min.setX(-max.x); + min.setY(-max.y); + min.setZ(-max.z); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + return new Vector3f(0.0f, 0.0f, 0.0f); + } + + /** + * Get the radius of the sphere + * @return Radius of the sphere (in meters) + */ + public float getRadius() { + return this.margin; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f m = _ray.point1; + final float c = m.dot(m) - this.margin * this.margin; + // If the origin of the ray is inside the sphere, we return no longersection + if (c < 0.0f) { + return false; + } + final Vector3f rayDirection = _ray.point2.lessNew(_ray.point1); + final float b = m.dot(rayDirection); + // If the origin of the ray is outside the sphere and the ray + // is pointing away from the sphere, there is no longersection + if (b > 0.0f) { + return false; + } + final float raySquareLength = rayDirection.length2(); + // Compute the discriminant of the quadratic equation + final float discriminant = b * b - raySquareLength * c; + // If the discriminant is negative or the ray length is very small, there is no longersection + if (discriminant < 0.0f || raySquareLength < Constant.FLOAT_EPSILON) { + return false; + } + // Compute the solution "t" closest to the origin + float t = -b - FMath.sqrt(discriminant); + assert (t >= 0.0f); + // If the hit point is withing the segment ray fraction + if (t < _ray.maxFraction * raySquareLength) { + // Compute the longersection information + t /= raySquareLength; + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.hitFraction = t; + _raycastInfo.worldPoint = rayDirection.multiplyNew(t).add(_ray.point1); + _raycastInfo.worldNormal = _raycastInfo.worldPoint; + return true; + } + return false; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.margin = (this.margin / this.scaling.x) * _scaling.x; + super.setLocalScaling(_scaling); + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + return (_localPoint.length2() < this.margin * this.margin); + } +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/collision/shapes/TriangleShape.java b/src/org/atriasoft/ephysics/collision/shapes/TriangleShape.java new file mode 100644 index 0000000..15aa904 --- /dev/null +++ b/src/org/atriasoft/ephysics/collision/shapes/TriangleShape.java @@ -0,0 +1,195 @@ +package org.atriasoft.ephysics.collision.shapes; + +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.RaycastInfo; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.configuration.Defaults; +import org.atriasoft.ephysics.mathematics.Ray; + +/** + * This class represents a triangle collision shape that is centered + * at the origin and defined three points. + */ +public class TriangleShape extends ConvexShape { + /** + * Raycast test side for the triangle + */ + public enum TriangleRaycastSide { + FRONT, //!< Raycast against front triangle + BACK, //!< Raycast against back triangle + FRONT_AND_BACK //!< Raycast against front and back triangle + }; + + protected Vector3f[] points = new Vector3f[3]; //!< Three points of the triangle + protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) + + public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3) { + super(CollisionShapeType.TRIANGLE, Defaults.OBJECT_MARGIN); + this.points[0] = _point1; + this.points[1] = _point2; + this.points[2] = _point3; + this.raycastTestType = TriangleRaycastSide.FRONT; + } + + /** + * Constructor + * @param _point1 First point of the triangle + * @param _point2 Second point of the triangle + * @param _point3 Third point of the triangle + * @param _margin The collision margin (in meters) around the collision shape + */ + public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3, final float _margin) { + super(CollisionShapeType.TRIANGLE, _margin); + this.points[0] = _point1; + this.points[1] = _point2; + this.points[2] = _point3; + this.raycastTestType = TriangleRaycastSide.FRONT; + } + + @Override + public void computeAABB(final AABB aabb, final Transform3D _transform) { + final Vector3f worldPoint1 = _transform.multiplyNew(this.points[0]); + final Vector3f worldPoint2 = _transform.multiplyNew(this.points[1]); + final Vector3f worldPoint3 = _transform.multiplyNew(this.points[2]); + final Vector3f xAxis = new Vector3f(worldPoint1.x, worldPoint2.x, worldPoint3.x); + final Vector3f yAxis = new Vector3f(worldPoint1.y, worldPoint2.y, worldPoint3.y); + final Vector3f zAxis = new Vector3f(worldPoint1.z, worldPoint2.z, worldPoint3.z); + aabb.setMin(new Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); + aabb.setMax(new Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); + } + + @Override + public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) { + _tensor.setZero(); + } + + @Override + public void getLocalBounds(final Vector3f min, final Vector3f max) { + final Vector3f xAxis = new Vector3f(this.points[0].x, this.points[1].x, this.points[2].x); + final Vector3f yAxis = new Vector3f(this.points[0].y, this.points[1].y, this.points[2].y); + final Vector3f zAxis = new Vector3f(this.points[0].z, this.points[1].z, this.points[2].z); + min.setValue(xAxis.getMin() - this.margin, yAxis.getMin() - this.margin, zAxis.getMin() - this.margin); + max.setValue(xAxis.getMax() + this.margin, yAxis.getMax() + this.margin, zAxis.getMax() + this.margin); + } + + @Override + public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) { + final Vector3f dotProducts = new Vector3f(_direction.dot(this.points[0]), _direction.dot(this.points[1]), _direction.dot(this.points[2])); + return this.points[dotProducts.getMaxAxis()]; + } + + /// Return the raycast test type (front, back, front-back) + public TriangleRaycastSide getRaycastTestType() { + return this.raycastTestType; + } + + /** + * Return the coordinates of a given vertex of the triangle + * @param _index Index (0 to 2) of a vertex of the triangle + */ + public Vector3f getVertex(final int _index) { + assert (_index >= 0 && _index < 3); + return this.points[_index]; + } + + @Override + public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) { + final Vector3f pq = _ray.point2.lessNew(_ray.point1); + final Vector3f pa = this.points[0].lessNew(_ray.point1); + final Vector3f pb = this.points[1].lessNew(_ray.point1); + final Vector3f pc = this.points[2].lessNew(_ray.point1); + // Test if the line PQ is inside the eges BC, CA and AB. We use the triple + // product for this test. + final Vector3f m = pq.cross(pc); + float u = pb.dot(m); + if (this.raycastTestType == TriangleRaycastSide.FRONT) { + if (u < 0.0f) { + return false; + } + } else if (this.raycastTestType == TriangleRaycastSide.BACK) { + if (u > 0.0f) { + return false; + } + } + float v = -pa.dot(m); + if (this.raycastTestType == TriangleRaycastSide.FRONT) { + if (v < 0.0f) { + return false; + } + } else if (this.raycastTestType == TriangleRaycastSide.BACK) { + if (v > 0.0f) { + return false; + } + } else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) { + if (!FMath.sameSign(u, v)) { + return false; + } + } + float w = pa.dot(pq.cross(pb)); + if (this.raycastTestType == TriangleRaycastSide.FRONT) { + if (w < 0.0f) { + return false; + } + } else if (this.raycastTestType == TriangleRaycastSide.BACK) { + if (w > 0.0f) { + return false; + } + } else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) { + if (!FMath.sameSign(u, w)) { + return false; + } + } + // If the line PQ is in the triangle plane (case where u=v=w=0) + if (FMath.approxEqual(u, 0.0f) && FMath.approxEqual(v, 0) && FMath.approxEqual(w, 0)) { + return false; + } + // Compute the barycentric coordinates (u, v, w) to determine the + // longersection point R, R = u * a + v * b + w * c + final float denom = 1.0f / (u + v + w); + u *= denom; + v *= denom; + w *= denom; + // Compute the local hit point using the barycentric coordinates + final Vector3f localHitPoint = this.points[0].multiplyNew(u).add(this.points[1].multiplyNew(v)).add(this.points[2].multiplyNew(w)); + final float hitFraction = localHitPoint.lessNew(_ray.point1).length() / pq.length(); + if (hitFraction < 0.0f || hitFraction > _ray.maxFraction) { + return false; + } + final Vector3f localHitNormal = this.points[1].lessNew(this.points[0]).cross(this.points[2].lessNew(this.points[0])); + if (localHitNormal.dot(pq) > 0.0f) { + localHitNormal.less(localHitNormal); + } + _raycastInfo.body = _proxyShape.getBody(); + _raycastInfo.proxyShape = _proxyShape; + _raycastInfo.worldPoint = localHitPoint; + _raycastInfo.hitFraction = hitFraction; + _raycastInfo.worldNormal = localHitNormal; + return true; + } + + @Override + public void setLocalScaling(final Vector3f _scaling) { + this.points[0].divide(this.scaling).multiply(_scaling); + this.points[1].divide(this.scaling).multiply(_scaling); + this.points[2].divide(this.scaling).multiply(_scaling); + super.setLocalScaling(_scaling); + } + + /** + * Set the raycast test type (front, back, front-back) + * @param _testType Raycast test type for the triangle (front, back, front-back) + */ + public void setRaycastTestType(final TriangleRaycastSide _testType) { + this.raycastTestType = _testType; + } + + @Override + public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) { + return false; + } + +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/configuration/Defaults.java b/src/org/atriasoft/ephysics/configuration/Defaults.java new file mode 100644 index 0000000..d4c4ed4 --- /dev/null +++ b/src/org/atriasoft/ephysics/configuration/Defaults.java @@ -0,0 +1,86 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.configuration; + +import org.atriasoft.etk.math.Constant; + +/** + * + * @author Jason Sorensen + */ +public class Defaults { + + // Smallest decimal value (negative) + public static final float DECIMAL_SMALLEST = Float.MIN_VALUE; + + // Maximum decimal value + public static final float DECIMAL_LARGEST = Float.MAX_VALUE; + + // Default internal constant timestep in seconds + public static final float DEFAULT_TIMESTEP = 1.0f / 60.0f; + + // Default friction coefficient for a rigid body + public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f; + + // Default bounciness factor for a rigid body + public static final float DEFAULT_BOUNCINESS = 0.5f; + + // True if the spleeping technique is enabled + public static final boolean SPLEEPING_ENABLED = true; + + // Object margin for collision detection in meters (for the GJK-EPA Algorithm) + public static final float OBJECT_MARGIN = 0.04f; + + // Distance threshold for two contact points for a valid persistent contact (in meters) + public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f; + + // Velocity threshold for contact velocity restitution + public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f; + + // Number of iterations when solving the velocity constraints of the Sequential Impulse technique + public static final int DEFAULT_VELOCITY_SOLVER_NUM_ITERATIONS = 10; + + // Number of iterations when solving the position constraints of the Sequential Impulse technique + public static final int DEFAULT_POSITION_SOLVER_NUM_ITERATIONS = 5; + + // Time (in seconds) that a body must stay still to be considered sleeping + public static final float DEFAULT_TIME_BEFORE_SLEEP = 1.0f; + + // A body with a linear velocity smaller than the sleep linear velocity (in m/s) + // might enter sleeping mode. + public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.02f; + + // A body with angular velocity smaller than the sleep angular velocity (in rad/s) + // might enter sleeping mode + public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 3.0f * (Constant.PI / 180.0f); + + /// Maximum number of contact manifolds in an overlapping pair that involves two + /// convex collision shapes. + public static final int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; + + /// Maximum number of contact manifolds in an overlapping pair that involves at + /// least one concave collision shape. + public static final int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +} diff --git a/src/org/atriasoft/ephysics/constraint/BallAndSocketJoint.java b/src/org/atriasoft/ephysics/constraint/BallAndSocketJoint.java new file mode 100644 index 0000000..51d5eeb --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/BallAndSocketJoint.java @@ -0,0 +1,215 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.engine.ConstraintSolverData; + +public class BallAndSocketJoint extends Joint { + + private static float BETA = 0.2f; //!< Beta value for the bias factor of position correction + private Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1) + private Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2) + private Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space + private Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space + private Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates) + private Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates) + private Vector3f biasVector = new Vector3f(0, 0, 0); //!< Bias vector for the raint + private Matrix3f inverseMassMatrix = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the raint + private final Vector3f impulse = new Vector3f(0, 0, 0); //!< Accumulated impulse + /// Constructor + + public BallAndSocketJoint(final BallAndSocketJointInfo jointInfo) { + super(jointInfo); + // Compute the local-space anchor point for each body + this.localAnchorPointBody1 = this.body1.getTransform().inverseNew().multiply(jointInfo.anchorPointWorldSpace); + this.localAnchorPointBody2 = this.body2.getTransform().inverseNew().multiply(jointInfo.anchorPointWorldSpace); + } + + @Override + public void initBeforeSolve(final ConstraintSolverData raintSolverData) { + + // Initialize the bodies index in the velocity array + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1); + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2); + + // Get the bodies center of mass and orientations + final Vector3f x1 = this.body1.centerOfMassWorld; + final Vector3f x2 = this.body2.centerOfMassWorld; + final Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + final Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); + + // Get the inertia tensor of bodies + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = orientationBody1.multiply(this.localAnchorPointBody1); + this.r2World = orientationBody2.multiply(this.localAnchorPointBody2); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // Compute the matrix K=JM^-1J^t (3x3 matrix) + final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew())); + + // Compute the inverse mass matrix K^-1 + this.inverseMassMatrix.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrix = massMatrix.inverseNew(); + } + + // Compute the bias "b" of the raint + this.biasVector.setZero(); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + final float biasFactor = (BETA / raintSolverData.timeStep); + this.biasVector = (x2.addNew(this.r2World).less(x1).less(this.r1World)).multiply(biasFactor); + } + + // If warm-starting is not enabled + if (!raintSolverData.isWarmStartingActive) { + + // Reset the accumulated impulse + this.impulse.setZero(); + } + } + + @Override + public void solvePositionConstraint(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Compute J*v + final Vector3f Jv = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World)); + + // Compute the Lagrange multiplier lambda + final Vector3f deltaLambda = this.inverseMassMatrix.multiplyNew(Jv.multiplyNew(-1).less(this.biasVector)); + this.impulse.add(deltaLambda); + + // Compute the impulse P=J^T * lambda for the body 1 + final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1); + final Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the body 2 + final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiplyNew(-1); + + // Apply the impulse to the body 2 + v2.add(deltaLambda.multiplyNew(this.body2.massInverse)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + } + + @Override + public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) { + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (this.positionCorrectionTechnique != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) { + return; + } + + // Get the bodies center of mass and orientations + final Vector3f x1 = raintSolverData.positions[this.indexBody1]; + final Vector3f x2 = raintSolverData.positions[this.indexBody2]; + final Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + final Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Recompute the inverse inertia tensors + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = q1.multiply(this.localAnchorPointBody1); + this.r2World = q2.multiply(this.localAnchorPointBody2); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints + final float inverseMassBodies = inverseMassBody1 + inverseMassBody2; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiplyNew(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiplyNew(skewSymmetricMatrixU2.transposeNew())); + this.inverseMassMatrix.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrix = massMatrix.inverseNew(); + } + + // Compute the raint error (value of the C(x) function) + final Vector3f raintError = x2.addNew(this.r2World).less(x1).less(this.r1World); + + // Compute the Lagrange multiplier lambda + // TODO : Do not solve the system by computing the inverse each time and multiplying with the + // right-hand side vector but instead use a method to directly solve the linear system. + final Vector3f lambda = this.inverseMassMatrix.multiplyNew(raintError.multiplyNew(-1)); + + // Compute the impulse of body 1 + final Vector3f linearImpulseBody1 = lambda.multiplyNew(-1); + final Vector3f angularImpulseBody1 = lambda.cross(this.r1World); + + // Compute the pseudo velocity of body 1 + final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1); + final Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body center of mass and orientation of body 1 + x1.add(v1); + q1.add((new Quaternion(0, w1)).multiplyNew(q1).multiplyNew(0.5f)); + q1.normalize(); + + // Compute the impulse of body 2 + final Vector3f angularImpulseBody2 = lambda.cross(this.r2World).multiplyNew(-1); + + // Compute the pseudo velocity of body 2 + final Vector3f v2 = lambda.multiplyNew(inverseMassBody2); + final Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2.add(v2); + q2.add((new Quaternion(0, w2)).multiplyNew(q2).multiply(0.5f)); + q2.normalize(); + } + + @Override + public void warmstart(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Compute the impulse P=J^T * lambda for the body 1 + final Vector3f linearImpulseBody1 = this.impulse.multiplyNew(-1); + final Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the body 2 + final Vector3f angularImpulseBody2 = this.impulse.cross(this.r2World).multiplyNew(-1); + + // Apply the impulse to the body to the body 2 + v2.add(this.impulse.multiplyNew(this.body2.massInverse)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + } + +} diff --git a/src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java b/src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java new file mode 100644 index 0000000..e17e4a2 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java @@ -0,0 +1,26 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * It is used to gather the information needed to create a ball-and-socket + * joint. This structure will be used to create the actual ball-and-socket joint. + */ +public class BallAndSocketJointInfo extends JointInfo { + + public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + + /** + * Constructor + * @param _rigidBody1 Pointer to the first body of the joint + * @param _rigidBody2 Pointer to the second body of the joint + * @param _initAnchorPointWorldSpace The anchor point in world-space coordinates + */ + public BallAndSocketJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) { + super(_rigidBody1, _rigidBody2, JointType.BALLSOCKETJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + + } +} diff --git a/src/org/atriasoft/ephysics/constraint/ContactPoint.java b/src/org/atriasoft/ephysics/constraint/ContactPoint.java new file mode 100644 index 0000000..204f7eb --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/ContactPoint.java @@ -0,0 +1,174 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.CollisionBody; + +/** + * This class represents a collision contact point between two + * bodies in the physics engine. + */ +public class ContactPoint { + + private final CollisionBody body1; //!< First rigid body of the contact + private final CollisionBody body2; //!< Second rigid body of the contact + private final Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space + + private float penetrationDepth; //!< Penetration depth + private final Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1 + private final Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2 + private Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space + private Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space + private boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step) + private Vector3f frictionVectors_1; //!< Two orthogonal vectors that span the tangential friction plane + private Vector3f frictionVectors_2; //!< Two orthogonal vectors that span the tangential friction plane + private float penetrationImpulse; //!< Cached penetration impulse + private float frictionImpulse1; //!< Cached first friction impulse + private float frictionImpulse2; //!< Cached second friction impulse + private Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse + /// Constructor + + public ContactPoint(final ContactPointInfo contactInfo) { + this.body1 = contactInfo.shape1.getBody(); + this.body2 = contactInfo.shape2.getBody(); + this.normal = contactInfo.normal.clone(); + this.penetrationDepth = contactInfo.penetrationDepth; + this.localPointOnBody1 = contactInfo.localPoint1.clone(); + this.localPointOnBody2 = contactInfo.localPoint2.clone(); + this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiplyNew(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1)); + this.worldPointOnBody2 = contactInfo.shape2.getBody().getTransform().multiplyNew(contactInfo.shape2.getLocalToBodyTransform().multiply(contactInfo.localPoint2)); + this.isRestingContact = false; + + this.frictionVectors_1 = new Vector3f(0, 0, 0); + this.frictionVectors_2 = new Vector3f(0, 0, 0); + + assert (this.penetrationDepth >= 0.0f); + + } + + /// Return the reference to the body 1 + public CollisionBody getBody1() { + return this.body1; + } + + /// Return the reference to the body 2 + public CollisionBody getBody2() { + return this.body2; + } + + /// Return the cached first friction impulse + public float getFrictionImpulse1() { + return this.frictionImpulse1; + } + + /// Return the cached second friction impulse + public float getFrictionImpulse2() { + return this.frictionImpulse2; + } + + /// Get the second friction vector + public Vector3f getFrictionvec2() { + return this.frictionVectors_2; + } + + /// Get the first friction vector + public Vector3f getFrictionVector1() { + return this.frictionVectors_1; + } + + /// Return true if the contact is a resting contact + public boolean getIsRestingContact() { + return this.isRestingContact; + } + + /// Return the contact local point on body 1 + public Vector3f getLocalPointOnBody1() { + return this.localPointOnBody1; + } + + /// Return the contact local point on body 2 + public Vector3f getLocalPointOnBody2() { + return this.localPointOnBody2; + } + + /// Return the normal vector of the contact + public Vector3f getNormal() { + return this.normal; + } + + /// Return the penetration depth + public float getPenetrationDepth() { + return this.penetrationDepth; + } + + /// Return the cached penetration impulse + public float getPenetrationImpulse() { + return this.penetrationImpulse; + } + + /// Return the cached rolling resistance impulse + public Vector3f getRollingResistanceImpulse() { + return this.rollingResistanceImpulse; + } + + /// Return the contact world point on body 1 + public Vector3f getWorldPointOnBody1() { + return this.worldPointOnBody1; + } + + /// Return the contact world point on body 2 + public Vector3f getWorldPointOnBody2() { + return this.worldPointOnBody2; + } + + /// Set the first cached friction impulse + public void setFrictionImpulse1(final float impulse) { + this.frictionImpulse1 = impulse; + } + + /// Set the second cached friction impulse + public void setFrictionImpulse2(final float impulse) { + this.frictionImpulse2 = impulse; + } + + /// Set the second friction vector + public void setFrictionvec2(final Vector3f frictionvec2) { + this.frictionVectors_2 = frictionvec2; + } + + /// Set the first friction vector + public void setFrictionVector1(final Vector3f frictionVector1) { + this.frictionVectors_1 = frictionVector1; + } + + /// Set the this.isRestingContact variable + public void setIsRestingContact(final boolean isRestingContact) { + this.isRestingContact = isRestingContact; + } + + /// Set the penetration depth of the contact + public void setPenetrationDepth(final float penetrationDepth) { + this.penetrationDepth = penetrationDepth; + } + + /// Set the cached penetration impulse + public void setPenetrationImpulse(final float impulse) { + this.penetrationImpulse = impulse; + } + + /// Set the cached rolling resistance impulse + public void setRollingResistanceImpulse(final Vector3f impulse) { + this.rollingResistanceImpulse = impulse; + } + + /// Set the contact world point on body 1 + public void setWorldPointOnBody1(final Vector3f worldPoint) { + this.worldPointOnBody1 = worldPoint; + } + + /// Set the contact world point on body 2 + public void setWorldPointOnBody2(final Vector3f worldPoint) { + this.worldPointOnBody2 = worldPoint; + } + +} diff --git a/src/org/atriasoft/ephysics/constraint/ContactPointInfo.java b/src/org/atriasoft/ephysics/constraint/ContactPointInfo.java new file mode 100644 index 0000000..eac4710 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/ContactPointInfo.java @@ -0,0 +1,54 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.shapes.CollisionShape; + +/** + * This structure contains informations about a collision contact + * computed during the narrow-phase collision detection. Those + * informations are used to compute the contact set for a contact + * between two bodies. + */ +public class ContactPointInfo { + public ProxyShape shape1; //!< First proxy shape of the contact + public ProxyShape shape2; //!< Second proxy shape of the contact + public CollisionShape collisionShape1; //!< First collision shape + public CollisionShape collisionShape2; //!< Second collision shape + public Vector3f normal; //!< Normalized normal vector of the collision contact in world space + public float penetrationDepth; //!< Penetration depth of the contact + public Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1 + public Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2 + + public ContactPointInfo() { + this.shape1 = null; + this.shape2 = null; + this.collisionShape1 = null; + this.collisionShape2 = null; + } + + public ContactPointInfo(final ContactPointInfo obj) { + this.shape1 = obj.shape1; + this.shape2 = obj.shape2; + this.collisionShape1 = obj.collisionShape1; + this.collisionShape2 = obj.collisionShape2; + this.normal = obj.normal; + this.penetrationDepth = obj.penetrationDepth; + this.localPoint1 = obj.localPoint1; + this.localPoint2 = obj.localPoint2; + } + + public ContactPointInfo(final ProxyShape _proxyShape1, final ProxyShape _proxyShape2, final CollisionShape _collShape1, final CollisionShape _collShape2, final Vector3f _normal, + final float _penetrationDepth, final Vector3f _localPoint1, final Vector3f _localPoint2) { + this.shape1 = _proxyShape1; + this.shape2 = _proxyShape2; + this.collisionShape1 = _collShape1; + this.collisionShape2 = _collShape2; + this.normal = _normal; + this.penetrationDepth = _penetrationDepth; + this.localPoint1 = _localPoint1; + this.localPoint2 = _localPoint2; + + } +}; \ No newline at end of file diff --git a/src/org/atriasoft/ephysics/constraint/ContactsPositionCorrectionTechnique.java b/src/org/atriasoft/ephysics/constraint/ContactsPositionCorrectionTechnique.java new file mode 100644 index 0000000..4e7775a --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/ContactsPositionCorrectionTechnique.java @@ -0,0 +1,12 @@ +package org.atriasoft.ephysics.constraint; + +/// Position correction technique used in the contact solver (for contacts) +/// BAUMGARTE_CONTACTS : Faster but can be innacurate and can lead to unexpected bounciness +/// in some situations (due to error correction factor being added to +/// the bodies momentum). +/// SPLIT_IMPULSES : A bit slower but the error correction factor is not added to the +/// bodies momentum. This is the option used by default. +public enum ContactsPositionCorrectionTechnique { + BAUMGARTE_CONTACTS, + SPLIT_IMPULSES +} diff --git a/src/org/atriasoft/ephysics/constraint/FixedJoint.java b/src/org/atriasoft/ephysics/constraint/FixedJoint.java new file mode 100644 index 0000000..e94b76c --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/FixedJoint.java @@ -0,0 +1,313 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.engine.ConstraintSolverData; + +public class FixedJoint extends Joint { + + protected static float BETA = 0.2f; //!< Beta value for the bias factor of position correction + protected Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1) + protected Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2) + protected Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space + protected Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space + protected Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates) + protected Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates) + protected Vector3f impulseTranslation = new Vector3f(0, 0, 0); //!< Accumulated impulse for the 3 translation raints + protected Vector3f impulseRotation = new Vector3f(0, 0, 0); //!< Accumulate impulse for the 3 rotation raints + protected Matrix3f inverseMassMatrixTranslation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix) + protected Matrix3f inverseMassMatrixRotation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix) + protected Vector3f biasTranslation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 translation raints + protected Vector3f biasRotation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 rotation raints + protected Quaternion initOrientationDifferenceInv = new Quaternion(); //!< Inverse of the initial orientation difference between the two bodies + + /// Constructor + public FixedJoint(final FixedJointInfo jointInfo) { + super(jointInfo); + // Compute the local-space anchor point for each body + final Transform3D transform1 = this.body1.getTransform(); + final Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + + // Compute the inverse of the initial orientation difference between the two bodies + this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew()); + this.initOrientationDifferenceInv.normalize(); + this.initOrientationDifferenceInv.inverse(); + } + + @Override + public void initBeforeSolve(final ConstraintSolverData raintSolverData) { + + // Initialize the bodies index in the velocity array + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1); + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2); + + // Get the bodies positions and orientations + final Vector3f x1 = this.body1.centerOfMassWorld; + final Vector3f x2 = this.body2.centerOfMassWorld; + final Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + final Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); + + // Get the inertia tensor of bodies + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = orientationBody1.multiply(this.localAnchorPointBody1); + this.r2World = orientationBody2.multiply(this.localAnchorPointBody2); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints + final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew())); + + // Compute the inverse mass matrix K^-1 for the 3 translation raints + this.inverseMassMatrixTranslation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslation = massMatrix.inverseNew(); + } + + // Compute the bias "b" of the raint for the 3 translation raints + final float biasFactor = (BETA / raintSolverData.timeStep); + this.biasTranslation.setZero(); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.biasTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor); + } + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + this.inverseMassMatrixRotation = this.i1.addNew(this.i2); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew(); + } + + // Compute the bias "b" for the 3 rotation raints + this.biasRotation.setZero(); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew()); + currentOrientationDifference.normalize(); + final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv); + this.biasRotation = qError.getVectorV().multiply(biasFactor * 2.0f); + } + + // If warm-starting is not enabled + if (!raintSolverData.isWarmStartingActive) { + // Reset the accumulated impulses + this.impulseTranslation.setZero(); + this.impulseRotation.setZero(); + } + } + + @Override + public void solvePositionConstraint(final ConstraintSolverData raintSolverData) { + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (this.positionCorrectionTechnique != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) { + return; + } + + // Get the bodies positions and orientations + final Vector3f x1 = raintSolverData.positions[this.indexBody1]; + final Vector3f x2 = raintSolverData.positions[this.indexBody2]; + final Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + final Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Recompute the inverse inertia tensors + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = q1.multiply(this.localAnchorPointBody1); + this.r2World = q2.multiply(this.localAnchorPointBody2); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // --------------- Translation Constraints --------------- // + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints + final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).add(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).add(skewSymmetricMatrixU2.transposeNew())); + this.inverseMassMatrixTranslation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslation = massMatrix.inverseNew(); + } + + // Compute position error for the 3 translation raints + final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World); + + // Compute the Lagrange multiplier lambda + final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1)); + + // Compute the impulse of body 1 + final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1); + Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World); + + // Compute the pseudo velocity of body 1 + final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1); + Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + x1.add(v1); + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse of body 2 + final Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiply(-1); + + // Compute the pseudo velocity of body 2 + final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2); + Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2.add(v2); + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + this.inverseMassMatrixRotation = this.i1.addNew(this.i2); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew(); + } + + // Compute the position error for the 3 rotation raints + final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew()); + currentOrientationDifference.normalize(); + final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv); + final Vector3f errorRotation = qError.getVectorV().multiply(2.0f); + + // Compute the Lagrange multiplier lambda for the 3 rotation raints + final Vector3f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 + angularImpulseBody1 = lambdaRotation.multiplyNew(-1); + + // Compute the pseudo velocity of body 1 + w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the pseudo velocity of body 2 + w2 = this.i2.multiplyNew(lambdaRotation); + + // Update the body position/orientation of body 2 + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + } + + @Override + public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // --------------- Translation Constraints --------------- // + + // Compute J*v for the 3 translation raints + final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World)); + + // Compute the Lagrange multiplier lambda + final Vector3f deltaLambda = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.biasTranslation)); + this.impulseTranslation.add(deltaLambda); + + // Compute the impulse P=J^T * lambda for body 1 + final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1); + Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for body 2 + final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiply(-1); + + // Apply the impulse to the body 2 + v2.add(deltaLambda.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation raints + final Vector3f JvRotation = w2.lessNew(w1); + + // Compute the Lagrange multiplier lambda for the 3 rotation raints + final Vector3f deltaLambda2 = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.biasRotation)); + this.impulseRotation.add(deltaLambda2); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 + angularImpulseBody1 = deltaLambda2.multiplyNew(-1); + + // Apply the impulse to the body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(deltaLambda2)); + } + + @Override + public void warmstart(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Compute the impulse P=J^T * lambda for the 3 translation raints for body 1 + final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1); + final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 + angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1)); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 3 translation raints for body 2 + final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2 + angularImpulseBody2.add(this.impulseRotation); + + // Apply the impulse to the body 2 + v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + } + +} diff --git a/src/org/atriasoft/ephysics/constraint/FixedJointInfo.java b/src/org/atriasoft/ephysics/constraint/FixedJointInfo.java new file mode 100644 index 0000000..6a89b1a --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/FixedJointInfo.java @@ -0,0 +1,25 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * This structure is used to gather the information needed to create a fixed + * joint. This structure will be used to create the actual fixed joint. + */ +public class FixedJointInfo extends JointInfo { + + Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + + /** + * Constructor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates + */ + FixedJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) { + super(_rigidBody1, _rigidBody2, JointType.FIXEDJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + } +} diff --git a/src/org/atriasoft/ephysics/constraint/HingeJoint.java b/src/org/atriasoft/ephysics/constraint/HingeJoint.java new file mode 100644 index 0000000..790120f --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/HingeJoint.java @@ -0,0 +1,862 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector2f; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.engine.ConstraintSolverData; +import org.atriasoft.ephysics.mathematics.Matrix2f; + +/** + * It represents a hinge joint that allows arbitrary rotation + * between two bodies around a single axis. This joint has one degree of freedom. It + * can be useful to simulate doors or pendulumns. + */ +public class HingeJoint extends Joint { + + private static float BETA; //!< Beta value for the bias factor of position correction + private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + private final Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1) + private final Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2) + private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates) + private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates) + private Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1 + private Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space + private Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space + private Vector3f b2CrossA1; //!< Cross product of vector b2 and a1 + private Vector3f c2CrossA1; //!< Cross product of vector c2 and a1; + private final Vector3f impulseTranslation; //!< Impulse for the 3 translation raints + private final Vector2f impulseRotation; //!< Impulse for the 2 rotation raints + private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint + private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint + private float impulseMotor; //!< Accumulated impulse for the motor raint; + private Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints + private Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints + private float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix) + private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor + private Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints + private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints + private float bLowerLimit; //!< Bias of the lower limit raint + private float bUpperLimit; //!< Bias of the upper limit raint + private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies + private boolean isLimitEnabled; //!< True if the joint limits are enabled + private boolean isMotorEnabled; //!< True if the motor of the joint in enabled + private float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian) + private float upperLimit; //!< Upper limit (maximum translation distance) + private boolean isLowerLimitViolated; //!< True if the lower limit is violated + private boolean isUpperLimitViolated; //!< True if the upper limit is violated + private float motorSpeed; //!< Motor speed (in rad/s) + private float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed + /// Reset the limits + + /// Constructor + public HingeJoint(final HingeJointInfo jointInfo) { + super(jointInfo); + this.impulseTranslation = new Vector3f(0, 0, 0); + this.impulseRotation = new Vector2f(0, 0); + this.impulseLowerLimit = 0; + this.impulseUpperLimit = 0; + this.impulseMotor = 0; + this.isLimitEnabled = jointInfo.isLimitEnabled; + this.isMotorEnabled = jointInfo.isMotorEnabled; + this.lowerLimit = jointInfo.minAngleLimit; + this.upperLimit = jointInfo.maxAngleLimit; + + this.isLowerLimitViolated = false; + this.isUpperLimitViolated = false; + this.motorSpeed = jointInfo.motorSpeed; + this.maxMotorTorque = jointInfo.maxMotorTorque; + + assert (this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * Constant.PI); + assert (this.upperLimit >= 0 && this.upperLimit <= 2.0 * Constant.PI); + + // Compute the local-space anchor point for each body + final Transform3D transform1 = this.body1.getTransform(); + final Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + + // Compute the local-space hinge axis + this.hingeLocalAxisBody1 = transform1.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld); + this.hingeLocalAxisBody2 = transform2.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld); + this.hingeLocalAxisBody1.normalize(); + this.hingeLocalAxisBody2.normalize(); + + // Compute the inverse of the initial orientation difference between the two bodies + this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew()); + this.initOrientationDifferenceInv.normalize(); + this.initOrientationDifferenceInv.inverse(); + } + + /// Given an "inputAngle" in the range [-pi, pi], this method returns an + /// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the + /// two angle limits in arguments. + private float computeCorrespondingAngleNearLimits(final float inputAngle, final float lowerLimitAngle, final float upperLimitAngle) { + if (upperLimitAngle <= lowerLimitAngle) { + return inputAngle; + } else if (inputAngle > upperLimitAngle) { + final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(inputAngle - upperLimitAngle)); + final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(inputAngle - lowerLimitAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - Constant.PI_2) : inputAngle; + } else if (inputAngle < lowerLimitAngle) { + final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(upperLimitAngle - inputAngle)); + final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(lowerLimitAngle - inputAngle)); + return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + Constant.PI_2); + } else { + return inputAngle; + } + } + + /// Compute the current angle around the hinge axis + private float computeCurrentHingeAngle(final Quaternion orientationBody1, final Quaternion orientationBody2) { + + float hingeAngle; + + // Compute the current orientation difference between the two bodies + final Quaternion currentOrientationDiff = orientationBody2.multiplyNew(orientationBody1.inverseNew()); + currentOrientationDiff.normalize(); + + // Compute the relative rotation idering the initial orientation difference + final Quaternion relativeRotation = currentOrientationDiff.multiplyNew(this.initOrientationDifferenceInv); + relativeRotation.normalize(); + + // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit + // length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with : + // |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any + // rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation + // axis is not pointing in the same direction as the hinge axis, we use the rotation -q which + // has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details + // about this trick is explained in the source code of OpenTissue (http://www.opentissue.org). + final float cosHalfAngle = relativeRotation.getW(); + final float sinHalfAngleAbs = relativeRotation.getVectorV().length(); + + // Compute the dot product of the relative rotation axis and the hinge axis + final float dotProduct = relativeRotation.getVectorV().dot(this.mA1); + + // If the relative rotation axis and the hinge axis are pointing the same direction + if (dotProduct >= 0.0f) { + hingeAngle = 2.0f * FMath.atan2(sinHalfAngleAbs, cosHalfAngle); + } else { + hingeAngle = 2.0f * FMath.atan2(sinHalfAngleAbs, -cosHalfAngle); + } + + // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi] + hingeAngle = computeNormalizedAngle(hingeAngle); + + // Compute and return the corresponding angle near one the two limits + return computeCorrespondingAngleNearLimits(hingeAngle, this.lowerLimit, this.upperLimit); + } + + /// Given an angle in radian, this method returns the corresponding + /// angle in the range [-pi; pi] + private float computeNormalizedAngle(float angle) { + + // Convert it into the range [-2*pi; 2*pi] + angle = FMath.mod(angle, Constant.PI_2); + + // Convert it into the range [-pi; pi] + if (angle < -Constant.PI) { + return angle + Constant.PI_2; + } else if (angle > Constant.PI) { + return angle - Constant.PI_2; + } else { + return angle; + } + } + + /**Enable/Disable the limits of the joint + * @param isLimitEnabled True if you want to enable the limits of the joint and + * false otherwise + */ + public void enableLimit(final boolean isLimitEnabled) { + + if (this.isLimitEnabled != isLimitEnabled) { + + this.isLimitEnabled = isLimitEnabled; + + // Reset the limits + resetLimits(); + } + } + + /**Enable/Disable the motor of the joint + * @param isMotorEnabled True if you want to enable the motor of the joint and false otherwise + */ + public void enableMotor(final boolean isMotorEnabled) { + + this.isMotorEnabled = isMotorEnabled; + this.impulseMotor = 0.0f; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + + /// Return the maximum angle limit + /** + * @return The maximum limit angle of the joint (in radian) + */ + public float getMaxAngleLimit() { + return this.upperLimit; + } + + /// Return the maximum motor torque + /** + * @return The maximum torque of the joint motor (in Newtons) + */ + public float getMaxMotorTorque() { + return this.maxMotorTorque; + } + + /// Return the minimum angle limit + /** + * @return The minimum limit angle of the joint (in radian) + */ + public float getMinAngleLimit() { + return this.lowerLimit; + } + + /// Return the motor speed + /** + * @return The current speed of the joint motor (in radian per second) + */ + public float getMotorSpeed() { + return this.motorSpeed; + } + + /// Return the intensity of the current torque applied for the joint motor + /** + * @param timeStep The current time step (in seconds) + * @return The intensity of the current torque (in Newtons) of the joint motor + */ + public float getMotorTorque(final float timeStep) { + return this.impulseMotor / timeStep; + } + + @Override + public void initBeforeSolve(final ConstraintSolverData raintSolverData) { + + // Initialize the bodies index in the velocity array + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1); + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2); + + // Get the bodies positions and orientations + final Vector3f x1 = this.body1.centerOfMassWorld; + final Vector3f x2 = this.body2.centerOfMassWorld; + final Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + final Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); + + // Get the inertia tensor of bodies + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = orientationBody1.multiply(this.localAnchorPointBody1); + this.r2World = orientationBody2.multiply(this.localAnchorPointBody2); + + // Compute the current angle around the hinge axis + final float hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2); + + // Check if the limit raints are violated or not + final float lowerLimitError = hingeAngle - this.lowerLimit; + final float upperLimitError = this.upperLimit - hingeAngle; + final boolean oldIsLowerLimitViolated = this.isLowerLimitViolated; + this.isLowerLimitViolated = lowerLimitError <= 0; + if (this.isLowerLimitViolated != oldIsLowerLimitViolated) { + this.impulseLowerLimit = 0.0f; + } + final boolean oldIsUpperLimitViolated = this.isUpperLimitViolated; + this.isUpperLimitViolated = upperLimitError <= 0; + if (this.isUpperLimitViolated != oldIsUpperLimitViolated) { + this.impulseUpperLimit = 0.0f; + } + + // Compute vectors needed in the Jacobian + this.mA1 = orientationBody1.multiply(this.hingeLocalAxisBody1); + final Vector3f a2 = orientationBody2.multiply(this.hingeLocalAxisBody2); + this.mA1.normalize(); + a2.normalize(); + final Vector3f b2 = a2.getOrthoVector(); + final Vector3f c2 = a2.cross(b2); + this.b2CrossA1 = b2.cross(this.mA1); + this.c2CrossA1 = c2.cross(this.mA1); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix) + final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew())); + this.inverseMassMatrixTranslation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslation = massMatrix.inverseNew(); + } + + // Compute the bias "b" of the translation raints + this.bTranslation.setZero(); + final float biasFactor = (BETA / raintSolverData.timeStep); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor); + } + + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix) + final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1); + final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1); + final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1); + final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1); + final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1); + final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1); + final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1); + final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1); + + final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22); + this.inverseMassMatrixRotation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotation = matrixKRotation.inverseNew(); + } + + // Compute the bias "b" of the rotation raints + this.bRotation.setZero(); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bRotation = new Vector2f(this.mA1.dot(b2) * biasFactor, this.mA1.dot(c2) * biasFactor); + } + + // If warm-starting is not enabled + if (!raintSolverData.isWarmStartingActive) { + + // Reset all the accumulated impulses + this.impulseTranslation.setZero(); + this.impulseRotation.setZero(); + this.impulseLowerLimit = 0.0f; + this.impulseUpperLimit = 0.0f; + this.impulseMotor = 0.0f; + } + + // If the motor or limits are enabled + if (this.isMotorEnabled || (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated))) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) + this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1)); + this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 1.0f / this.inverseMassMatrixLimitMotor : 0.0f; + + if (this.isLimitEnabled) { + + // Compute the bias "b" of the lower limit raint + this.bLowerLimit = 0.0f; + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bLowerLimit = biasFactor * lowerLimitError; + } + + // Compute the bias "b" of the upper limit raint + this.bUpperLimit = 0.0f; + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bUpperLimit = biasFactor * upperLimitError; + } + } + } + + } + + /// Return true if the limits or the joint are enabled + /** + * @return True if the limits of the joint are enabled and false otherwise + */ + public boolean isLimitEnabled() { + return this.isLimitEnabled; + } + + /// Return true if the motor of the joint is enabled + /** + * @return True if the motor of joint is enabled and false otherwise + */ + public boolean isMotorEnabled() { + return this.isMotorEnabled; + } + + private void resetLimits() { + + // Reset the accumulated impulses for the limits + this.impulseLowerLimit = 0.0f; + this.impulseUpperLimit = 0.0f; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + + /// Set the maximum angle limit + /** + * @param upperLimit The maximum limit angle of the joint (in radian) + */ + public void setMaxAngleLimit(final float upperLimit) { + + assert (upperLimit >= 0 && upperLimit <= 2.0 * Constant.PI); + + if (upperLimit != this.upperLimit) { + + this.upperLimit = upperLimit; + + // Reset the limits + resetLimits(); + } + } + + /// Set the maximum motor torque + /** + * @param maxMotorTorque The maximum torque (in Newtons) of the joint motor + */ + public void setMaxMotorTorque(final float maxMotorTorque) { + + if (maxMotorTorque != this.maxMotorTorque) { + + assert (this.maxMotorTorque >= 0.0f); + this.maxMotorTorque = maxMotorTorque; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + } + + /// Set the minimum angle limit + /** + * @param lowerLimit The minimum limit angle of the joint (in radian) + */ + public void setMinAngleLimit(final float lowerLimit) { + + assert (this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * Constant.PI); + + if (lowerLimit != this.lowerLimit) { + + this.lowerLimit = lowerLimit; + + // Reset the limits + resetLimits(); + } + } + + /// Set the motor speed + public void setMotorSpeed(final float motorSpeed) { + + if (motorSpeed != this.motorSpeed) { + + this.motorSpeed = motorSpeed; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + } + + @Override + public void solvePositionConstraint(final ConstraintSolverData raintSolverData) { + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (this.positionCorrectionTechnique != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) { + return; + } + + // Get the bodies positions and orientations + final Vector3f x1 = raintSolverData.positions[this.indexBody1]; + final Vector3f x2 = raintSolverData.positions[this.indexBody2]; + final Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + final Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Recompute the inverse inertia tensors + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Compute the vector from body center to the anchor point in world-space + this.r1World = q1.multiply(this.localAnchorPointBody1); + this.r2World = q2.multiply(this.localAnchorPointBody2); + + // Compute the current angle around the hinge axis + final float hingeAngle = computeCurrentHingeAngle(q1, q2); + + // Check if the limit raints are violated or not + final float lowerLimitError = hingeAngle - this.lowerLimit; + final float upperLimitError = this.upperLimit - hingeAngle; + this.isLowerLimitViolated = lowerLimitError <= 0; + this.isUpperLimitViolated = upperLimitError <= 0; + + // Compute vectors needed in the Jacobian + this.mA1 = q1.multiply(this.hingeLocalAxisBody1); + final Vector3f a2 = q2.multiply(this.hingeLocalAxisBody2); + this.mA1.normalize(); + a2.normalize(); + final Vector3f b2 = a2.getOrthoVector(); + final Vector3f c2 = a2.cross(b2); + this.b2CrossA1 = b2.cross(this.mA1); + this.c2CrossA1 = c2.cross(this.mA1); + + // Compute the corresponding skew-symmetric matrices + final Matrix3f skewSymmetricMatrixU1 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r1World); + final Matrix3f skewSymmetricMatrixU2 = Matrix3f.computeSkewSymmetricMatrixForCrossProduct(this.r2World); + + // --------------- Translation Constraints --------------- // + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints + final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse; + final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies); + massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew())); + massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew())); + this.inverseMassMatrixTranslation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslation = massMatrix.inverseNew(); + } + + // Compute position error for the 3 translation raints + final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World); + + // Compute the Lagrange multiplier lambda + final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1)); + + // Compute the impulse of body 1 + final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1); + Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World); + + // Compute the pseudo velocity of body 1 + final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1); + Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + x1.add(v1); + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse of body 2 + Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiplyNew(-1); + + // Compute the pseudo velocity of body 2 + final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2); + Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2.add(v2); + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix) + final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1); + final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1); + final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1); + final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1); + final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1); + final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1); + final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1); + final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1); + + final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22); + this.inverseMassMatrixRotation.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotation = matrixKRotation.inverseNew(); + } + + // Compute the position error for the 3 rotation raints + final Vector2f errorRotation = new Vector2f(this.mA1.dot(b2), this.mA1.dot(c2)); + + // Compute the Lagrange multiplier lambda for the 3 rotation raints + final Vector2f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 + angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(lambdaRotation.x).less(this.c2CrossA1.multiplyNew(lambdaRotation.y)); + + // Compute the pseudo velocity of body 1 + w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse of body 2 + angularImpulseBody2 = this.b2CrossA1.multiplyNew(lambdaRotation.x).add(this.c2CrossA1.multiplyNew(lambdaRotation.y)); + + // Compute the pseudo velocity of body 2 + w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + + // --------------- Limits Constraints --------------- // + + if (this.isLimitEnabled) { + + if (this.isLowerLimitViolated || this.isUpperLimitViolated) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1)); + this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 1.0f / this.inverseMassMatrixLimitMotor : 0.0f; + } + + // If the lower limit is violated + if (this.isLowerLimitViolated) { + + // Compute the Lagrange multiplier lambda for the lower limit raint + final float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError); + + // Compute the impulse P=J^T * lambda of body 1 + final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaLowerLimit); + + // Compute the pseudo velocity of body 1 + final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp); + + // Update the body position/orientation of body 1 + q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda of body 2 + final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(lambdaLowerLimit); + + // Compute the pseudo velocity of body 2 + final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp); + + // Update the body position/orientation of body 2 + q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f)); + q2.normalize(); + } + + // If the upper limit is violated + if (this.isUpperLimitViolated) { + + // Compute the Lagrange multiplier lambda for the upper limit raint + final float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError); + + // Compute the impulse P=J^T * lambda of body 1 + final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaUpperLimit); + + // Compute the pseudo velocity of body 1 + final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp); + + // Update the body position/orientation of body 1 + q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda of body 2 + final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-lambdaUpperLimit); + + // Compute the pseudo velocity of body 2 + final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp); + + // Update the body position/orientation of body 2 + q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f)); + q2.normalize(); + } + } + } + + @Override + public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // --------------- Translation Constraints --------------- // + + // Compute J*v + final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World)); + + // Compute the Lagrange multiplier lambda + final Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation)); + this.impulseTranslation.add(deltaLambdaTranslation); + + // Compute the impulse P=J^T * lambda of body 1 + final Vector3f linearImpulseBody1 = deltaLambdaTranslation.multiplyNew(-1); + Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda of body 2 + Vector3f angularImpulseBody2 = deltaLambdaTranslation.cross(this.r2World).multiplyNew(-1); + + // Apply the impulse to the body 2 + v2.add(deltaLambdaTranslation.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 2 rotation raints + final Vector2f JvRotation = new Vector2f(-this.b2CrossA1.dot(w1) + this.b2CrossA1.dot(w2), -this.c2CrossA1.dot(w1) + this.c2CrossA1.dot(w2)); + + // Compute the Lagrange multiplier lambda for the 2 rotation raints + final Vector2f deltaLambdaRotation = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation)); + this.impulseRotation.add(deltaLambdaRotation); + + // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1 + angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(deltaLambdaRotation.x).less(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y)); + + // Apply the impulse to the body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2 + angularImpulseBody2 = this.b2CrossA1.multiplyNew(deltaLambdaRotation.x).add(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y)); + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + // --------------- Limits Constraints --------------- // + + if (this.isLimitEnabled) { + + // If the lower limit is violated + if (this.isLowerLimitViolated) { + + // Compute J*v for the lower limit raint + final float JvLowerLimit = w2.lessNew(w1).dot(this.mA1); + + // Compute the Lagrange multiplier lambda for the lower limit raint + float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit - this.bLowerLimit); + final float lambdaTemp = this.impulseLowerLimit; + this.impulseLowerLimit = FMath.max(this.impulseLowerLimit + deltaLambdaLower, 0.0f); + deltaLambdaLower = this.impulseLowerLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 1 + final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaLower); + + // Apply the impulse to the body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp)); + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 + final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaLower); + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp)); + } + + // If the upper limit is violated + if (this.isUpperLimitViolated) { + + // Compute J*v for the upper limit raint + final float JvUpperLimit = w2.lessNew(w1).multiplyNew(-1).dot(this.mA1); + + // Compute the Lagrange multiplier lambda for the upper limit raint + float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit - this.bUpperLimit); + final float lambdaTemp = this.impulseUpperLimit; + this.impulseUpperLimit = FMath.max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f); + deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 1 + final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(deltaLambdaUpper); + + // Apply the impulse to the body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp)); + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 + final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-deltaLambdaUpper); + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp)); + } + } + + // --------------- Motor --------------- // + + // If the motor is enabled + if (this.isMotorEnabled) { + + // Compute J*v for the motor + final float JvMotor = this.mA1.dot(w1.lessNew(w2)); + + // Compute the Lagrange multiplier lambda for the motor + final float maxMotorImpulse = this.maxMotorTorque * raintSolverData.timeStep; + float deltaLambdaMotor = this.inverseMassMatrixLimitMotor * (-JvMotor - this.motorSpeed); + final float lambdaTemp = this.impulseMotor; + this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = this.impulseMotor - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the motor of body 1 + final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaMotor); + + // Apply the impulse to the body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp)); + + // Compute the impulse P=J^T * lambda for the motor of body 2 + final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaMotor); + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp)); + } + + } + + @Override + public void warmstart(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Compute the impulse P=J^T * lambda for the 2 rotation raints + final Vector3f rotationImpulse = this.b2CrossA1.multiplyNew(-1).multiply(this.impulseRotation.x).less(this.c2CrossA1.multiplyNew(this.impulseRotation.y)); + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints + final Vector3f limitsImpulse = this.mA1.multiplyNew(this.impulseUpperLimit - this.impulseLowerLimit); + + // Compute the impulse P=J^T * lambda for the motor raint + final Vector3f motorImpulse = this.mA1.multiplyNew(-this.impulseMotor); + + // Compute the impulse P=J^T * lambda for the 3 translation raints of body 1 + final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1); + final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); + + // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1 + angularImpulseBody1.add(rotationImpulse); + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1 + angularImpulseBody1.add(limitsImpulse); + + // Compute the impulse P=J^T * lambda for the motor raint of body 1 + angularImpulseBody1.add(motorImpulse); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 3 translation raints of body 2 + final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1); + + // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2 + angularImpulseBody2.add(rotationImpulse.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2 + angularImpulseBody2.add(limitsImpulse.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the motor raint of body 2 + angularImpulseBody2.add(motorImpulse.multiplyNew(-1)); + + // Apply the impulse to the body 2 + v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + } + +} diff --git a/src/org/atriasoft/ephysics/constraint/HingeJointInfo.java b/src/org/atriasoft/ephysics/constraint/HingeJointInfo.java new file mode 100644 index 0000000..28bba00 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/HingeJointInfo.java @@ -0,0 +1,89 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * It is used to gather the information needed to create a hinge joint. + * This structure will be used to create the actual hinge joint. + */ +public class HingeJointInfo extends JointInfo { + + public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + public Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates) + public boolean isLimitEnabled; //!< True if the hinge joint limits are enabled + public boolean isMotorEnabled; //!< True if the hinge joint motor is enabled + public float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0] + public float maxAngleLimit; //!< Maximum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [0, 2*pi] + public float motorSpeed; //!< Motor speed (in radian/second) + public float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed + + /** + * Constructor without limits and without motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates + * @param _initRotationAxisWorld The initial rotation axis in world-space coordinates + */ + public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld) { + super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.rotationAxisWorld = _initRotationAxisWorld; + this.isLimitEnabled = false; + this.isMotorEnabled = false; + this.minAngleLimit = -1; + this.maxAngleLimit = 1; + this.motorSpeed = 0; + this.maxMotorTorque = 0; + + } + + /** + * Constructor with limits but without motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates + * @param _initRotationAxisWorld The intial rotation axis in world-space coordinates + * @param _initMinAngleLimit The initial minimum limit angle (in radian) + * @param _initMaxAngleLimit The initial maximum limit angle (in radian) + */ + public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit, + final float _initMaxAngleLimit) { + super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.rotationAxisWorld = _initRotationAxisWorld; + this.isLimitEnabled = true; + this.isMotorEnabled = false; + this.minAngleLimit = _initMinAngleLimit; + this.maxAngleLimit = _initMaxAngleLimit; + this.motorSpeed = 0; + this.maxMotorTorque = 0; + + } + + /** + * Constructor with limits and motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space + * @param _initRotationAxisWorld The initial rotation axis in world-space + * @param _initMinAngleLimit The initial minimum limit angle (in radian) + * @param _initMaxAngleLimit The initial maximum limit angle (in radian) + * @param _initMotorSpeed The initial motor speed of the joint (in radian per second) + * @param _initMaxMotorTorque The initial maximum motor torque (in Newtons) + */ + public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit, + final float _initMaxAngleLimit, final float _initMotorSpeed, final float _initMaxMotorTorque) { + super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.rotationAxisWorld = _initRotationAxisWorld; + this.isLimitEnabled = true; + this.isMotorEnabled = false; + this.minAngleLimit = _initMinAngleLimit; + this.maxAngleLimit = _initMaxAngleLimit; + this.motorSpeed = _initMotorSpeed; + this.maxMotorTorque = _initMaxMotorTorque; + + } +}; diff --git a/src/org/atriasoft/ephysics/constraint/Joint.java b/src/org/atriasoft/ephysics/constraint/Joint.java new file mode 100644 index 0000000..2d305b2 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/Joint.java @@ -0,0 +1,80 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.ephysics.body.RigidBody; +import org.atriasoft.ephysics.engine.ConstraintSolverData; + +/** + * It represents a joint between two bodies. + */ +public abstract class Joint { + + protected RigidBody body1; //!< Pointer to the first body of the joint + protected RigidBody body2; //!< Pointer to the second body of the joint + protected JointType type; //!< Type of the joint + protected int indexBody1; //!< Body 1 index in the velocity array to solve the raint + protected int indexBody2; //!< Body 2 index in the velocity array to solve the raint + protected JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints) + protected boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other + public boolean isAlreadyInIsland; //!< True if the joint has already been added into an island + + /// Constructor + public Joint(final JointInfo jointInfo) { + this.body1 = jointInfo.body1; + this.body2 = jointInfo.body2; + this.type = jointInfo.type; + this.positionCorrectionTechnique = jointInfo.positionCorrectionTechnique; + this.isCollisionEnabled = jointInfo.isCollisionEnabled; + this.isAlreadyInIsland = false; + assert (this.body1 != null); + assert (this.body2 != null); + } + + /// Return the reference to the body 1 + public RigidBody getBody1() { + return this.body1; + } + + /// Return the reference to the body 2 + public RigidBody getBody2() { + return this.body2; + } + + /** Return the type of the raint + * @return The type of the joint + */ + public JointType getType() { + return this.type; + } + + /// Initialize before solving the joint + public abstract void initBeforeSolve(final ConstraintSolverData raintSolverData); + + /** Return true if the joint is active + * @return True if the joint is active + */ + public boolean isActive() { + return (this.body1.isActive() && this.body2.isActive()); + } + + /// Return true if the joint has already been added into an island + public boolean isAlreadyInIsland() { + return this.isAlreadyInIsland; + } + + /**Return true if the collision between the two bodies of the joint is enabled + * @return True if the collision is enabled between the two bodies of the joint + * is enabled and false otherwise + */ + public boolean isCollisionEnabled() { + return this.isCollisionEnabled; + } + + /// Solve the position raint + public abstract void solvePositionConstraint(final ConstraintSolverData raintSolverData); + + /// Solve the velocity raint + public abstract void solveVelocityConstraint(final ConstraintSolverData raintSolverData); + + /// Warm start the joint (apply the previous impulse at the beginning of the step) + public abstract void warmstart(final ConstraintSolverData raintSolverData); +} diff --git a/src/org/atriasoft/ephysics/constraint/JointInfo.java b/src/org/atriasoft/ephysics/constraint/JointInfo.java new file mode 100644 index 0000000..501284a --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/JointInfo.java @@ -0,0 +1,37 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * It is used to gather the information needed to create a joint. + */ +public class JointInfo { + //!< First rigid body of the joint + public final RigidBody body1; + //!< Second rigid body of the joint + public final RigidBody body2; + //!< Type of the joint + public final JointType type; + //!< Position correction technique used for the raint (used for joints). By default, the BAUMGARTE technique is used + public final JointsPositionCorrectionTechnique positionCorrectionTechnique; + //!< True if the two bodies of the joint are allowed to collide with each other + public final boolean isCollisionEnabled; + + /// Constructor + JointInfo(final JointType _raintType) { + this.body1 = null; + this.body2 = null; + this.type = _raintType; + this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL; + this.isCollisionEnabled = true; + } + + /// Constructor + JointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final JointType _raintType) { + this.body1 = _rigidBody1; + this.body2 = _rigidBody2; + this.type = _raintType; + this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL; + this.isCollisionEnabled = true; + } +} diff --git a/src/org/atriasoft/ephysics/constraint/JointListElement.java b/src/org/atriasoft/ephysics/constraint/JointListElement.java new file mode 100644 index 0000000..d8289d8 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/JointListElement.java @@ -0,0 +1,16 @@ +package org.atriasoft.ephysics.constraint; + +public class JointListElement { + + public Joint joint; //!< Pointer to the actual joint + public JointListElement next; //!< Next element of the list + + /** + * Constructor + */ + public JointListElement(final Joint _initJoint, final JointListElement _initNext) { + this.joint = _initJoint; + this.next = _initNext; + + } +} diff --git a/src/org/atriasoft/ephysics/constraint/JointType.java b/src/org/atriasoft/ephysics/constraint/JointType.java new file mode 100644 index 0000000..c63b273 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/JointType.java @@ -0,0 +1,9 @@ +package org.atriasoft.ephysics.constraint; + +/// Enumeration for the type of a raint +public enum JointType { + BALLSOCKETJOINT, + SLIDERJOINT, + HINGEJOINT, + FIXEDJOINT +} diff --git a/src/org/atriasoft/ephysics/constraint/JointsPositionCorrectionTechnique.java b/src/org/atriasoft/ephysics/constraint/JointsPositionCorrectionTechnique.java new file mode 100644 index 0000000..bad9f47 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/JointsPositionCorrectionTechnique.java @@ -0,0 +1,9 @@ +package org.atriasoft.ephysics.constraint; + +/// Position correction technique used in the raint solver (for joints). +/// BAUMGARTE_JOINTS : Faster but can be innacurate in some situations. +/// NON_LINEAR_GAUSS_SEIDEL : Slower but more precise. This is the option used by default. +public enum JointsPositionCorrectionTechnique { + BAUMGARTE_JOINTS, + NON_LINEAR_GAUSS_SEIDEL +} diff --git a/src/org/atriasoft/ephysics/constraint/SliderJoint.java b/src/org/atriasoft/ephysics/constraint/SliderJoint.java new file mode 100644 index 0000000..6e19529 --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/SliderJoint.java @@ -0,0 +1,853 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Transform3D; +import org.atriasoft.etk.math.Vector2f; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.engine.ConstraintSolverData; +import org.atriasoft.ephysics.mathematics.Matrix2f; + +public class SliderJoint extends Joint { + private static final float BETA = 0.2f; //!< Beta value for the position correction bias factor + private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + private final Vector3f sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1) + private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates) + private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates) + private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies + private Vector3f N1; //!< First vector orthogonal to the slider axis local-space of body 1 + private Vector3f N2; //!< Second vector orthogonal to the slider axis and N1 in local-space of body 1 + private Vector3f R1; //!< Vector r1 in world-space coordinates + private Vector3f R2; //!< Vector r2 in world-space coordinates + private Vector3f R2CrossN1; //!< Cross product of r2 and n1 + private Vector3f R2CrossN2; //!< Cross product of r2 and n2 + private Vector3f R2CrossSliderAxis; //!< Cross product of r2 and the slider axis + private Vector3f R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1 + private Vector3f R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2 + private Vector3f R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis + private Vector2f bTranslation; //!< Bias of the 2 translation raints + private Vector3f bRotation; //!< Bias of the 3 rotation raints + private float bLowerLimit; //!< Bias of the lower limit raint + private float bUpperLimit; //!< Bias of the upper limit raint + private Matrix2f inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix) + private Matrix3f inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix) + private float inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix) + private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor + private final Vector2f impulseTranslation; //!< Accumulated impulse for the 2 translation raints + private final Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints + private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint + private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint + private float impulseMotor; //!< Accumulated impulse for the motor + private boolean isLimitEnabled; //!< True if the slider limits are enabled + private boolean isMotorEnabled; //!< True if the motor of the joint in enabled + private Vector3f sliderAxisWorld; //!< Slider axis in world-space coordinates + private float lowerLimit; //!< Lower limit (minimum translation distance) + private float upperLimit; //!< Upper limit (maximum translation distance) + private boolean isLowerLimitViolated; //!< True if the lower limit is violated + private boolean isUpperLimitViolated; //!< True if the upper limit is violated + private float motorSpeed; //!< Motor speed (in m/s) + private float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + /// Constructor + + public SliderJoint(final SliderJointInfo jointInfo) { + super(jointInfo); + this.impulseTranslation = new Vector2f(0, 0); + this.impulseRotation = new Vector3f(0, 0, 0); + this.impulseLowerLimit = 0; + this.impulseUpperLimit = 0; + this.impulseMotor = 0; + this.isLimitEnabled = jointInfo.isLimitEnabled; + this.isMotorEnabled = jointInfo.isMotorEnabled; + this.lowerLimit = jointInfo.minTranslationLimit; + this.upperLimit = jointInfo.maxTranslationLimit; + this.isLowerLimitViolated = false; + this.isUpperLimitViolated = false; + this.motorSpeed = jointInfo.motorSpeed; + this.maxMotorForce = jointInfo.maxMotorForce; + + assert (this.upperLimit >= 0.0f); + assert (this.lowerLimit <= 0.0f); + assert (this.maxMotorForce >= 0.0f); + + // Compute the local-space anchor point for each body + final Transform3D transform1 = this.body1.getTransform(); + final Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace); + + // Compute the inverse of the initial orientation difference between the two bodies + this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew()); + this.initOrientationDifferenceInv.normalize(); + this.initOrientationDifferenceInv.inverse(); + + // Compute the slider axis in local-space of body 1 + this.sliderAxisBody1 = this.body1.getTransform().getOrientation().inverseNew().multiply(jointInfo.sliderAxisWorldSpace); + this.sliderAxisBody1.normalize(); + } + + /// Enable/Disable the limits of the joint + /** + * @param isLimitEnabled True if you want to enable the joint limits and false + * otherwise + */ + public void enableLimit(final boolean isLimitEnabled) { + + if (isLimitEnabled != this.isLimitEnabled) { + + this.isLimitEnabled = isLimitEnabled; + + // Reset the limits + resetLimits(); + } + } + + /// Enable/Disable the motor of the joint + /** + * @param isMotorEnabled True if you want to enable the joint motor and false + * otherwise + */ + public void enableMotor(final boolean isMotorEnabled) { + + this.isMotorEnabled = isMotorEnabled; + this.impulseMotor = 0.0f; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + + /// Return the maximum motor force + /** + * @return The maximum force of the joint motor (in Newton x meters) + */ + public float getMaxMotorForce() { + return this.maxMotorForce; + } + + /// Return the maximum translation limit + /** + * @return The maximum translation limit of the joint (in meters) + */ + public float getMaxTranslationLimit() { + return this.upperLimit; + } + + /// Return the minimum translation limit + /** + * @return The minimum translation limit of the joint (in meters) + */ + public float getMinTranslationLimit() { + return this.lowerLimit; + } + + /// Return the intensity of the current force applied for the joint motor + /** + * @param timeStep Time step (in seconds) + * @return The current force of the joint motor (in Newton x meters) + */ + public float getMotorForce(final float timeStep) { + return this.impulseMotor / timeStep; + } + + /// Return the motor speed + /** + * @return The current motor speed of the joint (in meters per second) + */ + public float getMotorSpeed() { + return this.motorSpeed; + } + + /// Return the current translation value of the joint + /** + * @return The current translation distance of the joint (in meters) + */ + public float getTranslation() { + + // TODO : Check if we need to compare rigid body position or center of mass here + + // Get the bodies positions and orientations + final Vector3f x1 = this.body1.getTransform().getPosition(); + final Vector3f x2 = this.body2.getTransform().getPosition(); + final Quaternion q1 = this.body1.getTransform().getOrientation(); + final Quaternion q2 = this.body2.getTransform().getOrientation(); + + // Compute the two anchor points in world-space coordinates + final Vector3f anchorBody1 = x1.addNew(q1.multiply(this.localAnchorPointBody1)); + final Vector3f anchorBody2 = x2.addNew(q2.multiply(this.localAnchorPointBody2)); + + // Compute the vector u (difference between anchor points) + final Vector3f u = anchorBody2.lessNew(anchorBody1); + + // Compute the slider axis in world-space + final Vector3f sliderAxisWorld = q1.multiply(this.sliderAxisBody1); + sliderAxisWorld.normalize(); + + // Compute and return the translation value + return u.dot(sliderAxisWorld); + } + + @Override + public void initBeforeSolve(final ConstraintSolverData raintSolverData) { + { + + // Initialize the bodies index in the veloc ity array + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body1); + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.get(this.body2); + + // Get the bodies positions and orientations + final Vector3f x1 = this.body1.centerOfMassWorld; + final Vector3f x2 = this.body2.centerOfMassWorld; + final Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + final Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); + + // Get the inertia tensor of bodies + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Vector from body center to the anchor point + this.R1 = orientationBody1.multiply(this.localAnchorPointBody1); + this.R2 = orientationBody2.multiply(this.localAnchorPointBody2); + + // Compute the vector u (difference between anchor points) + final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1); + + // Compute the two orthogonal vectors to the slider axis in world-space + this.sliderAxisWorld = orientationBody1.multiply(this.sliderAxisBody1); + this.sliderAxisWorld.normalize(); + this.N1 = this.sliderAxisWorld.getOrthoVector(); + this.N2 = this.sliderAxisWorld.cross(this.N1); + + // Check if the limit raints are violated or not + final float uDotSliderAxis = u.dot(this.sliderAxisWorld); + final float lowerLimitError = uDotSliderAxis - this.lowerLimit; + final float upperLimitError = this.upperLimit - uDotSliderAxis; + final boolean oldIsLowerLimitViolated = this.isLowerLimitViolated; + this.isLowerLimitViolated = lowerLimitError <= 0; + if (this.isLowerLimitViolated != oldIsLowerLimitViolated) { + this.impulseLowerLimit = 0.0f; + } + final boolean oldIsUpperLimitViolated = this.isUpperLimitViolated; + this.isUpperLimitViolated = upperLimitError <= 0; + if (this.isUpperLimitViolated != oldIsUpperLimitViolated) { + this.impulseUpperLimit = 0.0f; + } + + // Compute the cross products used in the Jacobians + this.R2CrossN1 = this.R2.cross(this.N1); + this.R2CrossN2 = this.R2.cross(this.N2); + this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); + final Vector3f r1PlusU = this.R1.addNew(u); + this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1); + this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2); + this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld); + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // raints (2x2 matrix) + final float sumInverseMass = this.body1.massInverse + this.body2.massInverse; + final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1); + final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2); + final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1); + final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2); + final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1); + final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2); + final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1); + final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2); + + final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22); + this.inverseMassMatrixTranslationConstraint.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslationConstraint = matrixKTranslation.inverseNew(); + } + + // Compute the bias "b" of the translation raint + this.bTranslation.setZero(); + final float biasFactor = (BETA / raintSolverData.timeStep); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bTranslation.setX(u.dot(this.N1)); + this.bTranslation.setY(u.dot(this.N2)); + this.bTranslation.multiply(biasFactor); + } + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + this.inverseMassMatrixRotationConstraint = this.i1.addNew(this.i2); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew(); + } + + // Compute the bias "b" of the rotation raint + this.bRotation.setZero(); + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew()); + currentOrientationDifference.normalize(); + final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv); + this.bRotation = qError.getVectorV().multiplyNew(biasFactor * 2.0f); + } + // If the limits are enabled + if (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated)) { + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis)) + + this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis)); + this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 1.0f / this.inverseMassMatrixLimit : 0.0f; + // Compute the bias "b" of the lower limit raint + this.bLowerLimit = 0.0f; + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bLowerLimit = biasFactor * lowerLimitError; + } + // Compute the bias "b" of the upper limit raint + this.bUpperLimit = 0.0f; + if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.bUpperLimit = biasFactor * upperLimitError; + } + } + + // If the motor is enabled + if (this.isMotorEnabled) { + // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) + this.inverseMassMatrixMotor = this.body1.massInverse + this.body2.massInverse; + this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0f) ? 1.0f / this.inverseMassMatrixMotor : 0.0f; + } + + // If warm-starting is not enabled + if (!raintSolverData.isWarmStartingActive) { + // Reset all the accumulated impulses + this.impulseTranslation.setZero(); + this.impulseRotation.setZero(); + this.impulseLowerLimit = 0.0f; + this.impulseUpperLimit = 0.0f; + this.impulseMotor = 0.0f; + } + } + } + + /// Return true if the limits or the joint are enabled + /** + * @return True if the joint limits are enabled + */ + public boolean isLimitEnabled() { + return this.isLimitEnabled; + } + + /// Return true if the motor of the joint is enabled + /** + * @return True if the joint motor is enabled + */ + public boolean isMotorEnabled() { + return this.isMotorEnabled; + } + + /// Reset the limits + private void resetLimits() { + + // Reset the accumulated impulses for the limits + this.impulseLowerLimit = 0.0f; + this.impulseUpperLimit = 0.0f; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + + /// Set the maximum motor force + /** + * @param maxMotorForce The maximum force of the joint motor (in Newton x meters) + */ + public void setMaxMotorForce(final float maxMotorForce) { + + if (maxMotorForce != this.maxMotorForce) { + + assert (this.maxMotorForce >= 0.0f); + this.maxMotorForce = maxMotorForce; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + } + + /// Set the maximum translation limit + /** + * @param lowerLimit The maximum translation limit of the joint (in meters) + */ + public void setMaxTranslationLimit(final float upperLimit) { + + assert (this.lowerLimit <= upperLimit); + + if (upperLimit != this.upperLimit) { + + this.upperLimit = upperLimit; + + // Reset the limits + resetLimits(); + } + } + + /// Set the minimum translation limit + /** + * @param lowerLimit The minimum translation limit of the joint (in meters) + */ + public void setMinTranslationLimit(final float lowerLimit) { + + assert (lowerLimit <= this.upperLimit); + + if (lowerLimit != this.lowerLimit) { + + this.lowerLimit = lowerLimit; + + // Reset the limits + resetLimits(); + } + } + + /// Set the motor speed + /** + * @param motorSpeed The speed of the joint motor (in meters per second) + */ + public void setMotorSpeed(final float motorSpeed) { + + if (motorSpeed != this.motorSpeed) { + + this.motorSpeed = motorSpeed; + + // Wake up the two bodies of the joint + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); + } + } + + @Override + public void solvePositionConstraint(final ConstraintSolverData raintSolverData) { + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // do not execute this method + if (this.positionCorrectionTechnique != JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL) { + return; + } + + // Get the bodies positions and orientations + final Vector3f x1 = raintSolverData.positions[this.indexBody1]; + final Vector3f x2 = raintSolverData.positions[this.indexBody2]; + final Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + final Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Recompute the inertia tensor of bodies + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); + + // Vector from body center to the anchor point + this.R1 = q1.multiply(this.localAnchorPointBody1); + this.R2 = q2.multiply(this.localAnchorPointBody2); + + // Compute the vector u (difference between anchor points) + final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1); + + // Compute the two orthogonal vectors to the slider axis in world-space + this.sliderAxisWorld = q1.multiply(this.sliderAxisBody1); + this.sliderAxisWorld.normalize(); + this.N1 = this.sliderAxisWorld.getOrthoVector(); + this.N2 = this.sliderAxisWorld.cross(this.N1); + + // Check if the limit raints are violated or not + final float uDotSliderAxis = u.dot(this.sliderAxisWorld); + final float lowerLimitError = uDotSliderAxis - this.lowerLimit; + final float upperLimitError = this.upperLimit - uDotSliderAxis; + this.isLowerLimitViolated = lowerLimitError <= 0; + this.isUpperLimitViolated = upperLimitError <= 0; + + // Compute the cross products used in the Jacobians + this.R2CrossN1 = this.R2.cross(this.N1); + this.R2CrossN2 = this.R2.cross(this.N2); + this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); + final Vector3f r1PlusU = this.R1.addNew(u); + this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1); + this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2); + this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld); + + // --------------- Translation Constraints --------------- // + + // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // raints (2x2 matrix) + final float sumInverseMass = this.body1.massInverse + this.body2.massInverse; + final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1); + final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2); + final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1); + final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2); + final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1); + final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2); + final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1); + final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2); + + final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22); + this.inverseMassMatrixTranslationConstraint.setZero(); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixTranslationConstraint = matrixKTranslation.inverseNew(); + } + + // Compute the position error for the 2 translation raints + final Vector2f translationError = new Vector2f(u.dot(this.N1), u.dot(this.N2)); + + // Compute the Lagrange multiplier lambda for the 2 translation raints + final Vector2f lambdaTranslation = this.inverseMassMatrixTranslationConstraint.multiplyNew(translationError.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 + final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.N2.multiplyNew(lambdaTranslation.y)); + Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.R1PlusUCrossN2.multiplyNew(lambdaTranslation.y)); + + // Apply the impulse to the body 1 + final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1); + Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + x1.add(v1); + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 + final Vector3f linearImpulseBody2 = this.N1.multiplyNew(lambdaTranslation.x).addNew(this.N2.multiplyNew(lambdaTranslation.y)); + Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(lambdaTranslation.x).add(this.R2CrossN2.multiplyNew(lambdaTranslation.y)); + + // Apply the impulse to the body 2 + final Vector3f v2 = linearImpulseBody2.multiplyNew(inverseMassBody2); + Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + x2.add(v2); + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + + // --------------- Rotation Constraints --------------- // + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + this.inverseMassMatrixRotationConstraint = this.i1.addNew(this.i2); + if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) { + this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew(); + } + + // Compute the position error for the 3 rotation raints + final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew()); + currentOrientationDifference.normalize(); + final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv); + final Vector3f errorRotation = qError.getVectorV().multiply(2.0f); + + // Compute the Lagrange multiplier lambda for the 3 rotation raints + final Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint.multiplyNew(errorRotation.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 + angularImpulseBody1 = lambdaRotation.multiplyNew(-1); + + // Apply the impulse to the body 1 + w1 = this.i1.multiplyNew(angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 + angularImpulseBody2 = lambdaRotation; + + // Apply the impulse to the body 2 + w2 = this.i2.multiplyNew(angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f)); + q2.normalize(); + + // --------------- Limits Constraints --------------- // + + if (this.isLimitEnabled) { + + if (this.isLowerLimitViolated || this.isUpperLimitViolated) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) + this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis)) + + this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis)); + this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 1.0f / this.inverseMassMatrixLimit : 0.0f; + } + + // If the lower limit is violated + if (this.isLowerLimitViolated) { + + // Compute the Lagrange multiplier lambda for the lower limit raint + final float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError); + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 1 + final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-lambdaLowerLimit); + final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-lambdaLowerLimit); + + // Apply the impulse to the body 1 + final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1); + final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp); + + // Update the body position/orientation of body 1 + x1.add(v1_tmp); + q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 + final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(lambdaLowerLimit); + final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(lambdaLowerLimit); + + // Apply the impulse to the body 2 + final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2); + final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp); + + // Update the body position/orientation of body 2 + x2.add(v2_tmp); + q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f)); + q2.normalize(); + } + + // If the upper limit is violated + if (this.isUpperLimitViolated) { + + // Compute the Lagrange multiplier lambda for the upper limit raint + final float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError); + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 1 + final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(lambdaUpperLimit); + final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(lambdaUpperLimit); + + // Apply the impulse to the body 1 + final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1); + final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp); + + // Update the body position/orientation of body 1 + x1.add(v1_tmp); + q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f)); + q1.normalize(); + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 + final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-lambdaUpperLimit); + final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-lambdaUpperLimit); + + // Apply the impulse to the body 2 + final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2); + final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp); + + // Update the body position/orientation of body 2 + x2.add(v2_tmp); + q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f)); + q2.normalize(); + } + } + } + + @Override + public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) { + + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // --------------- Translation Constraints --------------- // + + // Compute J*v for the 2 translation raints + final float el1 = -this.N1.dot(v1) - w1.dot(this.R1PlusUCrossN1) + this.N1.dot(v2) + w2.dot(this.R2CrossN1); + final float el2 = -this.N2.dot(v1) - w1.dot(this.R1PlusUCrossN2) + this.N2.dot(v2) + w2.dot(this.R2CrossN2); + + final Vector2f JvTranslation = new Vector2f(el1, el2); + + // Compute the Lagrange multiplier lambda for the 2 translation raints + final Vector2f deltaLambda = this.inverseMassMatrixTranslationConstraint.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation)); + this.impulseTranslation.add(deltaLambda); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 + final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(deltaLambda.x).less(this.N2.multiplyNew(deltaLambda.y)); + Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(deltaLambda.x).less(this.R1PlusUCrossN2.multiplyNew(deltaLambda.y)); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 + final Vector3f linearImpulseBody2 = this.N1.multiplyNew(deltaLambda.x).add(this.N2.multiplyNew(deltaLambda.y)); + Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(deltaLambda.x).add(this.R2CrossN2.multiplyNew(deltaLambda.y)); + + // Apply the impulse to the body 2 + v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation raints + final Vector3f JvRotation = w2.lessNew(w1); + + // Compute the Lagrange multiplier lambda for the 3 rotation raints + final Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation)); + this.impulseRotation.add(deltaLambda2); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 + angularImpulseBody1 = deltaLambda2.multiplyNew(-1); + + // Apply the impulse to the body to body 1 + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 + angularImpulseBody2 = deltaLambda2; + + // Apply the impulse to the body 2 + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + + // --------------- Limits Constraints --------------- // + + if (this.isLimitEnabled) { + + // If the lower limit is violated + if (this.isLowerLimitViolated) { + + // Compute J*v for the lower limit raint + final float JvLowerLimit = this.sliderAxisWorld.dot(v2) + this.R2CrossSliderAxis.dot(w2) - this.sliderAxisWorld.dot(v1) - this.R1PlusUCrossSliderAxis.dot(w1); + + // Compute the Lagrange multiplier lambda for the lower limit raint + float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit - this.bLowerLimit); + final float lambdaTemp = this.impulseLowerLimit; + this.impulseLowerLimit = FMath.max(this.impulseLowerLimit + deltaLambdaLower, 0.0f); + deltaLambdaLower = this.impulseLowerLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 1 + final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaLower); + final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-deltaLambdaLower); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp)); + + // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 + final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaLower); + final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(deltaLambdaLower); + + // Apply the impulse to the body 2 + v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp)); + } + + // If the upper limit is violated + if (this.isUpperLimitViolated) { + + // Compute J*v for the upper limit raint + final float JvUpperLimit = this.sliderAxisWorld.dot(v1) + this.R1PlusUCrossSliderAxis.dot(w1) - this.sliderAxisWorld.dot(v2) - this.R2CrossSliderAxis.dot(w2); + + // Compute the Lagrange multiplier lambda for the upper limit raint + float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit - this.bUpperLimit); + final float lambdaTemp = this.impulseUpperLimit; + this.impulseUpperLimit = FMath.max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f); + deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 1 + final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaUpper); + final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(deltaLambdaUpper); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp)); + + // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 + final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaUpper); + final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-deltaLambdaUpper); + + // Apply the impulse to the body 2 + v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp)); + } + } + + // --------------- Motor --------------- // + + if (this.isMotorEnabled) { + + // Compute J*v for the motor + final float JvMotor = this.sliderAxisWorld.dot(v1) - this.sliderAxisWorld.dot(v2); + + // Compute the Lagrange multiplier lambda for the motor + final float maxMotorImpulse = this.maxMotorForce * raintSolverData.timeStep; + float deltaLambdaMotor = this.inverseMassMatrixMotor * (-JvMotor - this.motorSpeed); + final float lambdaTemp = this.impulseMotor; + this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse); + deltaLambdaMotor = this.impulseMotor - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the motor of body 1 + final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaMotor); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1)); + + // Compute the impulse P=J^T * lambda for the motor of body 2 + final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaMotor); + + // Apply the impulse to the body 2 + v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2)); + } + } + + @Override + public void warmstart(final ConstraintSolverData raintSolverData) { + // Get the velocities + final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + final float inverseMassBody1 = this.body1.massInverse; + final float inverseMassBody2 = this.body2.massInverse; + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1 + final float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit; + final Vector3f linearImpulseLimits = this.sliderAxisWorld.multiplyNew(impulseLimits); + + // Compute the impulse P=J^T * lambda for the motor raint of body 1 + final Vector3f impulseMotor = this.sliderAxisWorld.multiplyNew(this.impulseMotor); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 + final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.N2.multiplyNew(this.impulseTranslation.y)); + final Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.R1PlusUCrossN2.multiplyNew(this.impulseTranslation.y)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 + angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1)); + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1 + linearImpulseBody1.add(linearImpulseLimits); + angularImpulseBody1.add(this.R1PlusUCrossSliderAxis.multiplyNew(impulseLimits)); + + // Compute the impulse P=J^T * lambda for the motor raint of body 1 + linearImpulseBody1.add(impulseMotor); + + // Apply the impulse to the body 1 + v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1)); + w1.add(this.i1.multiplyNew(angularImpulseBody1)); + + // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 + final Vector3f linearImpulseBody2 = this.N1.multiplyNew(this.impulseTranslation.x).add(this.N2.multiplyNew(this.impulseTranslation.y)); + final Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(this.impulseTranslation.x).add(this.R2CrossN2.multiplyNew(this.impulseTranslation.y)); + + // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 + angularImpulseBody2.add(this.impulseRotation); + + // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2 + linearImpulseBody2.less(linearImpulseLimits); + angularImpulseBody2.add(this.R2CrossSliderAxis.multiplyNew(-impulseLimits)); + + // Compute the impulse P=J^T * lambda for the motor raint of body 2 + linearImpulseBody2.add(impulseMotor.multiplyNew(-1)); + + // Apply the impulse to the body 2 + v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2)); + w2.add(this.i2.multiplyNew(angularImpulseBody2)); + } + +} diff --git a/src/org/atriasoft/ephysics/constraint/SliderJointInfo.java b/src/org/atriasoft/ephysics/constraint/SliderJointInfo.java new file mode 100644 index 0000000..617d2bd --- /dev/null +++ b/src/org/atriasoft/ephysics/constraint/SliderJointInfo.java @@ -0,0 +1,89 @@ +package org.atriasoft.ephysics.constraint; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * This structure is used to gather the information needed to create a slider + * joint. This structure will be used to create the actual slider joint. + */ +public class SliderJointInfo extends JointInfo { + + public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + public Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates) + public boolean isLimitEnabled; //!< True if the slider limits are enabled + public boolean isMotorEnabled; //!< True if the slider motor is enabled + public float minTranslationLimit; //!< Mininum allowed translation if limits are enabled + public float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled + public float motorSpeed; //!< Motor speed + public float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed + + /** + * Constructor without limits and without motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space + * @param _initSliderAxisWorldSpace The initial slider axis in world-space + */ + public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace) { + super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.sliderAxisWorldSpace = _initSliderAxisWorldSpace; + this.isLimitEnabled = false; + this.isMotorEnabled = false; + this.minTranslationLimit = -1.0f; + this.maxTranslationLimit = 1.0f; + this.motorSpeed = 0; + this.maxMotorForce = 0; + + } + + /** + * Constructor with limits and no motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space + * @param _initSliderAxisWorldSpace The initial slider axis in world-space + * @param _initMinTranslationLimit The initial minimum translation limit (in meters) + * @param _initMaxTranslationLimit The initial maximum translation limit (in meters) + */ + public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace, + final float _initMinTranslationLimit, final float _initMaxTranslationLimit) { + super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.sliderAxisWorldSpace = _initSliderAxisWorldSpace; + this.isLimitEnabled = true; + this.isMotorEnabled = false; + this.minTranslationLimit = _initMinTranslationLimit; + this.maxTranslationLimit = _initMaxTranslationLimit; + this.motorSpeed = 0; + this.maxMotorForce = 0; + + } + + /** + * Constructor with limits and motor + * @param _rigidBody1 The first body of the joint + * @param _rigidBody2 The second body of the joint + * @param _initAnchorPointWorldSpace The initial anchor point in world-space + * @param _initSliderAxisWorldSpace The initial slider axis in world-space + * @param _initMinTranslationLimit The initial minimum translation limit (in meters) + * @param _initMaxTranslationLimit The initial maximum translation limit (in meters) + * @param _initMotorSpeed The initial speed of the joint motor (in meters per second) + * @param _initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters) + */ + public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace, + final float _initMinTranslationLimit, final float _initMaxTranslationLimit, final float _initMotorSpeed, final float _initMaxMotorForce) { + super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT); + this.anchorPointWorldSpace = _initAnchorPointWorldSpace; + this.sliderAxisWorldSpace = _initSliderAxisWorldSpace; + this.isLimitEnabled = true; + this.isMotorEnabled = true; + this.minTranslationLimit = _initMinTranslationLimit; + this.maxTranslationLimit = _initMaxTranslationLimit; + this.motorSpeed = _initMotorSpeed; + this.maxMotorForce = _initMaxMotorForce; + + } +} diff --git a/src/org/atriasoft/ephysics/engine/CollisionCallback.java b/src/org/atriasoft/ephysics/engine/CollisionCallback.java new file mode 100644 index 0000000..7b78e50 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/CollisionCallback.java @@ -0,0 +1,17 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.ephysics.constraint.ContactPointInfo; + +/** + * This class can be used to register a callback for collision test queries. + * You should implement your own class inherited from this one and implement + * the notifyRaycastHit() method. This method will be called for each ProxyShape + * that is hit by the ray. + */ +public interface CollisionCallback { + /** + * This method will be called for contact. + * @param _contactPointInfo Contact information property. + */ + void notifyContact(ContactPointInfo _contactPointInfo); +} diff --git a/src/org/atriasoft/ephysics/engine/CollisionWorld.java b/src/org/atriasoft/ephysics/engine/CollisionWorld.java new file mode 100644 index 0000000..bcbdcb1 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/CollisionWorld.java @@ -0,0 +1,254 @@ +package org.atriasoft.ephysics.engine; + +import java.util.ArrayList; +import java.util.List; + +import org.atriasoft.ephysics.RaycastCallback; +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.collision.CollisionDetection; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.broadphase.DTree; +import org.atriasoft.ephysics.collision.narrowphase.CollisionDispatch; +import org.atriasoft.ephysics.collision.shapes.AABB; +import org.atriasoft.ephysics.mathematics.Ray; +import org.atriasoft.ephysics.mathematics.Set; +import org.atriasoft.etk.math.Transform3D; + +/** + * This class represent a world where it is possible to move bodies + * by hand and to test collision between each other. In this kind of + * world, the bodies movement is not computed using the laws of physics. + */ +public class CollisionWorld { + + public CollisionDetection collisionDetection; //!< Reference to the collision detection + protected Set bodies = new Set(Set.getCollisionBodyCoparator()); //!< All the bodies (rigid and soft) of the world + protected int currentBodyID; //!< Current body ID + protected List freeBodiesIDs = new ArrayList<>(); //!< List of free ID for rigid bodies + public EventListener eventListener; //!< Pointer to an event listener object + /// Return the next available body ID + + /// Constructor + public CollisionWorld() { + this.collisionDetection = new CollisionDetection(this); + this.currentBodyID = 0; + this.eventListener = null; + } + + int computeNextAvailableBodyID() { + // Compute the body ID + int bodyID; + if (!this.freeBodiesIDs.isEmpty()) { + bodyID = this.freeBodiesIDs.get(0); + this.freeBodiesIDs.remove(bodyID); + } else { + bodyID = this.currentBodyID; + this.currentBodyID++; + } + return bodyID; + } + + /* + public Set::Iterator getBodiesEndIterator() { + return this.bodies.end(); + } + */ + /** + * Create a collision body and add it to the world + * @param transform Transform3Dation mapping the local-space of the body to world-space + * @return A pointer to the body that has been created in the world + */ + public CollisionBody createCollisionBody(final Transform3D transform) { + // Get the next available body ID + final int bodyID = computeNextAvailableBodyID(); + // Largest index cannot be used (it is used for invalid index) + assert (bodyID < Integer.MAX_VALUE); + // Create the collision body + final CollisionBody collisionBody = new CollisionBody(transform, this, bodyID); + assert (collisionBody != null); + // Add the collision body to the world + this.bodies.add(collisionBody); + // Return the pointer to the rigid body + return collisionBody; + } + + /** + * Destroy a collision body + * @param collisionBody Pointer to the body to destroy + */ + public void destroyCollisionBody(CollisionBody collisionBody) { + // Remove all the collision shapes of the body + collisionBody.removeAllCollisionShapes(); + // Add the body ID to the list of free IDs + this.freeBodiesIDs.add(collisionBody.getID()); + // Remove the collision body from the list of bodies + this.bodies.erase(collisionBody); + collisionBody = null; + } + + /** + * Get an iterator to the beginning of the bodies of the physics world + * @return An starting iterator to the set of bodies of the world + */ + /* + public Set::Iterator getBodiesBeginIterator() { + return this.bodies.begin(); + } + */ + /** + * Get an iterator to the end of the bodies of the physics world + * @return An ending iterator to the set of bodies of the world + */ + + public CollisionDetection getCollisionDetection() { + return this.collisionDetection; + } + + /** + * Ray cast method + * @param _ray Ray to use for raycasting + * @param _raycastCallback Pointer to the class with the callback method + * @param _raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted + */ + public void raycast(final Ray _ray, final RaycastCallback _raycastCallback) { + raycast(_ray, _raycastCallback, 0xFFFF); + } + + public void raycast(final Ray _ray, final RaycastCallback _raycastCallback, final int _raycastWithCategoryMaskBits) { + this.collisionDetection.raycast(_raycastCallback, _ray, _raycastWithCategoryMaskBits); + } + + /// Reset all the contact manifolds linked list of each body + void resetContactManifoldListsOfBodies() { + // For each rigid body of the world + for (final CollisionBody it : this.bodies) { + // Reset the contact manifold list of the body + it.resetContactManifoldsList(); + } + } + + /** + * Set the collision dispatch configuration + * This can be used to replace default collision detection algorithms by your + * custom algorithm for instance. + * @param _CollisionDispatch Pointer to a collision dispatch object describing + * which collision detection algorithm to use for two given collision shapes + */ + public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) { + this.collisionDetection.setCollisionDispatch(_collisionDispatch); + } + + /** + * Test if the AABBs of two bodies overlap + * @param _body1 Pointer to the first body to test + * @param _body2 Pointer to the second body to test + * @return True if the AABBs of the two bodies overlap and false otherwise + */ + public boolean testAABBOverlap(final CollisionBody _body1, final CollisionBody _body2) { + // If one of the body is not active, we return no overlap + if (!_body1.isActive() || !_body2.isActive()) { + return false; + } + // Compute the AABBs of both bodies + final AABB body1AABB = _body1.getAABB(); + final AABB body2AABB = _body2.getAABB(); + // Return true if the two AABBs overlap + return body1AABB.testCollision(body2AABB); + } + + /** + * Test if the AABBs of two proxy shapes overlap + * @param _shape1 Pointer to the first proxy shape to test + * @param _shape2 Pointer to the second proxy shape to test + */ + public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) { + return this.collisionDetection.testAABBOverlap(_shape1, _shape2); + } + + /** + * Test and report collisions between two bodies + * @param _body1 Pointer to the first body to test + * @param _body2 Pointer to the second body to test + * @param _callback Pointer to the object with the callback method + */ + public void testCollision(final CollisionBody _body1, final CollisionBody _body2, final CollisionCallback _callback) { + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + for (ProxyShape shape = _body1.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes1.add(shape.broadPhaseID); + } + final Set shapes2 = new Set(Set.getDTreeCoparator()); + for (ProxyShape shape = _body2.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes2.add(shape.broadPhaseID); + } + // Perform the collision detection and report contacts + this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2); + } + + /** + * Test and report collisions between a body and all the others bodies of the world. + * @param _body Pointer to the first body to test + * @param _callback Pointer to the object with the callback method + */ + public void testCollision(final CollisionBody _body, final CollisionCallback _callback) { + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + // For each shape of the body + for (ProxyShape shape = _body.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes1.add(shape.broadPhaseID); + } + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, emptySet); + } + + /** + * Test and report collisions between all shapes of the world + * @param _callback Pointer to the object with the callback method + */ + public void testCollision(final CollisionCallback _callback) { + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.testCollisionBetweenShapes(_callback, emptySet, emptySet); + } + + /** + * Test and report collisions between a given shape and all the others shapes of the world. + * @param _shape Pointer to the proxy shape to test + * @param _callback Pointer to the object with the callback method + */ + public void testCollision(final ProxyShape _shape, final CollisionCallback _callback) { + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + // Create the sets of shapes + final Set shapes = new Set(Set.getDTreeCoparator()); + shapes.add(_shape.broadPhaseID); + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.testCollisionBetweenShapes(_callback, shapes, emptySet); + } + + /** + * Test and report collisions between two given shapes + * @param _shape1 Pointer to the first proxy shape to test + * @param _shape2 Pointer to the second proxy shape to test + * @param _callback Pointer to the object with the callback method + */ + public void testCollision(final ProxyShape _shape1, final ProxyShape _shape2, final CollisionCallback _callback) { + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + shapes1.add(_shape1.broadPhaseID); + final Set shapes2 = new Set(Set.getDTreeCoparator()); + shapes2.add(_shape2.broadPhaseID); + // Perform the collision detection and report contacts + this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2); + } +} diff --git a/src/org/atriasoft/ephysics/engine/ConstraintSolver.java b/src/org/atriasoft/ephysics/engine/ConstraintSolver.java new file mode 100644 index 0000000..af837f4 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/ConstraintSolver.java @@ -0,0 +1,163 @@ +package org.atriasoft.ephysics.engine; + +import java.util.List; +import java.util.Map; + +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; +import org.atriasoft.ephysics.constraint.Joint; + +/** + * This class represents the raint solver that is used to solve raints between + * the rigid bodies. The raint solver is based on the "Sequential Impulse" technique + * described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + * + * A raint between two bodies is represented by a function C(x) which is equal to zero + * when the raint is satisfied. The condition C(x)=0 describes a valid position and the + * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + * the Jacobian matrix of the raint, v is a vector that contains the velocity of both + * bodies and b is the raint bias. We are looking for a force Fc that will act on the + * bodies to keep the raint satisfied. Note that from the work principle, we have + * Fc = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + * Lagrange multiplier. Therefore, finding the force Fc is equivalent to finding the Lagrange + * multiplier lambda. + + * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + * body to change its velocity. The idea of the Sequential Impulse technique is to apply + * impulses to bodies of each raints in order to keep the raint satisfied. + * + * --- Step 1 --- + * + * First, we integrate the applied force Fa acting of each rigid body (like gravity, ...) and + * we obtain some new velocities v2' that tends to violate the raints. + * + * v2' = v1 + dt * M^-1 * Fa + * + * where M is a matrix that contains mass and inertia tensor information. + * + * --- Step 2 --- + * + * During the second step, we iterate over all the raints for a certain number of + * iterations and for each raint we compute the impulse to apply to the bodies needed + * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + * M * deltaV = Pc where M is the mass of the body, deltaV is the difference of velocity and + * Pc is the raint impulse to apply to the body. Therefore, we have + * v2 = v2' + M^-1 * Pc. For each raint, we can compute the Lagrange multiplier lambda + * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the + * Lagrange multiplier lambda, we can compute the impulse Pc = J^t * lambda * dt to apply to + * the bodies to satisfy the raint. + * + * --- Step 3 --- + * + * In the third step, we integrate the new position x2 of the bodies using the new velocities + * v2 computed in the second step with : x2 = x1 + dt * v2. + * + * Note that in the following code (as it is also explained in the slides from Erin Catto), + * the value lambda is not only the lagrange multiplier but is the multiplication of the + * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + * lambda, we mean (lambda * dt). + * + * We are using the accumulated impulse technique that is also described in the slides from + * Erin Catto. + * + * We are also using warm starting. The idea is to warm start the solver at the beginning of + * each step by applying the last impulstes for the raints that we already existing at the + * previous step. This allows the iterative solver to converge faster towards the solution. + * + * For contact raints, we are also using split impulses so that the position correction + * that uses Baumgarte stabilization does not change the momentum of the bodies. + * + * There are two ways to apply the friction raints. Either the friction raints are + * applied at each contact point or they are applied only at the center of the contact manifold + * between two bodies. If we solve the friction raints at each contact point, we need + * two raints (two tangential friction directions) and if we solve the friction + * raints at the center of the contact manifold, we need two raints for tangential + * friction but also another twist friction raint to prevent spin of the body around the + * contact manifold center. + */ +public class ConstraintSolver { + + private final Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array + private float timeStep; //!< Current time step + private boolean isWarmStartingActive; //!< True if the warm starting of the solver is active + private final ConstraintSolverData raintSolverData; //!< Constraint solver data used to initialize and solve the raints + /// Constructor + + public ConstraintSolver(final Map mapBodyToVelocityIndex) { + this.mapBodyToConstrainedVelocityIndex = mapBodyToVelocityIndex; + this.isWarmStartingActive = true; + this.raintSolverData = new ConstraintSolverData(mapBodyToVelocityIndex); + + } + + /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active + public boolean getIsNonLinearGaussSeidelPositionCorrectionActive() { + return this.isWarmStartingActive; + } + + /// Initialize the raint solver for a given island + public void initializeForIsland(final float dt, final Island island) { + assert (island != null); + assert (island.getNbBodies() > 0); + assert (island.getNbJoints() > 0); + // Set the current time step + this.timeStep = dt; + // Initialize the raint solver data used to initialize and solve the raints + this.raintSolverData.timeStep = this.timeStep; + this.raintSolverData.isWarmStartingActive = this.isWarmStartingActive; + // For each joint of the island + final List joints = island.getJoints(); + for (int iii = 0; iii < island.getNbJoints(); ++iii) { + // Initialize the raint before solving it + joints.get(iii).initBeforeSolve(this.raintSolverData); + // Warm-start the raint if warm-starting is enabled + if (this.isWarmStartingActive) { + joints.get(iii).warmstart(this.raintSolverData); + } + } + } + + /// Set the rained positions/orientations arrays + public void setConstrainedPositionsArrays(final Vector3f[] rainedPositions, final Quaternion[] rainedOrientations) { + assert (rainedPositions != null); + assert (rainedOrientations != null); + this.raintSolverData.positions = rainedPositions; + this.raintSolverData.orientations = rainedOrientations; + } + + /// Set the rained velocities arrays + public void setConstrainedVelocitiesArrays(final Vector3f[] rainedLinearVelocities, final Vector3f[] rainedAngularVelocities) { + assert (rainedLinearVelocities != null); + assert (rainedAngularVelocities != null); + this.raintSolverData.linearVelocities = rainedLinearVelocities; + this.raintSolverData.angularVelocities = rainedAngularVelocities; + } + + /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. + public void setIsNonLinearGaussSeidelPositionCorrectionActive(final boolean isActive) { + this.isWarmStartingActive = isActive; + } + + /// Solve the position raints + public void solvePositionConstraints(final Island island) { + assert (island != null); + assert (island.getNbJoints() > 0); + final List joints = island.getJoints(); + for (int iii = 0; iii < joints.size(); ++iii) { + joints.get(iii).solvePositionConstraint(this.raintSolverData); + } + } + + /// Solve the raints + public void solveVelocityConstraints(final Island island) { + assert (island != null); + assert (island.getNbJoints() > 0); + // For each joint of the island + final List joints = island.getJoints(); + for (int iii = 0; iii < island.getNbJoints(); ++iii) { + joints.get(iii).solveVelocityConstraint(this.raintSolverData); + } + } +} diff --git a/src/org/atriasoft/ephysics/engine/ConstraintSolverData.java b/src/org/atriasoft/ephysics/engine/ConstraintSolverData.java new file mode 100644 index 0000000..3f75cbc --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/ConstraintSolverData.java @@ -0,0 +1,28 @@ +package org.atriasoft.ephysics.engine; + +import java.util.Map; + +import org.atriasoft.etk.math.Quaternion; +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.body.RigidBody; + +/** + * This structure contains data from the raint solver that are used to solve + * each joint raint. + */ +public class ConstraintSolverData { + + public float timeStep; //!< Current time step of the simulation + public Vector3f[] linearVelocities = null; //!< Array with the bodies linear velocities + public Vector3f[] angularVelocities = null; //!< Array with the bodies angular velocities + public Vector3f[] positions = null; //!< Reference to the bodies positions + public Quaternion[] orientations = null; //!< Reference to the bodies orientations + public Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array + public boolean isWarmStartingActive; //!< True if warm starting of the solver is active + + public ConstraintSolverData(final Map refMapBodyToConstrainedVelocityIndex) { + this.mapBodyToConstrainedVelocityIndex = refMapBodyToConstrainedVelocityIndex; + + } +} diff --git a/src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java b/src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java new file mode 100644 index 0000000..dae63b6 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java @@ -0,0 +1,53 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.ephysics.collision.ContactManifold; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * Contact solver internal data structure to store all the information relative to a contact manifold. + */ +public class ContactManifoldSolver { + int indexBody1; //!< Index of body 1 in the raint solver + int indexBody2; //!< Index of body 2 in the raint solver + float massInverseBody1; //!< Inverse of the mass of body 1 + float massInverseBody2; //!< Inverse of the mass of body 2 + Matrix3f inverseInertiaTensorBody1 = new Matrix3f(); //!< Inverse inertia tensor of body 1 + Matrix3f inverseInertiaTensorBody2 = new Matrix3f(); //!< Inverse inertia tensor of body 2 + ContactPointSolver[] contacts = new ContactPointSolver[ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact point raints + int nbContacts; //!< Number of contact points + boolean isBody1DynamicType; //!< True if the body 1 is of type dynamic + boolean isBody2DynamicType; //!< True if the body 2 is of type dynamic + float restitutionFactor; //!< Mix of the restitution factor for two bodies + float frictionCoefficient; //!< Mix friction coefficient for the two bodies + float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies + ContactManifold externalContactManifold; //!< Pointer to the external contact manifold + // - Variables used when friction raints are apply at the center of the manifold-// + Vector3f normal = new Vector3f(0, 0, 0); //!< Average normal vector of the contact manifold + Vector3f frictionPointBody1 = new Vector3f(0, 0, 0); //!< Point on body 1 where to apply the friction raints + Vector3f frictionPointBody2 = new Vector3f(0, 0, 0); //!< Point on body 2 where to apply the friction raints + Vector3f r1Friction = new Vector3f(0, 0, 0); //!< R1 vector for the friction raints + Vector3f r2Friction = new Vector3f(0, 0, 0); //!< R2 vector for the friction raints + Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector + Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector + Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector + Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector + float inverseFriction1Mass; //!< Matrix K for the first friction raint + float inverseFriction2Mass; //!< Matrix K for the second friction raint + float inverseTwistFrictionMass; //!< Matrix K for the twist friction raint + Matrix3f inverseRollingResistance = new Matrix3f(); //!< Matrix K for the rolling resistance raint + Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction direction at contact manifold center + Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction direction at contact manifold center + Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old 1st friction direction at contact manifold center + Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old 2nd friction direction at contact manifold center + float friction1Impulse; //!< First friction direction impulse at manifold center + float friction2Impulse; //!< Second friction direction impulse at manifold center + float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center + Vector3f rollingResistanceImpulse = new Vector3f(0, 0, 0); //!< Rolling resistance impulse + + public ContactManifoldSolver() { + for (int iii = 0; iii < ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD; iii++) { + this.contacts[iii] = new ContactPointSolver(); + } + } +} diff --git a/src/org/atriasoft/ephysics/engine/ContactPointSolver.java b/src/org/atriasoft/ephysics/engine/ContactPointSolver.java new file mode 100644 index 0000000..379bb7e --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/ContactPointSolver.java @@ -0,0 +1,37 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.etk.math.Vector3f; + +import org.atriasoft.ephysics.constraint.ContactPoint; + +/** + * Contact solver internal data structure that to store all the + * information relative to a contact point + */ +public class ContactPointSolver { + float penetrationImpulse; //!< Accumulated normal impulse + float friction1Impulse; //!< Accumulated impulse in the 1st friction direction + float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction + float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction + Vector3f rollingResistanceImpulse = new Vector3f(0, 0, 0); //!< Accumulated rolling resistance impulse + Vector3f normal = new Vector3f(0, 0, 0); //!< Normal vector of the contact + Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction vector in the tangent plane + Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction vector in the tangent plane + Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old first friction vector in the tangent plane + Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old second friction vector in the tangent plane + Vector3f r1 = new Vector3f(0, 0, 0); //!< Vector from the body 1 center to the contact point + Vector3f r2 = new Vector3f(0, 0, 0); //!< Vector from the body 2 center to the contact point + Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector + Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector + Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector + Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector + Vector3f r1CrossN = new Vector3f(0, 0, 0); //!< Cross product of r1 with the contact normal + Vector3f r2CrossN = new Vector3f(0, 0, 0); //!< Cross product of r2 with the contact normal + float penetrationDepth; //!< Penetration depth + float restitutionBias; //!< Velocity restitution bias + float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration + float inverseFriction1Mass; //!< Inverse of the matrix K for the 1st friction + float inverseFriction2Mass; //!< Inverse of the matrix K for the 2nd friction + boolean isRestingContact; //!< True if the contact was existing last time step + ContactPoint externalContact; //!< Pointer to the external contact +} diff --git a/src/org/atriasoft/ephysics/engine/ContactSolver.java b/src/org/atriasoft/ephysics/engine/ContactSolver.java new file mode 100644 index 0000000..677afea --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/ContactSolver.java @@ -0,0 +1,1109 @@ +package org.atriasoft.ephysics.engine; + +import java.util.ArrayList; +import java.util.List; +import java.util.Map; + +import org.atriasoft.ephysics.Configuration; +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.body.RigidBody; +import org.atriasoft.ephysics.collision.ContactManifold; +import org.atriasoft.ephysics.constraint.ContactPoint; +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.FMath; +import org.atriasoft.etk.math.Matrix3f; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents the contact solver that is used to solve rigid bodies contacts. + * The raint solver is based on the "Sequential Impulse" technique described by + * Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + * + * A raint between two bodies is represented by a function C(x) which is equal to zero + * when the raint is satisfied. The condition C(x)=0 describes a valid position and the + * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + * the Jacobian matrix of the raint, v is a vector that contains the velocity of both + * bodies and b is the raint bias. We are looking for a force F_c that will act on the + * bodies to keep the raint satisfied. Note that from the work principle, we have + * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + * multiplier lambda. + * + * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + * body to change its velocity. The idea of the Sequential Impulse technique is to apply + * impulses to bodies of each raints in order to keep the raint satisfied. + * + * --- Step 1 --- + * + * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + * we obtain some new velocities v2' that tends to violate the raints. + * + * v2' = v1 + dt * M^-1 * F_a + * + * where M is a matrix that contains mass and inertia tensor information. + * + * --- Step 2 --- + * + * During the second step, we iterate over all the raints for a certain number of + * iterations and for each raint we compute the impulse to apply to the bodies needed + * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + * P_c is the raint impulse to apply to the body. Therefore, we have + * v2 = v2' + M^-1 * P_c. For each raint, we can compute the Lagrange multiplier lambda + * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the + * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + * the bodies to satisfy the raint. + * + * --- Step 3 --- + * + * In the third step, we integrate the new position x2 of the bodies using the new velocities + * v2 computed in the second step with : x2 = x1 + dt * v2. + * + * Note that in the following code (as it is also explained in the slides from Erin Catto), + * the value lambda is not only the lagrange multiplier but is the multiplication of the + * Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + * lambda, we mean (lambda * dt). + * + * We are using the accumulated impulse technique that is also described in the slides from + * Erin Catto. + * + * We are also using warm starting. The idea is to warm start the solver at the beginning of + * each step by applying the last impulstes for the raints that we already existing at the + * previous step. This allows the iterative solver to converge faster towards the solution. + * + * For contact raints, we are also using split impulses so that the position correction + * that uses Baumgarte stabilization does not change the momentum of the bodies. + * + * There are two ways to apply the friction raints. Either the friction raints are + * applied at each contact point or they are applied only at the center of the contact manifold + * between two bodies. If we solve the friction raints at each contact point, we need + * two raints (two tangential friction directions) and if we solve the friction + * raints at the center of the contact manifold, we need two raints for tangential + * friction but also another twist friction raint to prevent spin of the body around the + * contact manifold center. + */ +public class ContactSolver { + + private static float BETA = 0.2f; //!< Beta value for the penetration depth position correction without split impulses + private static float BETA_SPLIT_IMPULSE = 0.2f; //!< Beta value for the penetration depth position correction with split impulses + private static float SLOP = 0.01f; //!< Slop distance (allowed penetration distance between bodies) + private Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + private Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + private float timeStep; //!< Current time step + private List contactConstraints = new ArrayList<>(); //!< Contact raints + private Vector3f[] linearVelocities; //!< Array of linear velocities + private Vector3f[] angularVelocities; //!< Array of angular velocities + private final Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array + private final boolean isWarmStartingActive; //!< True if the warm starting of the solver is active + private boolean isSplitImpulseActive; //!< True if the split impulse position correction is active + private boolean isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction raints at the contact manifold center only instead of 2 friction raints at each contact point + + /** + * Constructor + * @param _mapBodyToVelocityIndex + */ + public ContactSolver(final Map _mapBodyToVelocityIndex) { + this.splitLinearVelocities = null; + this.splitAngularVelocities = null; + this.linearVelocities = null; + this.angularVelocities = null; + this.mapBodyToConstrainedVelocityIndex = _mapBodyToVelocityIndex; + this.isWarmStartingActive = false; // TODO default true + this.isSplitImpulseActive = true; // TODO default true + this.isSolveFrictionAtContactManifoldCenterActive = true; // TODO default true + } + + /** + * Apply an impulse to the two bodies of a raint + * @param _impulse Impulse to apply + * @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); + // Update the velocities of the body 1 by applying the impulse P + this.linearVelocities[_manifold.indexBody1].add(_impulse.linearImpulseBody1.multiplyNew(_manifold.massInverseBody1)); + //Log.info(" -- this.angularVelocities[_manifold.indexBody1]=" + this.angularVelocities[_manifold.indexBody1]); + this.angularVelocities[_manifold.indexBody1].add(_manifold.inverseInertiaTensorBody1.multiplyNew(_impulse.angularImpulseBody1)); + //Log.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].add(_impulse.linearImpulseBody2.multiplyNew(_manifold.massInverseBody2)); + //Log.info(" -- this.angularVelocities[_manifold.indexBody2]=" + this.angularVelocities[_manifold.indexBody2]); + this.angularVelocities[_manifold.indexBody2].add(_manifold.inverseInertiaTensorBody2.multiplyNew(_impulse.angularImpulseBody2)); + //Log.info(" -- this.angularVelocities[_manifold.indexBody2]=" + this.angularVelocities[_manifold.indexBody2]); + } + + /** + * Apply an impulse to the two bodies of a raint + * @param _impulse Impulse to apply + * @param _manifold Constraint to apply the impulse + */ + private void applySplitImpulse(final Impulse _impulse, final ContactManifoldSolver _manifold) { + // Update the velocities of the body 1 by applying the impulse P + this.splitLinearVelocities[_manifold.indexBody1].add(_impulse.linearImpulseBody1.multiplyNew(_manifold.massInverseBody1)); + this.splitAngularVelocities[_manifold.indexBody1].add(_manifold.inverseInertiaTensorBody1.multiplyNew(_impulse.angularImpulseBody1)); + // Update the velocities of the body 1 by applying the impulse P + this.splitLinearVelocities[_manifold.indexBody2].add(_impulse.linearImpulseBody2.multiplyNew(_manifold.massInverseBody2)); + this.splitAngularVelocities[_manifold.indexBody2].add(_manifold.inverseInertiaTensorBody2.multiplyNew(_impulse.angularImpulseBody2)); + } + + /** + * Clean up the raint solver + */ + public void cleanup() { + this.contactConstraints.clear(); + } + + /** + * Compute the first friction raint impulse + * @param _deltaLambda Ratio to apply at the calculation. + * @param _contactPoint Contact point property + * @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); + return new Impulse(_contactPoint.frictionVector1.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossT1.multiplyNew(-1).multiply(_deltaLambda), + _contactPoint.frictionVector1.multiplyNew(_deltaLambda), _contactPoint.r2CrossT1.multiplyNew(_deltaLambda)); + } + + /** + * Compute the second friction raint impulse + * @param _deltaLambda Ratio to apply at the calculation. + * @param _contactPoint Contact point property + * @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); + return new Impulse(_contactPoint.frictionvec2.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossT2.multiplyNew(-1).multiplyNew(_deltaLambda), + _contactPoint.frictionvec2.multiplyNew(_deltaLambda), _contactPoint.r2CrossT2.multiplyNew(_deltaLambda)); + } + + /** + * Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + * plane for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. + * @param _deltaVelocity Velocity ratio (with the delta time step) + * @param[in,out] _contactPoint Contact point property + */ + private void computeFrictionVectors(final Vector3f _deltaVelocity, final ContactManifoldSolver _contactManifold) { + assert (_contactManifold.normal.length() > 0.0f); + // Compute the velocity difference vector in the tangential plane + final Vector3f normalVelocity = _contactManifold.normal.multiplyNew(_deltaVelocity.dot(_contactManifold.normal)); + final Vector3f tangentVelocity = _deltaVelocity.lessNew(normalVelocity); + // If the velocty difference in the tangential plane is not zero + final float lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > Constant.FLOAT_EPSILON) { + // Compute the first friction vector in the direction of the tangent + // velocity difference + _contactManifold.frictionVector1 = tangentVelocity.divideNew(lengthTangenVelocity); + //Log.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); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + _contactManifold.frictionvec2 = _contactManifold.normal.cross(_contactManifold.frictionVector1).safeNormalizeNew(); + } + + /** + * Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + * plane for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. + * @param _deltaVelocity Velocity ratio (with the delta time step) + * @param[in,out] _contactPoint Contact point property + */ + 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); + // Compute the velocity difference vector in the tangential plane + final Vector3f normalVelocity = _contactPoint.normal.multiplyNew(_deltaVelocity.dot(_contactPoint.normal)); + //Log.error("===3==>>>>>> normalVelocity=" + normalVelocity); + final Vector3f tangentVelocity = _deltaVelocity.lessNew(normalVelocity); + //Log.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)); + if (lengthTangenVelocity > Constant.FLOAT_EPSILON) { + // Compute the first friction vector in the direction of the tangent + // velocity difference + _contactPoint.frictionVector1 = tangentVelocity.divideNew(lengthTangenVelocity); + //Log.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); + } + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + _contactPoint.frictionvec2 = _contactPoint.normal.cross(_contactPoint.frictionVector1).safeNormalizeNew(); + } + + /** + * Compute the mixed friction coefficient from the friction coefficient of each body + * @param _body1 First body to compute + * @param _body2 Second body to compute + * @return Mixed friction coefficient + */ + private float computeMixedFrictionCoefficient(final RigidBody _body1, final RigidBody _body2) { + // Use the geometric mean to compute the mixed friction coefficient + return FMath.sqrt(_body1.getMaterial().getFrictionCoefficient() * _body2.getMaterial().getFrictionCoefficient()); + } + + /** + * Compute the collision restitution factor from the restitution factor of each body + * @param _body1 First body to compute + * @param _body2 Second body to compute + * @return Collision restitution factor + */ + 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)); + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; + } + + /** + * Compute the mixed rolling resistance factor between two bodies + * @param _body1 First body to compute + * @param _body2 Second body to compute + * @return Mixed rolling resistance + */ + private float computeMixedRollingResistance(final RigidBody _body1, final RigidBody _body2) { + return 0.5f * (_body1.getMaterial().getRollingResistance() + _body2.getMaterial().getRollingResistance()); + } + + /** + * Compute a penetration raint impulse + * @param _deltaLambda Ratio to apply at the calculation. + * @param[in,out] _contactPoint Contact point property + * @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); + return new Impulse(_contactPoint.normal.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossN.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.normal.multiplyNew(_deltaLambda), + _contactPoint.r2CrossN.multiplyNew(_deltaLambda)); + } + + /** + * Initialize the contact raints before solving the system + */ + private void initializeContactConstraints() { + // For each contact raint + for (int ccc = 0; ccc < this.contactConstraints.size(); ccc++) { + //Log.warning("}}}} 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); + final Matrix3f I2 = manifold.inverseInertiaTensorBody2; + //Log.warning("}}}} I2=" + I2); + // If we solve the friction raints at the center of the contact manifold + if (this.isSolveFrictionAtContactManifoldCenterActive) { + manifold.normal = new Vector3f(0.0f, 0.0f, 0.0f); + //Log.error("]]] manifold.normal=" + manifold.normal); + } + //Log.warning("}}}} 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); + // For each contact point raint + for (int iii = 0; iii < manifold.nbContacts; iii++) { + //Log.warning("}}}} 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); + final ContactPoint externalContact = contactPoint.externalContact; + // Compute the velocity difference + final Vector3f deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); + //Log.warning("}}}} 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); + // Compute the inverse mass matrix K for the penetration raint + final float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(contactPoint.r1CrossN)).cross(contactPoint.r1)).dot(contactPoint.normal) + + ((I2.multiplyNew(contactPoint.r2CrossN)).cross(contactPoint.r2)).dot(contactPoint.normal); + //Log.warning("}}}} massPenetration=" + FMath.floatToString(massPenetration)); + if (massPenetration > 0.0f) { + contactPoint.inversePenetrationMass = 1.0f / massPenetration; + } + //Log.warning("}}}} 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); + if (!this.isSolveFrictionAtContactManifoldCenterActive) { + // Compute the friction vectors + computeFrictionVectors(deltaV, contactPoint); + //Log.warning("}}}} 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); + // Compute the inverse mass matrix K for the friction + // raints at each contact point + final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + + ((I1.multiplyNew(contactPoint.r1CrossT1)).cross(contactPoint.r1)).dot(contactPoint.frictionVector1) + + ((I2.multiplyNew(contactPoint.r2CrossT1)).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); + final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(contactPoint.r1CrossT2)).cross(contactPoint.r1)).dot(contactPoint.frictionvec2) + + ((I2.multiplyNew(contactPoint.r2CrossT2)).cross(contactPoint.r2)).dot(contactPoint.frictionvec2); + if (friction1Mass > 0.0f) { + contactPoint.inverseFriction1Mass = 1.0f / friction1Mass; + } + if (friction2Mass > 0.0f) { + contactPoint.inverseFriction2Mass = 1.0f / friction2Mass; + } + } + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // 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); + 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.RESTITUTION_VELOCITY_THRESHOLD=" + FMath.floatToString(Configuration.RESTITUTION_VELOCITY_THRESHOLD)); + //Log.warning("}}}}+++++++++++++++++++++ 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)); + } + // If the warm starting of the contact solver is active + if (this.isWarmStartingActive) { + // Get the cached accumulated impulses from the previous step + contactPoint.penetrationImpulse = externalContact.getPenetrationImpulse(); + contactPoint.friction1Impulse = externalContact.getFrictionImpulse1(); + contactPoint.friction2Impulse = externalContact.getFrictionImpulse2(); + contactPoint.rollingResistanceImpulse = externalContact.getRollingResistanceImpulse(); + } + // Initialize the split impulses to zero + 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); + manifold.normal.add(contactPoint.normal); + //Log.error("]]] manifold.normal=" + manifold.normal); + } + } + // Compute the inverse K matrix for the rolling resistance raint + manifold.inverseRollingResistance.setZero(); + if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { + manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1.addNew(manifold.inverseInertiaTensorBody2); + manifold.inverseRollingResistance = manifold.inverseRollingResistance.inverseNew(); + } + // If we solve the friction raints at the center of the contact manifold + if (this.isSolveFrictionAtContactManifoldCenterActive) { + //Log.error("]]] manifold.normal=" + manifold.normal); + manifold.normal.normalize(); + //Log.error("]]] manifold.normal=" + manifold.normal); + final Vector3f deltaVFrictionPoint = v2.addNew(w2.cross(manifold.r2Friction)).less(v1).less(w1.cross(manifold.r1Friction)); + //Log.error("]]] deltaVFrictionPoint=" + deltaVFrictionPoint); + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, manifold); + // Compute the inverse mass matrix K for the friction raints at the center of + // the contact manifold + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + 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); + final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(manifold.r1CrossT1)).cross(manifold.r1Friction)).dot(manifold.frictionVector1) + + ((I2.multiplyNew(manifold.r2CrossT1)).cross(manifold.r2Friction)).dot(manifold.frictionVector1); + //Log.error("]]] friction1Mass=" + FMath.floatToString(friction1Mass)); + final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(manifold.r1CrossT2)).cross(manifold.r1Friction)).dot(manifold.frictionvec2) + + ((I2.multiplyNew(manifold.r2CrossT2)).cross(manifold.r2Friction)).dot(manifold.frictionvec2); + //Log.error("]]] friction2Mass=" + FMath.floatToString(friction2Mass)); + final float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1.multiplyNew(manifold.normal)) + + manifold.normal.dot(manifold.inverseInertiaTensorBody2.multiplyNew(manifold.normal)); + //Log.error("]]] frictionTwistMass=" + FMath.floatToString(frictionTwistMass)); + if (friction1Mass > 0.0f) { + manifold.inverseFriction1Mass = 1.0f / friction1Mass; + //Log.error("]]] manifold.inverseFriction1Mass=" + FMath.floatToString(manifold.inverseFriction1Mass)); + } + if (friction2Mass > 0.0f) { + manifold.inverseFriction2Mass = 1.0f / friction2Mass; + //Log.error("]]] manifold.inverseFriction2Mass=" + FMath.floatToString(manifold.inverseFriction2Mass)); + } + if (frictionTwistMass > 0.0f) { + manifold.inverseTwistFrictionMass = 1.0f / frictionTwistMass; + //Log.error("]]] manifold.inverseTwistFrictionMass=" + FMath.floatToString(manifold.inverseTwistFrictionMass)); + } + } + } + } + + /** + * Initialize the raint solver for a given island + * @param _dt Delta step time + * @param _island Island list property + */ + public void initializeForIsland(final float _dt, final Island _island) { + assert (_island != null); + assert (_island.getNbBodies() > 0); + assert (_island.getNbContactManifolds() > 0); + assert (this.splitLinearVelocities != null); + assert (this.splitAngularVelocities != null); + // Set the current time step + // TODO: optimize this... this is so ugly ... + this.timeStep = _dt; + { + this.contactConstraints = new ArrayList<>(_island.getNbContactManifolds()); + } + // this.contactConstraints.resize(_island.getNbContactManifolds()); + // For each contact manifold of the island + final List contactManifolds = _island.getContactManifold(); + for (int iii = 0; iii < _island.getNbContactManifolds(); ++iii) { + final ContactManifold externalManifold = contactManifolds.get(iii); + final ContactManifoldSolver internalManifold = new ContactManifoldSolver(); // TODO Note no real need to reallocate a new one ==> maybe reuse (done in C++ not in java ...) + this.contactConstraints.add(internalManifold); + + assert (externalManifold.getNbContactPoints() > 0); + // Get the two bodies of the contact + final RigidBody body1 = (RigidBody) (externalManifold.getContactPoint(0).getBody1()); + final RigidBody body2 = (RigidBody) (externalManifold.getContactPoint(0).getBody2()); + assert (body1 != null); + assert (body2 != null); + // Get the position of the two bodies + final Vector3f x1 = body1.centerOfMassWorld; + final Vector3f x2 = body2.centerOfMassWorld; + // Initialize the internal contact manifold structure using the external + // contact manifold + internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.get(body1); + internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.get(body2); + internalManifold.inverseInertiaTensorBody1 = body1.getInertiaTensorInverseWorld(); + internalManifold.inverseInertiaTensorBody2 = body2.getInertiaTensorInverseWorld(); + internalManifold.massInverseBody1 = body1.massInverse; + internalManifold.massInverseBody2 = body2.massInverse; + internalManifold.nbContacts = externalManifold.getNbContactPoints(); + internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + internalManifold.rollingResistanceFactor = computeMixedRollingResistance(body1, body2); + internalManifold.externalContactManifold = externalManifold; + internalManifold.isBody1DynamicType = body1.getType() == BodyType.DYNAMIC; + internalManifold.isBody2DynamicType = body2.getType() == BodyType.DYNAMIC; + // If we solve the friction raints at the center of the contact manifold + 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); + } + // For each contact point of the contact manifold + for (int ccc = 0; ccc < externalManifold.getNbContactPoints(); ++ccc) { + final ContactPointSolver contactPoint = internalManifold.contacts[ccc]; + // Get a contact point + final ContactPoint externalContact = externalManifold.getContactPoint(ccc); + // Get the contact point on the two bodies + final Vector3f p1 = externalContact.getWorldPointOnBody1(); + final Vector3f p2 = externalContact.getWorldPointOnBody2(); + contactPoint.externalContact = externalContact; + contactPoint.normal = externalContact.getNormal(); + contactPoint.r1 = p1.lessNew(x1); + contactPoint.r2 = p2.lessNew(x2); + contactPoint.penetrationDepth = externalContact.getPenetrationDepth(); + contactPoint.isRestingContact = externalContact.getIsRestingContact(); + externalContact.setIsRestingContact(true); + contactPoint.oldFrictionVector1 = externalContact.getFrictionVector1().clone(); + contactPoint.oldFrictionvec2 = externalContact.getFrictionvec2().clone(); + contactPoint.penetrationImpulse = 0.0f; + contactPoint.friction1Impulse = 0.0f; + contactPoint.friction2Impulse = 0.0f; + contactPoint.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f); + // If we solve the friction raints at the center of the contact manifold + if (this.isSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1.add(p1); + internalManifold.frictionPointBody2.add(p2); + //Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); + //Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); + } + } + // If we solve the friction raints at the center of the contact manifold + if (this.isSolveFrictionAtContactManifoldCenterActive) { + internalManifold.frictionPointBody1.divide(internalManifold.nbContacts); + internalManifold.frictionPointBody2.divide(internalManifold.nbContacts); + //Log.error("]]] internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1); + //Log.error("]]] internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2); + internalManifold.r1Friction = internalManifold.frictionPointBody1.lessNew(x1); + internalManifold.r2Friction = internalManifold.frictionPointBody2.lessNew(x2); + //Log.error("]]] internalManifold.r1Friction=" + internalManifold.r1Friction); + //Log.error("]]] internalManifold.r2Friction=" + internalManifold.r2Friction); + internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1().clone(); + internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2().clone(); + //Log.error("]]] internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1); + //Log.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)); + } else { + // Initialize the accumulated impulses to zero + internalManifold.friction1Impulse = 0.0f; + internalManifold.friction2Impulse = 0.0f; + internalManifold.frictionTwistImpulse = 0.0f; + internalManifold.rollingResistanceImpulse = new Vector3f(0, 0, 0); + } + } + } + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); + } + + /** + * Get the split impulses position correction technique is used for contacts + * @return true the split status is Enable + * @return true the split status is Disable + */ + public boolean isSplitImpulseActive() { + return this.isSplitImpulseActive; + } + + /** + * Set the rained velocities arrays + * @param _rainedLinearVelocities Constrained Linear velocities Table pointer (not free) + * @param _rainedAngularVelocities Constrained angular velocities Table pointer (not free) + */ + public void setConstrainedVelocitiesArrays(final Vector3f[] _rainedLinearVelocities, final Vector3f[] _rainedAngularVelocities) { + assert (_rainedLinearVelocities != null); + assert (_rainedAngularVelocities != null); + this.linearVelocities = _rainedLinearVelocities; + this.angularVelocities = _rainedAngularVelocities; + } + + /** + * Activate or deactivate the solving of friction raints at the center of + * the contact manifold instead of solving them at each contact point + * @param _isActive Enable or not the center inertie + */ + public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean _isActive) { + this.isSolveFrictionAtContactManifoldCenterActive = _isActive; + } + + /** + * Activate or Deactivate the split impulses for contacts + * @param _isActive status to set. + */ + public void setIsSplitImpulseActive(final boolean _isActive) { + this.isSplitImpulseActive = _isActive; + } + + /** + * Set the split velocities arrays + * @param _splitLinearVelocities Split linear velocities Table pointer (not free) + * @param _splitAngularVelocities Split angular velocities Table pointer (not free) + */ + public void setSplitVelocitiesArrays(final Vector3f[] _splitLinearVelocities, final Vector3f[] _splitAngularVelocities) { + assert (_splitLinearVelocities != null); + assert (_splitAngularVelocities != null); + this.splitLinearVelocities = _splitLinearVelocities; + this.splitAngularVelocities = _splitAngularVelocities; + } + + /** + * Solve the contacts + */ + public void solve() { + //Log.warning("========================================"); + //Log.warning("== Contact SOLVER ... " + this.contactConstraints.size()); + //Log.warning("========================================"); + // For each contact manifold + for (int ccc = 0; ccc < this.contactConstraints.size(); ++ccc) { + //Log.warning("ccc=" + ccc + " / " + this.contactConstraints.size()); + final ContactManifoldSolver contactManifold = this.contactConstraints.get(ccc); + float sumPenetrationImpulse = 0.0f; + // Get the rained velocities + final Vector3f v1 = this.linearVelocities[contactManifold.indexBody1]; + 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); + for (int iii = 0; iii < contactManifold.nbContacts; ++iii) { + //Log.warning(" iii=" + iii); + final ContactPointSolver contactPoint = contactManifold.contacts[iii]; + // --------- Penetration --------- // + // Compute J*v + Vector3f deltaV = v2.addNew(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); + float Jv = deltaVDotN; + // Compute the bias "b" of the raint + final float beta = this.isSplitImpulseActive ? BETA_SPLIT_IMPULSE : 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)); + float biasPenetrationDepth = 0.0f; + if (contactPoint.penetrationDepth > 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))); + biasPenetrationDepth = -(beta / this.timeStep) * FMath.max(0.0f, contactPoint.penetrationDepth - SLOP); + //Log.warning(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); + } + //Log.warning(" contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias)); + //Log.warning(" biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth)); + final float b = biasPenetrationDepth + contactPoint.restitutionBias; + //Log.warning(" b=" + FMath.floatToString(b)); + // Compute the Lagrange multiplier lambda + float deltaLambda; + if (this.isSplitImpulseActive) { + deltaLambda = -(Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass; + } else { + deltaLambda = -(Jv + b) * contactPoint.inversePenetrationMass; + } + //Log.warning(" 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)); + // 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); + // 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); + // If the split impulse position correction is active + if (this.isSplitImpulseActive) { + // Split impulse (position correction) + final Vector3f v1Split = this.splitLinearVelocities[contactManifold.indexBody1]; + final Vector3f w1Split = this.splitAngularVelocities[contactManifold.indexBody1]; + final Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; + final Vector3f w2Split = this.splitAngularVelocities[contactManifold.indexBody2]; + final Vector3f deltaVSplit = v2Split.addNew(w2Split.cross(contactPoint.r2)).less(v1Split).less(w1Split.cross(contactPoint.r1)); + //Log.warning(" deltaVSplit=" + deltaVSplit); + final float JvSplit = deltaVSplit.dot(contactPoint.normal); + final float deltaLambdaSplit = -(JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; + final float lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = FMath.max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); + 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); + applySplitImpulse(splitImpulsePenetration, contactManifold); + } + // If we do not solve the friction raints at the center of the contact manifold + if (!this.isSolveFrictionAtContactManifoldCenterActive) { + // --------- Friction 1 --------- // + // Compute J*v + deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); + Jv = deltaV.dot(contactPoint.frictionVector1); + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + //Log.warning(" 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)); + // 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); + + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction1, contactManifold); + // --------- Friction 2 --------- // + // Compute J*v + deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1)); + Jv = deltaV.dot(contactPoint.frictionvec2); + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + //Log.warning(" 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)); + // 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); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction2, contactManifold); + // --------- Rolling resistance raint --------- // + if (contactManifold.rollingResistanceFactor > 0) { + // Compute J*v + final Vector3f JvRolling = w2.lessNew(w1); + // Compute the Lagrange multiplier lambda + Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiplyNew((JvRolling.multiplyNew(-1))); + final float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; + final Vector3f lambdaTempRolling = contactPoint.rollingResistanceImpulse; + contactPoint.rollingResistanceImpulse = contactPoint.rollingResistanceImpulse.addNew(deltaLambdaRolling).clampNew(rollingLimit); + deltaLambdaRolling = contactPoint.rollingResistanceImpulse.lessNew(lambdaTempRolling); + // Compute the impulse P=J^T * lambda + final Impulse impulseRolling = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), deltaLambdaRolling.multiplyNew(-1), new Vector3f(0.0f, 0.0f, 0.0f), 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); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseRolling, contactManifold); + } + } + } + //Log.info(" w1=" + w1); + //Log.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); + + // ------ First friction raint at the center of the contact manifol ------ // + // Compute J*v + Vector3f deltaV = v2.addNew(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)); + // 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); + 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); + contactManifold.friction1Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + // Compute the impulse P=J^T * lambda + Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiplyNew(-1).multiply(deltaLambda); + Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiplyNew(-1).multiply(deltaLambda); + Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiplyNew(deltaLambda); + Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiplyNew(deltaLambda); + + //Log.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); + // 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); + // ------ Second friction raint at the center of the contact manifol ----- // + // Compute J*v + deltaV = v2.addNew(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction)); + //Log.warning(">>>> deltaV=" + deltaV); + Jv = deltaV.dot(contactManifold.frictionvec2); + //Log.warning(">>>> Jv=" + FMath.floatToString(Jv)); + //Log.warning(">>>> contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass)); + // Compute the Lagrange multiplier lambda + deltaLambda = -Jv * contactManifold.inverseFriction2Mass; + //Log.warning(">>>> 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)); + contactManifold.friction2Impulse = FMath.max(-frictionLimit, FMath.min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); + //Log.warning(">>>> contactManifold.friction2Impulse=" + FMath.floatToString(contactManifold.friction2Impulse)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + //Log.warning(">>>> deltaLambda=" + FMath.floatToString(deltaLambda)); + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = contactManifold.frictionvec2.multiplyNew(-deltaLambda); + angularImpulseBody1 = contactManifold.r1CrossT2.multiplyNew(-deltaLambda); + linearImpulseBody2 = contactManifold.frictionvec2.multiplyNew(deltaLambda); + angularImpulseBody2 = contactManifold.r2CrossT2.multiplyNew(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); + // 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); + // ------ Twist friction raint at the center of the contact manifol ------ // + // Compute J*v + deltaV = w2.lessNew(w1); + //Log.warning(" deltaV=" + deltaV); + //Log.warning(" contactManifold.normal=" + contactManifold.normal); + Jv = deltaV.dot(contactManifold.normal); + //Log.warning(" Jv=" + FMath.floatToString(Jv)); + //Log.warning(" contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass); + deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); + //Log.warning(" deltaLambda=" + FMath.floatToString(deltaLambda)); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + //Log.warning(" contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient); + //Log.warning(" sumPenetrationImpulse=" + sumPenetrationImpulse); + //Log.warning(" frictionLimit=" + FMath.floatToString(frictionLimit)); + lambdaTemp = contactManifold.frictionTwistImpulse; + //Log.warning(" lambdaTemp=" + lambdaTemp); + contactManifold.frictionTwistImpulse = FMath.max(-frictionLimit, FMath.min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); + //Log.warning(" contactManifold.frictionTwistImpulse=" + contactManifold.frictionTwistImpulse); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + //Log.warning(" deltaLambda=" + deltaLambda); + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = new Vector3f(0.0f, 0.0f, 0.0f); + angularImpulseBody1 = contactManifold.normal.multiplyNew(-deltaLambda); + linearImpulseBody2 = new Vector3f(0.0f, 0.0f, 0.0f); + angularImpulseBody2 = contactManifold.normal.multiplyNew(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); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseTwistFriction, contactManifold); + // --------- Rolling resistance raint at the center of the contact manifold --------- // + if (contactManifold.rollingResistanceFactor > 0) { + // Compute J*v + final Vector3f JvRolling = w2.lessNew(w1); + // Compute the Lagrange multiplier lambda + Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiplyNew(JvRolling.multiplyNew(-1)); + final float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; + final Vector3f lambdaTempRolling = contactManifold.rollingResistanceImpulse; + contactManifold.rollingResistanceImpulse = (contactManifold.rollingResistanceImpulse.addNew(deltaLambdaRolling)).clampNew(rollingLimit); + deltaLambdaRolling = contactManifold.rollingResistanceImpulse.lessNew(lambdaTempRolling); + // Compute the impulse P=J^T * lambda + angularImpulseBody1 = deltaLambdaRolling.multiplyNew(-1); + angularImpulseBody2 = deltaLambdaRolling; + + final Impulse impulseRolling = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody1, new Vector3f(0.0f, 0.0f, 0.0f), 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); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseRolling, contactManifold); + } + } + } + //System.exit(-1); + } + + /** + * Store the computed impulses to use them to warm start the solver at the next iteration + */ + public void storeImpulses() { + // For each contact manifold + for (int ccc = 0; ccc < this.contactConstraints.size(); ++ccc) { + final ContactManifoldSolver manifold = this.contactConstraints.get(ccc); + for (int iii = 0; iii < manifold.nbContacts; ++iii) { + final ContactPointSolver contactPoint = manifold.contacts[iii]; + contactPoint.externalContact.setPenetrationImpulse(contactPoint.penetrationImpulse); + contactPoint.externalContact.setFrictionImpulse1(contactPoint.friction1Impulse); + contactPoint.externalContact.setFrictionImpulse2(contactPoint.friction2Impulse); + contactPoint.externalContact.setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse.clone()); + contactPoint.externalContact.setFrictionVector1(contactPoint.frictionVector1.clone()); + contactPoint.externalContact.setFrictionvec2(contactPoint.frictionvec2.clone()); + } + manifold.externalContactManifold.setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold.setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold.setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold.setRollingResistanceImpulse(manifold.rollingResistanceImpulse.clone()); + manifold.externalContactManifold.setFrictionVector1(manifold.frictionVector1.clone()); + manifold.externalContactManifold.setFrictionvec2(manifold.frictionvec2.clone()); + } + } + + /** + * Warm start the solver. + * For each raint, we apply the previous impulse (from the previous step) + * at the beginning. With this technique, we will converge faster towards the solution of the linear system + */ + public void warmStart() { + // Check that warm starting is active + if (!this.isWarmStartingActive) { + return; + } + // For each raint + for (int ccc = 0; ccc < this.contactConstraints.size(); ++ccc) { + final ContactManifoldSolver contactManifold = this.contactConstraints.get(ccc); + boolean atLeastOneRestingContactPoint = false; + for (int iii = 0; iii < contactManifold.nbContacts; ++iii) { + final ContactPointSolver contactPoint = contactManifold.contacts[iii]; + // If it is not a new contact (this contact was already existing at last time step) + if (contactPoint.isRestingContact) { + atLeastOneRestingContactPoint = true; + // --------- Penetration --------- // + // Compute the impulse P = J^T * lambda + final Impulse impulsePenetration = computePenetrationImpulse(contactPoint.penetrationImpulse, contactPoint); + // Apply the impulse to the bodies of the raint + applyImpulse(impulsePenetration, contactManifold); + // If we do not solve the friction raints at the center of the contact manifold + if (!this.isSolveFrictionAtContactManifoldCenterActive) { + // Project the old friction impulses (with old friction vectors) into + // the new friction vectors to get the new friction impulses + final Vector3f oldFrictionImpulse = contactPoint.oldFrictionVector1.multiplyNew(contactPoint.friction1Impulse) + .add(contactPoint.oldFrictionvec2.multiplyNew(contactPoint.friction2Impulse)); + contactPoint.friction1Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector1); + contactPoint.friction2Impulse = oldFrictionImpulse.dot(contactPoint.frictionvec2); + // --------- Friction 1 --------- // + // Compute the impulse P = J^T * lambda + final Impulse impulseFriction1 = computeFriction1Impulse(contactPoint.friction1Impulse, contactPoint); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction1, contactManifold); + // --------- Friction 2 --------- // + // Compute the impulse P=J^T * lambda + final Impulse impulseFriction2 = computeFriction2Impulse(contactPoint.friction2Impulse, contactPoint); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction2, contactManifold); + // ------ Rolling resistance------ // + if (contactManifold.rollingResistanceFactor > 0) { + // Compute the impulse P = J^T * lambda + final Impulse impulseRollingResistance = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), contactPoint.rollingResistanceImpulse.multiplyNew(-1), new Vector3f(0.0f, 0.0f, 0.0f), + contactPoint.rollingResistanceImpulse); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseRollingResistance, contactManifold); + } + } + } else { + // If it is a new contact point + // Initialize the accumulated impulses to zero + contactPoint.penetrationImpulse = 0.0f; + contactPoint.friction1Impulse = 0.0f; + contactPoint.friction2Impulse = 0.0f; + contactPoint.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f); + } + } + // If we solve the friction raints at the center of the contact manifold and there is + // at least one resting contact point in the contact manifold + if (this.isSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { + + // Project the old friction impulses (with old friction vectors) into the new friction + // vectors to get the new friction impulses + final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiplyNew(contactManifold.friction1Impulse) + .add(contactManifold.oldFrictionvec2.multiplyNew(contactManifold.friction2Impulse)); + + //Log.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); + // ------ First friction raint at the center of the contact manifold ------ // + // Compute the impulse P = J^T * lambda + Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiplyNew(-1).multiply(contactManifold.friction1Impulse); + Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiplyNew(-1).multiply(contactManifold.friction1Impulse); + Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiplyNew(contactManifold.friction1Impulse); + Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiplyNew(contactManifold.friction1Impulse); + + //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction1, contactManifold); + // ------ Second friction raint at the center of the contact manifold ----- // + // Compute the impulse P = J^T * lambda + linearImpulseBody1 = contactManifold.frictionvec2.multiplyNew(-1).multiply(contactManifold.friction2Impulse); + angularImpulseBody1 = contactManifold.r1CrossT2.multiplyNew(-1).multiply(contactManifold.friction2Impulse); + linearImpulseBody2 = contactManifold.frictionvec2.multiplyNew(contactManifold.friction2Impulse); + angularImpulseBody2 = contactManifold.r2CrossT2.multiplyNew(contactManifold.friction2Impulse); + + //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseFriction2, contactManifold); + // ------ Twist friction raint at the center of the contact manifold ------ // + // Compute the impulse P = J^T * lambda + linearImpulseBody1 = new Vector3f(0.0f, 0.0f, 0.0f); + angularImpulseBody1 = contactManifold.normal.multiplyNew(contactManifold.frictionTwistImpulse); + linearImpulseBody2 = new Vector3f(0.0f, 0.0f, 0.0f); + angularImpulseBody2 = contactManifold.normal.multiplyNew(contactManifold.frictionTwistImpulse); + + //Log.error("]]] linearImpulseBody1=" + linearImpulseBody1); + //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //Log.error("]]] linearImpulseBody2=" + linearImpulseBody2); + //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseTwistFriction, contactManifold); + // ------ Rolling resistance at the center of the contact manifold ------ // + // Compute the impulse P = J^T * lambda + angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiplyNew(-1); + angularImpulseBody2 = contactManifold.rollingResistanceImpulse; + + //Log.error("]]] angularImpulseBody1=" + angularImpulseBody1); + //Log.error("]]] angularImpulseBody2=" + angularImpulseBody2); + final Impulse impulseRollingResistance = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody1, new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody2); + // Apply the impulses to the bodies of the raint + applyImpulse(impulseRollingResistance, contactManifold); + } else + + { + // If it is a new contact manifold + // Initialize the accumulated impulses to zero + contactManifold.friction1Impulse = 0.0f; + contactManifold.friction2Impulse = 0.0f; + contactManifold.frictionTwistImpulse = 0.0f; + contactManifold.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f); + } + } + } +} diff --git a/src/org/atriasoft/ephysics/engine/DynamicsWorld.java b/src/org/atriasoft/ephysics/engine/DynamicsWorld.java new file mode 100644 index 0000000..85937b8 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/DynamicsWorld.java @@ -0,0 +1,1042 @@ +package org.atriasoft.ephysics.engine; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.atriasoft.ephysics.Configuration; +import org.atriasoft.ephysics.body.BodyType; +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.body.RigidBody; +import org.atriasoft.ephysics.collision.ContactManifold; +import org.atriasoft.ephysics.collision.ContactManifoldListElement; +import org.atriasoft.ephysics.collision.ContactManifoldSet; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.broadphase.DTree; +import org.atriasoft.ephysics.constraint.BallAndSocketJoint; +import org.atriasoft.ephysics.constraint.BallAndSocketJointInfo; +import org.atriasoft.ephysics.constraint.ContactsPositionCorrectionTechnique; +import org.atriasoft.ephysics.constraint.FixedJoint; +import org.atriasoft.ephysics.constraint.FixedJointInfo; +import org.atriasoft.ephysics.constraint.HingeJoint; +import org.atriasoft.ephysics.constraint.HingeJointInfo; +import org.atriasoft.ephysics.constraint.Joint; +import org.atriasoft.ephysics.constraint.JointInfo; +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; + +/** + * This class represents a dynamics world. This class inherits from + * the CollisionWorld class. In a dynamics world, bodies can collide + * and their movements are simulated using the laws of physics. + */ +public class DynamicsWorld extends CollisionWorld { + private static int kkk = 0; + protected ContactSolver contactSolver; //!< Contact solver + protected ConstraintSolver raintSolver; //!< Constraint solver + protected int nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique + protected int nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique + protected boolean isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled + protected List rigidBodies = new ArrayList<>(); //!< All the rigid bodies of the physics world + protected List joints = new ArrayList<>(); //!< All the joints of the world + protected Vector3f gravity = new Vector3f(); //!< Gravity vector of the world + public float timeStep; //!< Current frame time step (in seconds) + protected boolean isGravityEnabled; //!< True if the gravity force is on + protected Vector3f[] rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the constraints) + protected Vector3f[] rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the constraints) + protected Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + protected Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + protected Vector3f[] rainedPositions; //!< Array of rained rigid bodies position (for position error correction) + protected Quaternion[] rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction) + protected Map mapBodyToConstrainedVelocityIndex = new HashMap(); //!< Map body to their index in the rained velocities array + protected List islands = new ArrayList<>(); //!< Array with all the islands of awaken bodies + protected int numberBodiesCapacity; //!< Current allocated capacity for the bodies + + protected float sleepLinearVelocity; //!< Sleep linear velocity threshold + protected float sleepAngularVelocity; //!< Sleep angular velocity threshold + protected float timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity. + + /** + * Constructor + * @param gravity Gravity vector in the world (in meters per second squared) + */ + public DynamicsWorld(final Vector3f _gravity) { + super(); + this.contactSolver = new ContactSolver(this.mapBodyToConstrainedVelocityIndex); + this.raintSolver = new ConstraintSolver(this.mapBodyToConstrainedVelocityIndex); + this.nbVelocitySolverIterations = Configuration.DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS; + this.nbPositionSolverIterations = Configuration.DEFAULT_POSITION_SOLVER_NB_ITERATIONS; + this.isSleepingEnabled = Configuration.SPLEEPING_ENABLED; + this.gravity = _gravity; + this.isGravityEnabled = true; + this.numberBodiesCapacity = 0; + this.sleepLinearVelocity = Configuration.DEFAULT_SLEEP_LINEAR_VELOCITY; + this.sleepAngularVelocity = Configuration.DEFAULT_SLEEP_ANGULAR_VELOCITY; + this.timeBeforeSleep = Configuration.DEFAULT_TIME_BEFORE_SLEEP; + } + + /** + * Add the joint to the list of joints of the two bodies involved in the joint + * @param[in,out] _joint Joint to add at the body. + */ + protected void addJointToBody(final Joint _joint) { + if (_joint == null) { + //Log.warning("Request add null joint"); + return; + } + // Add the joint at the beginning of the linked list of joints of the first body + _joint.getBody1().jointsList = new JointListElement(_joint, _joint.getBody1().jointsList); + // Add the joint at the beginning of the linked list of joints of the second body + _joint.getBody2().jointsList = new JointListElement(_joint, _joint.getBody2().jointsList); + } + + /** + * Compute the islands of awake bodies. + * An island is an isolated group of rigid bodies that have raints (joints or contacts) + * between each other. This method computes the islands at each time step as follows: For each + * awake rigid body, we run a Depth First Search (DFS) through the raint graph of that body + * (graph where nodes are the bodies and where the edges are the raints between the bodies) to + * find all the bodies that are connected with it (the bodies that share joints or contacts with + * it). Then, we create an island with this group of connected bodies. + */ + protected void computeIslands() { + final int nbBodies = this.rigidBodies.size(); + // Clear all the islands + this.islands.clear(); + int nbContactManifolds = 0; + // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds + for (final RigidBody it : this.rigidBodies) { + final int nbBodyManifolds = it.resetIsAlreadyInIslandAndCountManifolds(); + nbContactManifolds += nbBodyManifolds; + } + for (final Joint it : this.joints) { + it.isAlreadyInIsland = false; + } + // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search + final List stackBodiesToVisit = new ArrayList<>(nbBodies); + for (int iii = 0; iii < nbBodies; iii++) { + stackBodiesToVisit.add(null); + } + + // For each rigid body of the world + for (final RigidBody body : this.rigidBodies) { + // If the body has already been added to an island, we go to the next body + if (body.isAlreadyInIsland) { + continue; + } + // If the body is static, we go to the next body + if (body.getType() == BodyType.STATIC) { + continue; + } + // If the body is sleeping or inactive, we go to the next body + if (body.isSleeping() || !body.isActive()) { + continue; + } + // Reset the stack of bodies to visit + int stackIndex = 0; + stackBodiesToVisit.set(stackIndex, body); + stackIndex++; + body.isAlreadyInIsland = true; + // Create the new island + this.islands.add(new Island(nbBodies, nbContactManifolds, this.joints.size())); + // While there are still some bodies to visit in the stack + while (stackIndex > 0) { + // Get the next body to visit from the stack + stackIndex--; + final RigidBody bodyToVisit = stackBodiesToVisit.get(stackIndex); + assert (bodyToVisit.isActive()); + // Awake the body if it is slepping + bodyToVisit.setIsSleeping(false); + // Add the body into the island + this.islands.get(this.islands.size() - 1).addBody(bodyToVisit); + // If the current body is static, we do not want to perform the DFS + // search across that body + if (bodyToVisit.getType() == BodyType.STATIC) { + continue; + } + // For each contact manifold in which the current body is involded + ContactManifoldListElement contactElement; + 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 + if (contactManifold.isAlreadyInIsland()) { + continue; + } + // Add the contact manifold into the island + this.islands.get(this.islands.size() - 1).addContactManifold(contactManifold); + contactManifold.isAlreadyInIsland = true; + // Get the other body of the contact manifold + final RigidBody body1 = (RigidBody) contactManifold.getBody1(); + final RigidBody body2 = (RigidBody) contactManifold.getBody2(); + final RigidBody otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1; + // Check if the other body has already been added to the island + if (otherBody.isAlreadyInIsland) { + continue; + } + // Insert the other body into the stack of bodies to visit + stackBodiesToVisit.set(stackIndex, otherBody); + stackIndex++; + otherBody.isAlreadyInIsland = true; + } + // For each joint in which the current body is involved + JointListElement jointElement; + for (jointElement = bodyToVisit.jointsList; jointElement != null; jointElement = jointElement.next) { + final Joint joint = jointElement.joint; + // Check if the current joint has already been added into an island + if (joint.isAlreadyInIsland()) { + continue; + } + // Add the joint into the island + this.islands.get(this.islands.size() - 1).addJoint(joint); + joint.isAlreadyInIsland = true; + // Get the other body of the contact manifold + final RigidBody body1 = joint.getBody1(); + final RigidBody body2 = joint.getBody2(); + final RigidBody otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1; + // Check if the other body has already been added to the island + if (otherBody.isAlreadyInIsland) { + continue; + } + // Insert the other body into the stack of bodies to visit + stackBodiesToVisit.set(stackIndex, otherBody); + stackIndex++; + otherBody.isAlreadyInIsland = true; + } + } + this.islands.get(this.islands.size() - 1).resetStaticBobyNotInIsland(); + } + } + + /** + * Create a joint between two bodies in the world and return a pointer to the new joint + * @param _jointInfo The information that is necessary to create the joint + * @return A pointer to the joint that has been created in the world + */ + public Joint createJoint(final JointInfo _jointInfo) { + Joint newJoint = null; + // Allocate memory to create the new joint + switch (_jointInfo.type) { + // Ball-and-Socket joint + case BALLSOCKETJOINT: + newJoint = new BallAndSocketJoint((BallAndSocketJointInfo) _jointInfo); + break; + // Slider joint + case SLIDERJOINT: + newJoint = new SliderJoint((SliderJointInfo) _jointInfo); + break; + // Hinge joint + case HINGEJOINT: + newJoint = new HingeJoint((HingeJointInfo) _jointInfo); + break; + // Fixed joint + case FIXEDJOINT: + newJoint = new FixedJoint((FixedJointInfo) _jointInfo); + break; + default: + assert (false); + return null; + } + // If the collision between the two bodies of the raint is disabled + if (!_jointInfo.isCollisionEnabled) { + // Add the pair of bodies in the set of body pairs that cannot collide with each other + this.collisionDetection.addNoCollisionPair(_jointInfo.body1, _jointInfo.body2); + } + // Add the joint into the world + this.joints.add(newJoint); + // Add the joint into the joint list of the bodies involved in the joint + addJointToBody(newJoint); + // Return the pointer to the created joint + return newJoint; + } + + /** + * Create a rigid body into the physics world + * @param _transform Transform3Dation from body local-space to world-space + * @return A pointer to the body that has been created in the world + */ + public RigidBody createRigidBody(final Transform3D _transform) { + // Compute the body ID + final int bodyID = computeNextAvailableBodyID(); + // Largest index cannot be used (it is used for invalid index) + assert (bodyID < Integer.MAX_VALUE); + // Create the rigid body + final RigidBody rigidBody = new RigidBody(_transform, this, bodyID); + assert (rigidBody != null); + // Add the rigid body to the physics world + this.bodies.add(rigidBody); + this.rigidBodies.add(rigidBody); + // Return the pointer to the rigid body + return rigidBody; + } + + /** + * Destroy a joint + * @param[in,out] _joint Pointer to the joint you want to destroy + */ + public void destroyJoint(Joint _joint) { + if (_joint == null) { + //Log.warning("Request destroy null joint"); + return; + } + // If the collision between the two bodies of the raint was disabled + if (!_joint.isCollisionEnabled()) { + // Remove the pair of bodies from the set of body pairs that cannot collide with each other + this.collisionDetection.removeNoCollisionPair(_joint.getBody1(), _joint.getBody2()); + } + // Wake up the two bodies of the joint + _joint.getBody1().setIsSleeping(false); + _joint.getBody2().setIsSleeping(false); + // Remove the joint from the world + this.joints.remove(_joint); + // Remove the joint from the joint list of the bodies involved in the joint + _joint.getBody1().removeJointFromjointsList(_joint); + _joint.getBody2().removeJointFromjointsList(_joint); + _joint = null; + } + + /** + * Destroy a rigid body and all the joints which it beints + * @param[in,out] _rigidBody Pointer to the body you want to destroy + */ + public void destroyRigidBody(RigidBody _rigidBody) { + // Remove all the collision shapes of the body + _rigidBody.removeAllCollisionShapes(); + // Add the body ID to the list of free IDs + this.freeBodiesIDs.add(_rigidBody.getID()); + // Destroy all the joints in which the rigid body to be destroyed is involved + for (JointListElement element = _rigidBody.jointsList; element != null; element = element.next) { + destroyJoint(element.joint); + } + // Reset the contact manifold list of the body + _rigidBody.resetContactManifoldsList(); + // Remove the rigid body from the list of rigid bodies + this.bodies.remove(_rigidBody); + this.rigidBodies.remove(_rigidBody); + _rigidBody = null; + } + + /** + * Enable/Disable the sleeping technique. + * The sleeping technique is used to put bodies that are not moving into sleep + * to speed up the simulation. + * @param _isSleepingEnabled True if you want to enable the sleeping technique and false otherwise + */ + public void enableSleeping(final boolean _isSleepingEnabled) { + this.isSleepingEnabled = _isSleepingEnabled; + if (!this.isSleepingEnabled) { + // For each body of the world + for (final RigidBody it : this.rigidBodies) { + // Wake up the rigid body + it.setIsSleeping(false); + } + } + } + + /** + * Get list of all contacts. + * @return The list of all contacts of the world + */ + public List getContactsList() { + final List contactManifolds = new ArrayList<>(); + // For each currently overlapping pair of bodies + for (final OverlappingPair pair : this.collisionDetection.overlappingPairs.values()) { + // For each contact manifold of the pair + final ContactManifoldSet manifoldSet = pair.getContactManifoldSet(); + for (int i = 0; i < manifoldSet.getNbContactManifolds(); i++) { + final ContactManifold manifold = manifoldSet.getContactManifold(i); + // Get the contact manifold + contactManifolds.add(manifold); + } + } + // Return all the contact manifold + return contactManifolds; + } + + /** + * Get the gravity vector of the world + * @return The current gravity vector (in meter per seconds squared) + */ + public Vector3f getGravity() { + return this.gravity; + } + + public List getIslands() { + return this.islands; + } + + /** + * Get the number of iterations for the position raint solver + */ + public int getNbIterationsPositionSolver() { + return this.nbPositionSolverIterations; + } + + /** + * Get the number of iterations for the velocity raint solver + * @return Number if iteration. + */ + public int getNbIterationsVelocitySolver() { + return this.nbVelocitySolverIterations; + } + + /** + * Get the number of all joints + * @return Number of joints in the world + */ + public int getNbJoints() { + return this.joints.size(); + } + + /** + * Get the number of rigid bodies in the world + * @return Number of rigid bodies in the world + */ + public int getNbRigidBodies() { + return this.rigidBodies.size(); + } + + /** + * Get an iterator to the beginning of the bodies of the physics world + * @return Starting iterator of the set of rigid bodies + */ + public List getRigidBodies() { + return this.rigidBodies; + } + + /** + * Return the current sleep angular velocity + * @return The sleep angular velocity (in radian per second) + */ + public float getSleepAngularVelocity() { + return this.sleepAngularVelocity; + } + + /** + * Get the sleep linear velocity + * @return The current sleep linear velocity (in meters per second) + */ + public float getSleepLinearVelocity() { + return this.sleepLinearVelocity; + } + + /** + * Get the time a body is required to stay still before sleeping + * @return Time a body is required to stay still before sleeping (in seconds) + */ + public float getTimeBeforeSleep() { + return this.timeBeforeSleep; + } + + /** + * Initialize the bodies velocities arrays for the next simulation step. + */ + protected void initVelocityArrays() { + // Allocate memory for the bodies velocity arrays + final int nbBodies = this.rigidBodies.size(); + // this.numberBodiesCapacity = 0; // TODO: Remove this, this is a test of portage + if (this.numberBodiesCapacity < nbBodies) { + this.numberBodiesCapacity = nbBodies; + this.splitLinearVelocities = new Vector3f[this.numberBodiesCapacity]; + this.splitAngularVelocities = new Vector3f[this.numberBodiesCapacity]; + this.rainedLinearVelocities = new Vector3f[this.numberBodiesCapacity]; + this.rainedAngularVelocities = new Vector3f[this.numberBodiesCapacity]; + this.rainedPositions = new Vector3f[this.numberBodiesCapacity]; + this.rainedOrientations = new Quaternion[this.numberBodiesCapacity]; + for (int iii = 0; iii < this.numberBodiesCapacity; iii++) { + this.splitLinearVelocities[iii] = Vector3f.zero(); + this.splitAngularVelocities[iii] = Vector3f.zero(); + this.rainedLinearVelocities[iii] = Vector3f.zero(); + this.rainedAngularVelocities[iii] = Vector3f.zero(); + this.rainedPositions[iii] = Vector3f.zero(); + this.rainedOrientations[iii] = Quaternion.identity(); + } + } else { + // Reset the velocities arrays + for (int iii = 0; iii < this.numberBodiesCapacity; iii++) { + this.splitLinearVelocities[iii].setZero(); + this.splitAngularVelocities[iii].setZero(); + this.rainedLinearVelocities[iii].setZero(); + this.rainedAngularVelocities[iii].setZero(); + this.rainedPositions[iii].setZero(); + this.rainedOrientations[iii].setIdentity(); + } + } + // Initialize the map of body indexes in the velocity arrays + this.mapBodyToConstrainedVelocityIndex.clear(); + int indexBody = 0; + for (final RigidBody it : this.rigidBodies) { + // Add the body into the map + this.mapBodyToConstrainedVelocityIndex.put(it, indexBody); + indexBody++; + } + } + + /** + * Integrate position and orientation of the rigid bodies. + * The positions and orientations of the bodies are integrated using + * the sympletic Euler time stepping scheme. + */ + protected void integrateRigidBodiesPositions() { + //Log.error("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++integrateRigidBodiesPositions"); + // For each island of the world + for (int i = 0; i < this.islands.size(); i++) { + final List bodies = this.islands.get(i).getBodies(); + // For each body of the island + for (int b = 0; b < bodies.size(); b++) { + //Log.error(" [" + b + "/" + bodies.size() + "]"); + // Get the rained velocity + final int indexArray = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b)); + final Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray].clone(); + final Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray].clone(); + //Log.error(" newAngVelocity = " + newAngVelocity); + // Add the split impulse velocity from Contact Solver (only used + // to update the position) + if (this.contactSolver.isSplitImpulseActive()) { + newLinVelocity.add(this.splitLinearVelocities[indexArray]); + 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(); + // Update the new rained position and orientation of the body + this.rainedPositions[indexArray] = newLinVelocity.multiplyNew(this.timeStep).add(currentPosition); + //Log.error(" currentOrientation = " + currentOrientation); + //Log.error(" newAngVelocity = " + newAngVelocity); + //Log.error(" this.timeStep = " + FMath.floatToString(this.timeStep)); + this.rainedOrientations[indexArray] = currentOrientation.addNew(new Quaternion(0, newAngVelocity).multiplyNew(currentOrientation).multiply(0.5f * this.timeStep)); + //Log.error(" this.rainedPositions[indexArray] = " + this.rainedPositions[indexArray]); + //Log.error(" this.rainedOrientations[indexArray] = " + this.rainedOrientations[indexArray]); + } + } + for (int iii = 1; iii < this.rainedPositions.length; iii++) { + Log.error(kkk + " this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]); + Log.error(kkk + " this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]); + } + //Log.error("------------------------------------------------------------------------------------------------"); + //Log.error("------------------------------------------------------------------------------------------------"); + //Log.error("------------------------------------------------------------------------------------------------"); + //Log.error("------------------------------------------------------------------------------------------------"); + if (kkk++ == 98) { + //System.exit(-1); + } + } + + /** + * Integrate the velocities of rigid bodies. + * This method only set the temporary velocities but does not update + * the actual velocitiy of the bodies. The velocities updated in this method + * might violate the raints and will be corrected in the raint and + * contact solver. + */ + protected void integrateRigidBodiesVelocities() { + // Initialize the bodies velocity arrays + initVelocityArrays(); + //Log.info("@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@"); + // For each island of the world + for (int iii = 0; iii < this.islands.size(); iii++) { + //Log.info("Manage island : " + iii + "/" + this.islands.size()); + final List bodies = this.islands.get(iii).getBodies(); + // For 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); + int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval); + //Log.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().addNew(bodies.get(bbb).externalForce.multiplyNew(this.timeStep * bodies.get(bbb).massInverse)); + //Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + this.rainedAngularVelocities[indexBody] = bodies.get(bbb).getAngularVelocity() + .addNew(bodies.get(bbb).getInertiaTensorInverseWorld().multiplyNew(bodies.get(bbb).externalTorque.multiplyNew(this.timeStep))); + //Log.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) { + // Integrate the gravity force + this.rainedLinearVelocities[indexBody].add(this.gravity.multiplyNew(this.timeStep * bodies.get(bbb).massInverse * bodies.get(bbb).getMass())); + } + //Log.info(" G this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + // Apply the velocity damping + // Damping force : F_c = -c' * v (c=damping factor) + // Equation : m * dv/dt = -c' * v + // => dv/dt = -c * v (with c=c'/m) + // => dv/dt + c * v = 0 + // Solution : v(t) = v0 * e^(-c * t) + // => v(t + dt) = v0 * e^(-c(t + dt)) + // = v0 * e^(-ct) * e^(-c * dt) + // = v(t) * e^(-c * dt) + // => v2 = v1 * e^(-c * dt) + // 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 linearDamping = FMath.pow(1.0f - linDampingFactor, this.timeStep); + //Log.info(" linearDamping=" + FMath.floatToString(linearDamping)); + final float angularDamping = FMath.pow(1.0f - angDampingFactor, this.timeStep); + //Log.info(" angularDamping=" + FMath.floatToString(angularDamping)); + this.rainedLinearVelocities[indexBody].multiply(linearDamping); + //Log.info(" this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]); + this.rainedAngularVelocities[indexBody].multiply(angularDamping); + //Log.info(" this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]); + indexBody++; + } + } + } + + /** + * Get if the gravity is enaled + * @return True if the gravity is enabled in the world + */ + public boolean isGravityEnabled() { + return this.isGravityEnabled; + } + + /** + * Get if the sleeping technique is enabled + * @return True if the sleeping technique is enabled and false otherwise + */ + public boolean isSleepingEnabled() { + return this.isSleepingEnabled; + } + + /** + * Reset the external force and torque applied to the bodies + */ + protected void resetBodiesForceAndTorque() { + // For each body of the world + this.rigidBodies.forEach((obj) -> { + obj.externalForce.setZero(); + obj.externalTorque.setZero(); + }); + } + + /** + * Set the position correction technique used for contacts + * @param _technique Technique used for the position correction (Baumgarte or Split Impulses) + */ + public void setContactsPositionCorrectionTechnique(final ContactsPositionCorrectionTechnique _technique) { + if (_technique == ContactsPositionCorrectionTechnique.BAUMGARTE_CONTACTS) { + this.contactSolver.setIsSplitImpulseActive(false); + } else { + this.contactSolver.setIsSplitImpulseActive(true); + } + } + + /** + * Set an event listener object to receive events callbacks. + * @note If you use null as an argument, the events callbacks will be disabled. + * @param _eventListener Pointer to the event listener object that will receive + * event callbacks during the simulation + */ + public void setEventListener(final EventListener _eventListener) { + this.eventListener = _eventListener; + } + + /** + * Set the gravity vector of the world + * @param _gravity The gravity vector (in meter per seconds squared) + */ + public void setGravity(final Vector3f _gravity) { + this.gravity = _gravity; + } + + /** + * Enable/Disable the gravity + * @param _isGravityEnabled True if you want to enable the gravity in the world + * and false otherwise + */ + public void setIsGratityEnabled(final boolean _isGravityEnabled) { + this.isGravityEnabled = _isGravityEnabled; + } + + /** + * Activate or deactivate the solving of friction raints at the center of the contact + * manifold instead of solving them at each contact point + * @param _isActive True if you want the friction to be solved at the center of + * the contact manifold and false otherwise + */ + public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean _isActive) { + this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(_isActive); + } + + /** + * Set the position correction technique used for joints + * @param _technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel) + */ + public void setJointsPositionCorrectionTechnique(final JointsPositionCorrectionTechnique _technique) { + if (_technique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) { + this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false); + } else { + this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true); + } + } + + /** + * Set the number of iterations for the position raint solver + * @param _nbIterations Number of iterations for the position solver + */ + public void setNbIterationsPositionSolver(final int _nbIterations) { + this.nbPositionSolverIterations = _nbIterations; + } + + /** + * Set the number of iterations for the velocity raint solver + * @param _nbIterations Number of iterations for the velocity solver + */ + public void setNbIterationsVelocitySolver(final int _nbIterations) { + this.nbVelocitySolverIterations = _nbIterations; + } + + /** + * Set the sleep angular velocity. + * When the velocity of a body becomes smaller than the sleep linear/angular + * velocity for a given amount of time, the body starts sleeping and does not need + * to be simulated anymore. + * @param _sleepAngularVelocity The sleep angular velocity (in radian per second) + */ + public void setSleepAngularVelocity(final float _sleepAngularVelocity) { + if (_sleepAngularVelocity < 0.0f) { + //Log.error("Can not set _sleepAngularVelocity=" + _sleepAngularVelocity + " < 0 "); + return; + } + this.sleepAngularVelocity = _sleepAngularVelocity; + } + + /** + * Set the sleep linear velocity + * @param _sleepLinearVelocity The new sleep linear velocity (in meters per second) + */ + public void setSleepLinearVelocity(final float _sleepLinearVelocity) { + if (_sleepLinearVelocity < 0.0f) { + //Log.error("Can not set _sleepLinearVelocity=" + _sleepLinearVelocity + " < 0 "); + return; + } + this.sleepLinearVelocity = _sleepLinearVelocity; + } + + /** + * Set the time a body is required to stay still before sleeping + * @param timeBeforeSleep Time a body is required to stay still before sleeping (in seconds) + */ + public void setTimeBeforeSleep(final float timeBeforeSleep) { + if (timeBeforeSleep < 0.0f) { + //Log.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 "); + return; + } + this.timeBeforeSleep = timeBeforeSleep; + } + + /** + * Solve the contacts and raints + */ + protected void solveContactsAndConstraints() { + // Set the velocities arrays + this.contactSolver.setSplitVelocitiesArrays(this.splitLinearVelocities, this.splitAngularVelocities); + this.contactSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); + this.raintSolver.setConstrainedVelocitiesArrays(this.rainedLinearVelocities, this.rainedAngularVelocities); + this.raintSolver.setConstrainedPositionsArrays(this.rainedPositions, this.rainedOrientations); + + //Log.info(")))))))))))))))"); + for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { + //Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); + } + // ---------- Solve velocity raints for joints and contacts ---------- // + final int idIsland = 0; + // For each island of the world + for (final Island island : this.islands) { + // 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); + if (!isConstraintsToSolve && !isContactsToSolve) { + continue; + } + // If there are contacts in the current island + if (isContactsToSolve) { + // Initialize the solver + this.contactSolver.initializeForIsland(this.timeStep, island); + // Warm start the contact solver + this.contactSolver.warmStart(); + } + // If there are raints + if (isConstraintsToSolve) { + // Initialize the raint solver + this.raintSolver.initializeForIsland(this.timeStep, island); + } + // For each iteration of the velocity solver + for (int i = 0; i < this.nbVelocitySolverIterations; i++) { + // Solve the raints + if (isConstraintsToSolve) { + this.raintSolver.solveVelocityConstraints(island); + } + // Solve the contacts + if (isContactsToSolve) { + this.contactSolver.solve(); + } + } + // Cache the lambda values in order to use them in the next + // step and cleanup the contact solver + if (isContactsToSolve) { + this.contactSolver.storeImpulses(); + this.contactSolver.cleanup(); + } + } + //Log.info("(((((((((((((("); + for (int iii = 0; iii < this.rainedAngularVelocities.length; iii++) { + //Log.info(" " + iii + " : " + this.rainedAngularVelocities[iii]); + } + } + + /** + * Solve the position error correction of the raints + */ + protected void solvePositionCorrection() { + // Do not continue if there is no raints + if (this.joints.isEmpty()) { + return; + } + // For each island of the world + for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { + // ---------- 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)); + } + } + } + + @Override + public void testCollision(final CollisionBody _body1, final CollisionBody _body2, final CollisionCallback _callback) { + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + for (ProxyShape shape = _body1.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes1.add(shape.broadPhaseID); + } + final Set shapes2 = new Set(Set.getDTreeCoparator()); + for (ProxyShape shape = _body2.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes2.add(shape.broadPhaseID); + } + // Perform the collision detection and report contacts + this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes1, shapes2); + } + + @Override + public void testCollision(final CollisionBody _body, final CollisionCallback _callback) { + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + // For each shape of the body + for (ProxyShape shape = _body.getProxyShapesList(); shape != null; shape = shape.getNext()) { + shapes1.add(shape.broadPhaseID); + } + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes1, emptySet); + } + + /// Test and report collisions between all shapes of the world + @Override + public void testCollision(final CollisionCallback _callback) { + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.reportCollisionBetweenShapes(_callback, emptySet, emptySet); + } + + @Override + public void testCollision(final ProxyShape _shape, final CollisionCallback _callback) { + // Create the sets of shapes + final Set shapes = new Set(Set.getDTreeCoparator()); + shapes.add(_shape.broadPhaseID); + final Set emptySet = new Set(Set.getDTreeCoparator()); + // Perform the collision detection and report contacts + this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes, emptySet); + } + + @Override + public void testCollision(final ProxyShape _shape1, final ProxyShape _shape2, final CollisionCallback _callback) { + // Create the sets of shapes + final Set shapes1 = new Set(Set.getDTreeCoparator()); + shapes1.add(_shape1.broadPhaseID); + final Set shapes2 = new Set(Set.getDTreeCoparator()); + shapes2.add(_shape2.broadPhaseID); + // Perform the collision detection and report contacts + this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes1, shapes2); + } + + /** + * Update the physics simulation + * @param timeStep The amount of time to step the simulation by (in seconds) + */ + public void update(final float timeStep) { + //Log.error("========> start compute " + this.rigidBodies.size()); + for (final CollisionBody elem : this.bodies) { + //Log.info(" " + elem.getID() + " / " + elem.getAABB()); + //Log.info(" " + elem.getID() + " / " + elem.getTransform()); + } + + this.timeStep = timeStep; + // Notify the event listener about the beginning of an internal tick + if (this.eventListener != null) { + this.eventListener.beginInternalTick(); + } + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); + if (this.rigidBodies.size() == 0) { + // no rigid body ==> no process to do ... + return; + } + // Compute the collision detection + this.collisionDetection.computeCollisionDetection(); + // 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()); + } + // Integrate the velocities + integrateRigidBodiesVelocities(); + for (final CollisionBody elem : this.bodies) { + //Log.info("222 " + elem.getID() + " / " + elem.getAABB()); + //Log.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()); + } + // 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()); + } + // 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()); + } + // Update the state (positions and velocities) of the bodies + //Log.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"); + for (final CollisionBody elem : this.bodies) { + //Log.info("--- " + elem.getID() + " / " + elem.getAABB()); + //Log.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()); + } + // 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()); + } + // Reset the external force and torque applied to the bodies + resetBodiesForceAndTorque(); + //Log.error("<< ============= end compute "); + } + + /** + * Update the postion/orientation of the bodies + */ + protected void updateBodiesState() { + // For each island of the world + for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { + // For each body of the island + final List bodies = this.islands.get(islandIndex).getBodies(); + for (int b = 0; b < this.islands.get(islandIndex).getNbBodies(); b++) { + final int index = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b)); + // Update the linear and angular velocity of the body + bodies.get(b).linearVelocity = this.rainedLinearVelocities[index].clone(); + if (bodies.get(b).isAngularReactionEnable() == true) { + bodies.get(b).angularVelocity = this.rainedAngularVelocities[index].clone(); + } + // Update the position of the center of mass of the body + bodies.get(b).centerOfMassWorld = this.rainedPositions[index].clone(); + // Update the orientation of the body + //Log.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]); + //Log.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew()); + if (bodies.get(b).isAngularReactionEnable() == true) { + bodies.get(b).getTransform().setOrientation(this.rainedOrientations[index].safeNormalizeNew()); + } + // 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"); + bodies.get(b).updateBroadPhaseState(); + } + } + } + + /** + * Put bodies to sleep if needed. + * For each island, if all the bodies have been almost still for a int enough period of + * time, we put all the bodies of the island to sleep. + */ + protected void updateSleepingBodies() { + 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++) { + float minSleepTime = Float.MAX_VALUE; + // For each body of the island + final List bodies = this.islands.get(i).getBodies(); + for (int b = 0; b < bodies.size(); b++) { + // Skip static bodies + if (bodies.get(b).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()) { + // Reset the sleep time of the body + bodies.get(b).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; + } + } + } + // If the velocity of all the bodies of the island is under the + // sleeping velocity threshold for a period of time larger than + // 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++) { + bodies.get(b).setIsSleeping(true); + } + } + } + } + +} diff --git a/src/org/atriasoft/ephysics/engine/EventListener.java b/src/org/atriasoft/ephysics/engine/EventListener.java new file mode 100644 index 0000000..08cca1c --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/EventListener.java @@ -0,0 +1,40 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.ephysics.constraint.ContactPointInfo; + +/** + * This class can be used to receive event callbacks from the physics engine. + * In order to receive callbacks, you need to create a new class that inherits from + * this one and you must the methods you need. Then, you need to register your + * new event listener class to the physics world using the DynamicsWorld::setEventListener() + * method. + */ +public interface EventListener { + /** + * Called when a new contact point is found between two bodies that were separated before + * @param contact Information about the contact + */ + void beginContact(ContactPointInfo contact); + + /** + * Called at the beginning of an internal tick of the simulation step. + * Each time the DynamicsWorld::update() method is called, the physics + * engine will do several internal simulation steps. This method is + * called at the beginning of each internal simulation step. + */ + void beginInternalTick(); + + /** + * Called at the end of an internal tick of the simulation step. + * Each time the DynamicsWorld::update() metho is called, the physics + * engine will do several internal simulation steps. This method is + * called at the end of each internal simulation step. + */ + void endInternalTick(); + + /** + * Called when a new contact point is found between two bodies + * @param contact Information about the contact + */ + void newContact(ContactPointInfo contact); +} diff --git a/src/org/atriasoft/ephysics/engine/Impulse.java b/src/org/atriasoft/ephysics/engine/Impulse.java new file mode 100644 index 0000000..ab4b3a4 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/Impulse.java @@ -0,0 +1,28 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.etk.math.Vector3f; + +/** + * Represents an impulse that we can apply to bodies in the contact or raint solver. + */ +public class Impulse { + public final Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body + public final Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body + public final Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body + public final Vector3f angularImpulseBody2; //!< Angular impulse applied to the second body + + public Impulse(final Impulse impulse) { + this.linearImpulseBody1 = impulse.linearImpulseBody1.clone(); + this.angularImpulseBody1 = impulse.angularImpulseBody1.clone(); + this.linearImpulseBody2 = impulse.linearImpulseBody2.clone(); + this.angularImpulseBody2 = impulse.angularImpulseBody2.clone(); + + } + + public Impulse(final Vector3f _initLinearImpulseBody1, final Vector3f _initAngularImpulseBody1, final Vector3f _initLinearImpulseBody2, final Vector3f _initAngularImpulseBody2) { + this.linearImpulseBody1 = _initLinearImpulseBody1.clone(); + this.angularImpulseBody1 = _initAngularImpulseBody1.clone(); + this.linearImpulseBody2 = _initLinearImpulseBody2.clone(); + this.angularImpulseBody2 = _initAngularImpulseBody2.clone(); + } +} diff --git a/src/org/atriasoft/ephysics/engine/Island.java b/src/org/atriasoft/ephysics/engine/Island.java new file mode 100644 index 0000000..29e6f35 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/Island.java @@ -0,0 +1,111 @@ +package org.atriasoft.ephysics.engine; + +import java.util.ArrayList; +import java.util.List; + +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; + +/** + * An island represent an isolated group of awake bodies that are connected with each other by + * some contraints (contacts or joints). + */ +public class Island { + private final List bodies = new ArrayList<>(); //!< Array with all the bodies of the island + private final List contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island + private final List joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island + + /** + * Constructor + */ + public Island(final int nbMaxBodies, final int nbMaxContactManifolds, final int nbMaxJoints) { + // Allocate memory for the arrays + //this.bodies.reserve(_nbMaxBodies); + //this.contactManifolds.reserve(_nbMaxContactManifolds); + //this.joints.reserve(_nbMaxJoints); + } + + /** + * Add a body. + */ + public void addBody(final RigidBody _body) { + if (_body.isSleeping() == true) { + Log.error("Try to add a body that is sleeping ..."); + return; + } + this.bodies.add(_body); + } + + /** + * Add a contact manifold. + */ + public void addContactManifold(final ContactManifold _contactManifold) { + this.contactManifolds.add(_contactManifold); + } + + /** + * Add a joint. + */ + public void addJoint(final Joint _joint) { + this.joints.add(_joint); + } + + /** + * Return a pointer to the array of bodies + */ + public List getBodies() { + return this.bodies; + } + + /** + * Return a pointer to the array of contact manifolds + */ + public List getContactManifold() { + return this.contactManifolds; + } + + /** + * Return a pointer to the array of joints + */ + public List getJoints() { + return this.joints; + } + + /** + * Get the number of body + * @return Number of bodies. + */ + public int getNbBodies() { + return this.bodies.size(); + } + + /** + * @ Get the number of contact manifolds + * Return the number of contact manifolds in the island + */ + public int getNbContactManifolds() { + return this.contactManifolds.size(); + } + + /** + * Return the number of joints in the island + */ + public int getNbJoints() { + return this.joints.size(); + } + + /** + * Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands + */ + public void resetStaticBobyNotInIsland() { + for (final RigidBody it : this.bodies) { + if (it.getType() == BodyType.STATIC) { + it.isAlreadyInIsland = false; + } + } + } + +} diff --git a/src/org/atriasoft/ephysics/engine/Material.java b/src/org/atriasoft/ephysics/engine/Material.java new file mode 100644 index 0000000..4d0ca34 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/Material.java @@ -0,0 +1,91 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.ephysics.Configuration; + +public class Material { + + private float frictionCoefficient; //!< Friction coefficient (positive value) + private float rollingResistance; //!< Rolling resistance factor (positive value) + private float bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body + + /// Constructor + public Material() { + this.frictionCoefficient = Configuration.DEFAULT_FRICTION_COEFFICIENT; + this.rollingResistance = Configuration.DEFAULT_ROLLING_RESISTANCE; + this.bounciness = Configuration.DEFAULT_BOUNCINESS; + } + + /// Copy-ructor + public Material(final Material material) { + this.frictionCoefficient = material.frictionCoefficient; + this.rollingResistance = material.rollingResistance; + this.bounciness = material.bounciness; + } + + /// Return the bounciness + /// @return Bounciness factor (between 0 and 1) where 1 is very bouncy + public float getBounciness() { + return this.bounciness; + } + + /**Return the friction coefficient + * @return Friction coefficient (positive value) + */ + public float getFrictionCoefficient() { + return this.frictionCoefficient; + } + + /** Return the rolling resistance factor. If this value is larger than zero, + * it will be used to slow down the body when it is rolling + * against another body. + * @return The rolling resistance factor (positive value) + */ + public float getRollingResistance() { + return this.rollingResistance; + } + + /// Overloaded assignment operator + public Material set(final Material material) { + + // Check for self-assignment + if (this != material) { + this.frictionCoefficient = material.frictionCoefficient; + this.bounciness = material.bounciness; + this.rollingResistance = material.rollingResistance; + } + + // Return this material + return this; + } + + /** Set the bounciness. + * The bounciness should be a value between 0 and 1. The value 1 is used for a + * very bouncy body and zero is used for a body that is not bouncy at all. + * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy + */ + public void setBounciness(final float bounciness) { + assert (bounciness >= 0.0f && bounciness <= 1.0f); + this.bounciness = bounciness; + } + + /** Set the friction coefficient. + * The friction coefficient has to be a positive value. The value zero is used for no + * friction at all. + * @param frictionCoefficient Friction coefficient (positive value) + */ + public void setFrictionCoefficient(final float frictionCoefficient) { + assert (frictionCoefficient >= 0.0f); + this.frictionCoefficient = frictionCoefficient; + } + + /** Set the rolling resistance factor. If this value is larger than zero, + * it will be used to slow down the body when it is rolling + * against another body. + * @param rollingResistance The rolling resistance factor + */ + public void setRollingResistance(final float rollingResistance) { + assert (rollingResistance >= 0); + this.rollingResistance = rollingResistance; + } + +} diff --git a/src/org/atriasoft/ephysics/engine/OverlappingPair.java b/src/org/atriasoft/ephysics/engine/OverlappingPair.java new file mode 100644 index 0000000..6b95d19 --- /dev/null +++ b/src/org/atriasoft/ephysics/engine/OverlappingPair.java @@ -0,0 +1,88 @@ +package org.atriasoft.ephysics.engine; + +import org.atriasoft.ephysics.body.CollisionBody; +import org.atriasoft.ephysics.collision.ContactManifoldSet; +import org.atriasoft.ephysics.collision.ProxyShape; +import org.atriasoft.ephysics.collision.broadphase.PairDTree; +import org.atriasoft.ephysics.constraint.ContactPoint; +import org.atriasoft.ephysics.mathematics.PairInt; +import org.atriasoft.etk.math.Vector3f; + +/** + * This class represents a pair of two proxy collision shapes that are overlapping + * during the broad-phase collision detection. It is created when + * the two proxy collision shapes start to overlap and is destroyed when they do not + * overlap anymore. This class contains a contact manifold that + * store all the contact points between the two bodies. + */ +public class OverlappingPair { + /// Return the pair of bodies index of the pair + public static PairInt computeBodiesIndexPair(final CollisionBody body1, final CollisionBody body2) { + // Construct the pair of body index + final PairInt indexPair = body1.getID() < body2.getID() ? new PairInt(body1.getID(), body2.getID()) : new PairInt(body2.getID(), body1.getID()); + assert (indexPair.first != indexPair.second); + return indexPair; + } + + /// Return the pair of bodies index + public static PairDTree computeID(final ProxyShape shape1, final ProxyShape shape2) { + assert (shape1.getBroadPhaseID() != null && shape2.getBroadPhaseID() != null); + // Construct the pair of body index + return new PairDTree(shape1.getBroadPhaseID(), shape2.getBroadPhaseID()); + } + + private final ContactManifoldSet contactManifoldSet; //!< Set of persistent contact manifolds + + private Vector3f cachedSeparatingAxis; //!< Cached previous separating axis + + /// Constructor + public OverlappingPair(final ProxyShape shape1, final ProxyShape shape2, final int nbMaxContactManifolds) { + this.contactManifoldSet = new ContactManifoldSet(shape1, shape2, nbMaxContactManifolds); + this.cachedSeparatingAxis = new Vector3f(1.0f, 1.0f, 1.0f); + } + + /// Add a contact to the contact cache + public void addContact(final ContactPoint contact) { + this.contactManifoldSet.addContactPoint(contact); + } + + /// Clear the contact points of the contact manifold + public void clearContactPoints() { + this.contactManifoldSet.clear(); + } + + /// Return the cached separating axis + public Vector3f getCachedSeparatingAxis() { + return this.cachedSeparatingAxis; + } + + /// Return the a reference to the contact manifold set + public ContactManifoldSet getContactManifoldSet() { + return this.contactManifoldSet; + } + + /// Return the number of contacts in the cache + public int getNbContactPoints() { + return this.contactManifoldSet.getTotalNbContactPoints(); + } + + /// Return the pointer to first proxy collision shape + public ProxyShape getShape1() { + return this.contactManifoldSet.getShape1(); + } + + /// Return the pointer to second body + public ProxyShape getShape2() { + return this.contactManifoldSet.getShape2(); + } + + /// Set the cached separating axis + public void setCachedSeparatingAxis(final Vector3f axis) { + this.cachedSeparatingAxis = axis; + } + + /// Update the contact cache + public void update() { + this.contactManifoldSet.update(); + } +} diff --git a/src/org/atriasoft/ephysics/internal/Log.java b/src/org/atriasoft/ephysics/internal/Log.java new file mode 100644 index 0000000..ad567c9 --- /dev/null +++ b/src/org/atriasoft/ephysics/internal/Log.java @@ -0,0 +1,68 @@ +package org.atriasoft.ephysics.internal; + +import io.scenarium.logger.LogLevel; +import io.scenarium.logger.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() {} + +} diff --git a/src/org/atriasoft/ephysics/mathematics/Mathematics.java b/src/org/atriasoft/ephysics/mathematics/Mathematics.java new file mode 100644 index 0000000..cf6c69a --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/Mathematics.java @@ -0,0 +1,96 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.mathematics; + +import org.atriasoft.etk.math.Vector3f; + +/** + * + * @author Jason Sorensen + */ +public class Mathematics { + + // Function to test if two real numbers are (almost) equal + // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON] + public static boolean ApproxEqual(final float a, final float b, final float epsilon) { + final float difference = a - b; + return (difference < epsilon && difference > -epsilon); + } + + public static float ArcCos(final float radians) { + return (float) StrictMath.acos(radians); + } + + public static float ArcSin(final float radians) { + return (float) StrictMath.asin(radians); + } + + public static float ArcTan2(final float a, final float b) { + return (float) StrictMath.atan2(a, b); + } + + // Function that returns the result of the "value" clamped by + // two others values "lowerLimit" and "upperLimit" + public static float Clamp(final float value, final float lowerLimit, final float upperLimit) { + assert (lowerLimit <= upperLimit); + return Math.min(Math.max(value, lowerLimit), upperLimit); + } + + /// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) + /// This method uses the technique described in the book Real-Time collision detection by + /// Christer Ericson. + public static void computeBarycentricCoordinatesInTriangle(final Vector3f a, final Vector3f b, final Vector3f c, final Vector3f p, Float u, Float v, Float w) { + final Vector3f v0 = b.lessNew(a); + final Vector3f v1 = c.lessNew(a); + final Vector3f v2 = p.lessNew(a); + + final float d00 = v0.dot(v0); + final float d01 = v0.dot(v1); + final float d11 = v1.dot(v1); + final float d20 = v2.dot(v0); + final float d21 = v2.dot(v1); + + final float denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = 1.0f - v - w; + } + + public static float Cos(final float radians) { + return (float) StrictMath.cos(radians); + } + + public static float Sin(final float radians) { + return (float) StrictMath.sin(radians); + } + + public static float Sqrt(final float a) { + return (float) StrictMath.sqrt(a); + } + + public static float Tan(final float radians) { + return (float) StrictMath.tan(radians); + } +} diff --git a/src/org/atriasoft/ephysics/mathematics/Matrix2f.java b/src/org/atriasoft/ephysics/mathematics/Matrix2f.java new file mode 100644 index 0000000..b336c1f --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/Matrix2f.java @@ -0,0 +1,249 @@ +/* + * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ + * Copyright (c) 2010-2013 Daniel Chappuis + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from the + * use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not claim + * that you wrote the original software. If you use this software in a + * product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * + * 3. This notice may not be removed or altered from any source distribution. + * + * This file has been modified during the port to Java and differ from the source versions. + */ +package org.atriasoft.ephysics.mathematics; + +import org.atriasoft.etk.math.Constant; +import org.atriasoft.etk.math.Vector2f; + +/** + * This class represents a 2x2 matrix. + * + * @author Jason Sorensen + */ +public class Matrix2f { + + // Rows of the matrix (m[row][column]) + float m00; + float m01; + float m10; + float m11; + + // Constructor + public Matrix2f() { + setZero(); + } + + // Constructor + public Matrix2f(final float value) { + set(value, value, value, value); + } + + // Constructor with arguments + public Matrix2f(final float a1, final float a2, final float b1, final float b2) { + set(a1, a2, b1, b2); + } + + // Copy-constructor + public Matrix2f(final Matrix2f matrix) { + set(matrix); + } + + // Return the matrix with absolute values + public Matrix2f abs() { + this.m00 = Math.abs(this.m00); + this.m01 = Math.abs(this.m01); + this.m10 = Math.abs(this.m10); + this.m11 = Math.abs(this.m11); + return this; + } + + // Overloaded operator for addition with assignment + public Matrix2f add(final Matrix2f matrix) { + this.m00 += matrix.m00; + this.m01 += matrix.m01; + this.m10 += matrix.m10; + this.m11 += matrix.m11; + return this; + } + + @Override + public boolean equals(final Object obj) { + + if (obj == null) { + return false; + } + if (getClass() != obj.getClass()) { + return false; + } + final Matrix2f other = (Matrix2f) obj; + if (Float.floatToIntBits(this.m00) != Float.floatToIntBits(other.m00)) { + return false; + } + if (Float.floatToIntBits(this.m10) != Float.floatToIntBits(other.m10)) { + return false; + } + if (Float.floatToIntBits(this.m01) != Float.floatToIntBits(other.m01)) { + return false; + } + return Float.floatToIntBits(this.m11) == Float.floatToIntBits(other.m11); + } + + // Return a column + public Vector2f getColumn(final int index) { + if (index == 0) { + return new Vector2f(this.m00, this.m10); + } else if (index == 1) { + return new Vector2f(this.m01, this.m11); + } + throw new IllegalArgumentException("Unknown column index: " + index); + } + + // Return the determinant of the matrix + public float getDeterminant() { + return this.m00 * this.m11 - this.m10 * this.m01; + } + + // Return a row + public Vector2f getRow(final int index) { + if (index == 0) { + return new Vector2f(this.m00, this.m01); + } else if (index == 1) { + return new Vector2f(this.m10, this.m11); + } + throw new IllegalArgumentException("Unknown row index: " + index); + } + + // Return the trace of the matrix + public float getTrace() { + return this.m00 + this.m11; + } + + @Override + public int hashCode() { + int hash = 5; + hash = 23 * hash + Float.floatToIntBits(this.m00); + hash = 23 * hash + Float.floatToIntBits(this.m10); + hash = 23 * hash + Float.floatToIntBits(this.m01); + hash = 23 * hash + Float.floatToIntBits(this.m11); + return hash; + } + + // Set the matrix to the identity matrix + public Matrix2f identity() { + this.m00 = 1.0f; + this.m01 = 0.0f; + this.m10 = 0.0f; + this.m11 = 1.0f; + return this; + } + + // Return the inverse matrix + public Matrix2f inverse() { + + // Compute the determinant of the matrix + final float determinant = getDeterminant(); + + // Check if the determinant is equal to zero + assert (Math.abs(determinant) > Constant.FLOAT_EPSILON); + final float invDeterminant = 1.0f / determinant; + + set(this.m11, -this.m01, -this.m10, this.m00); + + // Return the inverse matrix + return multiply(invDeterminant); + } + + public Matrix2f inverseNew() { + final Matrix2f tmp = new Matrix2f(this); + tmp.inverse(); + return tmp; + } + + // Overloaded operator for the negative of the matrix + public Matrix2f invert() { + this.m00 = -this.m00; + this.m01 = -this.m01; + this.m10 = -this.m10; + this.m11 = -this.m11; + return this; + } + + // Overloaded operator for substraction with assignment + public Matrix2f less(final Matrix2f matrix) { + this.m00 -= matrix.m00; + this.m01 -= matrix.m01; + this.m10 -= matrix.m10; + this.m11 -= matrix.m11; + return this; + } + + // Overloaded operator for multiplication with a number with assignment + public Matrix2f multiply(final float number) { + this.m00 *= number; + this.m01 *= number; + this.m10 *= number; + this.m11 *= number; + return this; + } + + // Overloaded operator for matrix multiplication + public Matrix2f multiply(final Matrix2f matrix) { + set(this.m00 * matrix.m00 + this.m01 * matrix.m10, this.m00 * matrix.m01 + this.m01 * matrix.m11, this.m10 * matrix.m00 + this.m11 * matrix.m10, this.m10 * matrix.m01 + this.m11 * matrix.m11); + return this; + } + + // Overloaded operator for multiplication with a vector + public Vector2f multiplyNew(final Vector2f vector) { + return new Vector2f(this.m00 * vector.x + this.m01 * vector.y, this.m10 * vector.x + this.m11 * vector.y); + } + + // Method to set all the values in the matrix + public final Matrix2f set(final float a1, final float a2, final float b1, final float b2) { + this.m00 = a1; + this.m01 = a2; + this.m10 = b1; + this.m11 = b2; + return this; + } + + public final Matrix2f set(final Matrix2f matrix) { + this.m00 = matrix.m00; + this.m01 = matrix.m01; + this.m10 = matrix.m10; + this.m11 = matrix.m11; + return this; + } + + // Set the matrix to zero + public final Matrix2f setZero() { + this.m00 = 0.0f; + this.m01 = 0.0f; + this.m10 = 0.0f; + this.m11 = 0.0f; + return this; + } + + @Override + public String toString() { + return "(00= " + this.m00 + ", 01= " + this.m01 + ", 10= " + this.m10 + ", 11= " + this.m11 + ")"; + } + + // Return the transpose matrix + public Matrix2f transpose() { + set(this.m00, this.m10, this.m01, this.m11); + return this; + } + +} diff --git a/src/org/atriasoft/ephysics/mathematics/PairInt.java b/src/org/atriasoft/ephysics/mathematics/PairInt.java new file mode 100644 index 0000000..07a94a2 --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/PairInt.java @@ -0,0 +1,67 @@ +package org.atriasoft.ephysics.mathematics; + +import java.util.Set; + +import javafx.collections.transformation.SortedList; + +public class PairInt { + public static int countInSet(final Set values, final PairInt sample) { + int count = 0; + for (final PairInt elem : values) { + if (elem.first != sample.first) { + continue; + } + if (elem.second != sample.second) { + continue; + } + count++; + } + return count; + } + + public static int countInSet(final SortedList values, final PairInt sample) { + int count = 0; + for (final PairInt elem : values) { + if (elem.first != sample.first) { + continue; + } + if (elem.second != sample.second) { + continue; + } + count++; + } + return count; + } + + public final int first; + + public final int second; + + public PairInt(final int first, final int second) { + this.first = first; + this.second = second; + } + + @Override + public boolean equals(final Object obj) { + if (this == obj) { + return true; + } + if (obj == null || getClass() != obj.getClass()) { + return false; + } + final PairInt tmp = (PairInt) obj; + if (this.first != tmp.first) { + return false; + } + if (this.second != tmp.second) { + return false; + } + return true; + } + + @Override + public String toString() { + return "PairInt [first=" + this.first + ", second=" + this.second + "]"; + } +} diff --git a/src/org/atriasoft/ephysics/mathematics/PairIntVector3f.java b/src/org/atriasoft/ephysics/mathematics/PairIntVector3f.java new file mode 100644 index 0000000..8fb379a --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/PairIntVector3f.java @@ -0,0 +1,14 @@ +package org.atriasoft.ephysics.mathematics; + +import org.atriasoft.etk.math.Vector3f; + +public class PairIntVector3f { + public final int first; + public final Vector3f second; + + public PairIntVector3f(final int first, final Vector3f second) { + this.first = first; + this.second = second; + } + +} diff --git a/src/org/atriasoft/ephysics/mathematics/Ray.java b/src/org/atriasoft/ephysics/mathematics/Ray.java new file mode 100644 index 0000000..a097491 --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/Ray.java @@ -0,0 +1,52 @@ +package org.atriasoft.ephysics.mathematics; + +import org.atriasoft.etk.math.Vector3f; + +public class Ray { + public Vector3f point1; //! implements Iterable { + public static Comparator getCollisionBodyCoparator() { + return new Comparator() { + @Override + public int compare(final CollisionBody a, final CollisionBody b) { + if (a.getID() < b.getID()) { + return -1; + } else if (a.getID() == b.getID()) { + return 0; + } + return -1; + } + }; + } + + public static Comparator getDTreeCoparator() { + return new Comparator() { + @Override + public int compare(final DTree a, final DTree b) { + if (a.uid < b.uid) { + return -1; + } else if (a.uid == b.uid) { + return 0; + } + return -1; + } + }; + } + + public static Comparator getIntegerComparator() { + return new Comparator() { + @Override + public int compare(final Integer a, final Integer b) { + if (a < b) { + return -1; + } else if (a == b) { + return 0; + } + return -1; + } + }; + } + + public static Comparator getPairDTreeCoparator() { + return new Comparator() { + @Override + public int compare(final PairDTree _pair1, final PairDTree _pair2) { + if (_pair1.first.uid < _pair2.first.uid) { + return -1; + } + if (_pair1.first.uid == _pair2.first.uid) { + if (_pair1.second.uid < _pair2.second.uid) { + return -1; + } + if (_pair1.second.uid == _pair2.second.uid) { + return 0; + } + return +1; + } + return +1; + } + }; + } + + public static Comparator getPairIntCoparator() { + return new Comparator() { + @Override + public int compare(final PairInt _pair1, final PairInt _pair2) { + if (_pair1.first < _pair2.first) { + return -1; + } + if (_pair1.first == _pair2.first) { + if (_pair1.second < _pair2.second) { + return -1; + } + if (_pair1.second == _pair2.second) { + return 0; + } + return +1; + } + return +1; + } + }; + } + + protected final List data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ... + + protected Comparator comparator = null; + + /* + new Comparator() { + @Override + public int compare(final TYPE _pair1, final TYPE _pair2) { + if (_pair1.first < _pair2.first) { + return -1; + } + if (_pair1.first == _pair2.first) { + if (_pair1.second < _pair2.second) { + return -1; + } + if (_pair1.second == _pair2.second) { + return 0; + } + return +1; + } + return +1; + } + }; + */ + /** + * @brief Constructor of the Set table. + */ + public Set(final Comparator comparator) { + this.comparator = comparator; + } + + /** + * @brief Add an element OR set an element value + * @note add and set is the same function. + * @param[in] _key Name of the value to set in the Set table. + * @param[in] _value Value to set in the Set table. + */ + public void add(final TYPE _key) { + for (int iii = 0; iii < this.data.size(); ++iii) { + if (_key == this.data.get(iii)) { + return; + } + // For multiple element: + if (this.comparator.compare(_key, this.data.get(iii)) == 0) { + // Find a position + this.data.add(iii, _key); + return; + } + } + this.data.add(_key); + } + + /** + * @brief Remove all entry in the Set table. + * @note It does not delete pointer if your value is a pointer type... + */ + public void clear() { + this.data.clear(); + } + + /** + * @brief Count the number of occurence of a specific element. + * @param[in] _key Name of the element to count iterence + * @return 0 No element was found + * @return 1 One element was found + */ + public int count(final TYPE _key) { + // TODO: search in a dichotomic way. + for (int iii = 0; iii < this.data.size(); iii++) { + if (this.comparator.compare(this.data.get(iii), _key) == 0) { + return 1; + } + } + return 0; + } + + public void erase(final int id) { + this.data.remove(id); + } + + public void erase(final TYPE id) { + this.data.remove(id); + } + + /** + * @brief Check if an element exist or not + * @param[in] _key Name of the Set requested + * @return true if the element exist + */ + public boolean exist(final TYPE _key) { + final int it = find(_key); + if (it == -1) { + return false; + } + return true; + } + + /** + * @brief Find an element position the the Set table + * @param[in] _key Name of the element to find + * @return Iterator on the element find or end() + */ + public int find(final TYPE _key) { + // TODO: search in a dichotomic way. + for (int iii = 0; iii < this.data.size(); iii++) { + if (this.comparator.compare(this.data.get(iii), _key) == 0) { + return iii; + } + } + return -1; + } + + /** + * @brief Get Element an a special position + * @param[in] _position Position in the Set + * @return An reference on the selected element + */ + public TYPE get(final int _position) { + return this.data.get(_position); + } + + /** + * @brief Check if the container have some element + * @return true The container is empty + * @return false The container have some element + */ + public boolean isEmpty() { + return this.data.size() == 0; + } + + @Override + public Iterator iterator() { + return this.data.iterator(); + } + + /** + * @brief Remove the last element of the vector + */ + public void popBack() { + this.data.remove(this.data.size() - 1); + } + + /** + * @brief Remove the first element of the vector + */ + public void popFront() { + this.data.remove(0); + } + + /** + * @brief: QuickSort implementation of sorting vector (all elements. + * @param[in,out] _data Vector to sort. + * @param[in] _high Comparator function of this element. + */ + void quickSort(final List _data, final Comparator _comparator) { + quickSort2(_data, 0, _data.size() - 1, _comparator); + } + + /** + * @brief: QuickSort implementation of sorting vector. + * @param[in,out] _data Vector to sort. + * @param[in] _low Lowest element to sort. + * @param[in] _high Highest element to sort. + * @param[in] _high Comparator function of this element. + */ + void quickSort(final List _data, final int _low, int _high, final Comparator _comparator) { + if (_high >= _data.size()) { + _high = _data.size() - 1; + } + /*if (_low >= _data.size()) { + _low = _data.size()-1; + }*/ + quickSort2(_data, _low, _high, _comparator); + } + + private void quickSort2(final List _data, final int _low, final int _high, final Comparator _comparator) { + if (_low >= _high) { + return; + } + // pi is partitioning index, arr[p] is now at right place + final int pi = quickSortPartition(_data, _low, _high, _comparator); + // Separately sort elements before partition and after partition + //if (pi != 0) { + quickSort2(_data, _low, pi - 1, _comparator); + //} + quickSort2(_data, pi + 1, _high, _comparator); + } + + private int quickSortPartition(final List _data, final int _low, final int _high, final Comparator _comparator) { + int iii = (_low - 1); + for (int jjj = _low; jjj < _high; ++jjj) { + if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) { + iii++; + Collections.swap(_data, iii, jjj); + } + } + Collections.swap(_data, iii + 1, _high); + return (iii + 1); + } + + public void remove(final TYPE obj) { + this.data.remove(obj); + } + + /** + * @brief Set the comparator of the set. + * @param[in] _comparator comparing function. + */ + public void setComparator(final Comparator _comparator) { + this.comparator = _comparator; + sort(); + } + + /** + * @brief Get the number of element in the Set table + * @return number of elements + */ + public int size() { + return this.data.size(); + } + + /** + * @brief Order the Set with the corect functor + */ + private void sort() { + quickSort(this.data, this.comparator); + } +} diff --git a/src/org/atriasoft/ephysics/mathematics/SetInteger.java b/src/org/atriasoft/ephysics/mathematics/SetInteger.java new file mode 100644 index 0000000..be1aca0 --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/SetInteger.java @@ -0,0 +1,212 @@ +package org.atriasoft.ephysics.mathematics; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; + +public class SetInteger { + private static void quickSort2(final List _data, final int _low, final int _high, final Comparator _comparator) { + if (_low >= _high) { + return; + } + // pi is partitioning index, arr[p] is now at right place + final int pi = quickSortPartition(_data, _low, _high, _comparator); + // Separately sort elements before partition and after partition + //if (pi != 0) { + quickSort2(_data, _low, pi - 1, _comparator); + //} + quickSort2(_data, pi + 1, _high, _comparator); + } + + private static int quickSortPartition(final List _data, final int _low, final int _high, final Comparator _comparator) { + int iii = (_low - 1); + for (int jjj = _low; jjj < _high; ++jjj) { + if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) { + iii++; + Collections.swap(_data, iii, jjj); + } + } + Collections.swap(_data, iii + 1, _high); + return (iii + 1); + } + + private final List data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ... + + private Comparator comparator = new Comparator() { + @Override + public int compare(final Integer a, final Integer b) { + if (a < b) { + return -1; + } else if (a == b) { + return 0; + } + return -1; + } + }; + + /** + * @brief Constructor of the Set table. + */ + public SetInteger() { + + } + + /** + * @brief Add an element OR set an element value + * @note add and set is the same function. + * @param[in] _key Name of the value to set in the Set table. + * @param[in] _value Value to set in the Set table. + */ + public void add(final Integer _key) { + for (int iii = 0; iii < this.data.size(); ++iii) { + if (_key == this.data.get(iii)) { + return; + } + if (this.comparator.compare(_key, this.data.get(iii)) == 0) { + // Find a position + this.data.add(iii, _key); + return; + } + } + this.data.add(_key); + } + + /** + * @brief Remove all entry in the Set table. + * @note It does not delete pointer if your value is a pointer type... + */ + public void clear() { + this.data.clear(); + } + + /** + * @brief Count the number of occurence of a specific element. + * @param[in] _key Name of the element to count iterence + * @return 0 No element was found + * @return 1 One element was found + */ + public int count(final Integer _key) { + // TODO: search in a dichotomic way. + for (int iii = 0; iii < this.data.size(); iii++) { + if (this.comparator.compare(this.data.get(iii), _key) == 0) { + return 1; + } + } + return 0; + } + + /** + * @brief Check if an element exist or not + * @param[in] _key Name of the Set requested + * @return true if the element exist + */ + public boolean exist(final Integer _key) { + final int it = find(_key); + if (it == -1) { + return false; + } + return true; + } + + /** + * @brief Find an element position the the Set table + * @param[in] _key Name of the element to find + * @return Iterator on the element find or end() + */ + public int find(final Integer _key) { + // TODO: search in a dichotomic way. + for (int iii = 0; iii < this.data.size(); iii++) { + if (this.comparator.compare(this.data.get(iii), _key) == 0) { + return iii; + } + } + return -1; + } + + /** + * @brief Get Element an a special position + * @param[in] _position Position in the Set + * @return An reference on the selected element + */ + public Integer get(final int _position) { + return this.data.get(_position); + } + + public List getRaw() { + return this.data; + } + + /** + * @brief Check if the container have some element + * @return true The container is empty + * @return false The container have some element + */ + public boolean isEmpty() { + return this.data.size() == 0; + } + + /** + * @brief Remove the last element of the vector + */ + public void popBack() { + this.data.remove(this.data.size() - 1); + } + + /** + * @brief Remove the first element of the vector + */ + public void popFront() { + this.data.remove(0); + } + + /** + * @brief: QuickSort implementation of sorting vector (all elements. + * @param[in,out] _data Vector to sort. + * @param[in] _high Comparator function of this element. + */ + void quickSort(final List _data, final Comparator _comparator) { + quickSort2(_data, 0, _data.size() - 1, _comparator); + } + + /** + * @brief: QuickSort implementation of sorting vector. + * @param[in,out] _data Vector to sort. + * @param[in] _low Lowest element to sort. + * @param[in] _high Highest element to sort. + * @param[in] _high Comparator function of this element. + */ + void quickSort(final List _data, final int _low, int _high, final Comparator _comparator) { + if (_high >= _data.size()) { + _high = _data.size() - 1; + } + /*if (_low >= _data.size()) { + _low = _data.size()-1; + }*/ + quickSort2(_data, _low, _high, _comparator); + } + + /** + * @brief Set the comparator of the set. + * @param[in] _comparator comparing function. + */ + public void setComparator(final Comparator _comparator) { + this.comparator = _comparator; + sort(); + } + + /** + * @brief Get the number of element in the Set table + * @return number of elements + */ + public int size() { + return this.data.size(); + } + + /** + * @brief Order the Set with the corect functor + */ + private void sort() { + quickSort(this.data, this.comparator); + } +} diff --git a/src/org/atriasoft/ephysics/mathematics/SetMultiple.java b/src/org/atriasoft/ephysics/mathematics/SetMultiple.java new file mode 100644 index 0000000..08f660f --- /dev/null +++ b/src/org/atriasoft/ephysics/mathematics/SetMultiple.java @@ -0,0 +1,41 @@ +package org.atriasoft.ephysics.mathematics; + +import java.util.Comparator; + +public class SetMultiple extends Set { + /** + * @brief Constructor of the Set table. + */ + public SetMultiple(final Comparator comparator) { + super(comparator); + } + + /** + * @brief Add an element OR set an element value + * @note add and set is the same function. + * @param[in] _key Name of the value to set in the Set table. + * @param[in] _value Value to set in the Set table. + */ + @Override + public void add(final TYPE _key) { + for (int iii = 0; iii < this.data.size(); ++iii) { + if (_key == this.data.get(iii)) { + return; + } + final int compareValue = this.comparator.compare(_key, this.data.get(iii)); + if (compareValue == 0) { + // Find a position + this.data.set(iii, _key); + return; + } + // for single Element + if (compareValue == 1) { + // Find a position + this.data.add(iii, _key); + return; + } + } + this.data.add(_key); + } + +}