diff --git a/src/org/atriaSoft/ephysics/ContactsPositionCorrectionTechnique.java b/src/org/atriaSoft/ephysics/ContactsPositionCorrectionTechnique.java new file mode 100644 index 0000000..7c4cd0e --- /dev/null +++ b/src/org/atriaSoft/ephysics/ContactsPositionCorrectionTechnique.java @@ -0,0 +1,12 @@ +package org.atriaSoft.ephysics; + +/// Position correction technique used in the contact solver (for contacts) +/// BAUMGARTECONTACTS : 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). +/// SPLITIMPULSES : 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 { + BAUMGARTECONTACTS, + SPLITIMPULSES, +} diff --git a/src/org/atriaSoft/ephysics/JointsPositionCorrectionTechnique.java b/src/org/atriaSoft/ephysics/JointsPositionCorrectionTechnique.java new file mode 100644 index 0000000..d4e3dc4 --- /dev/null +++ b/src/org/atriaSoft/ephysics/JointsPositionCorrectionTechnique.java @@ -0,0 +1,9 @@ +package org.atriaSoft.ephysics; + +/// Position correction technique used in the raint solver (for joints). +/// BAUMGARTEJOINTS : Faster but can be innacurate in some situations. +/// NONLINEARGAUSSSEIDEL : Slower but more precise. This is the option used by default. +public enum JointsPositionCorrectionTechnique { + BAUMGARTEJOINTS, + NONLINEARGAUSSSEIDEL +} diff --git a/src/org/atriaSoft/ephysics/Log.java b/src/org/atriaSoft/ephysics/Log.java new file mode 100644 index 0000000..f81a961 --- /dev/null +++ b/src/org/atriaSoft/ephysics/Log.java @@ -0,0 +1,29 @@ +package org.atriaSoft.ephysics; + +public class Log { + private static String LIBNAME = "ephysic"; + public static void print(String data) { + System.out.println(data); + } + public static void critical(String data) { + System.out.println("[C] " + LIBNAME + " | " + data); + } + public static void error(String data) { + System.out.println("[E] " + LIBNAME + " | " + data); + } + public static void warning(String data) { + System.out.println("[W] " + LIBNAME + " | " + data); + } + public static void info(String data) { + System.out.println("[I] " + LIBNAME + " | " + data); + } + public static void debug(String data) { + System.out.println("[D] " + LIBNAME + " | " + data); + } + public static void verbose(String data) { + System.out.println("[V] " + LIBNAME + " | " + data); + } + public static void todo(String data) { + System.out.println("[TODO] " + LIBNAME + " | " + data); + } +} diff --git a/src/org/atriaSoft/ephysics/Property.java b/src/org/atriaSoft/ephysics/Property.java new file mode 100644 index 0000000..ba1f2e4 --- /dev/null +++ b/src/org/atriaSoft/ephysics/Property.java @@ -0,0 +1,70 @@ +package org.atriaSoft.ephysics; + +public class Property { + // ------------------- Type definitions ------------------- // + //typedef Pair longpair; + // ------------------- Constants ------------------- // + public final static float FLTEPSILON = 0.00001f; // TODO: check this ... + /// Pi ant + public final static float PI = 3.14159265f; + + /// 2*Pi ant + public final static float PITIMES2 = 6.28318530f; + + /// Default friction coefficient for a rigid body + public final static float DEFAULTFRICTIONCOEFFICIENT = 0.3f; + + /// Default bounciness factor for a rigid body + public final static float DEFAULTBOUNCINESS = 0.5f; + + /// Default rolling resistance + public final static float DEFAULTROLLINGRESISTANCE = 0.0f; + + /// True if the spleeping technique is enabled + public final static boolean SPLEEPINGENABLED = true; + + /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) + public final static float OBJECTMARGIN = 0.04f; + + /// Distance threshold for two contact points for a valid persistent contact (in meters) + public final static float PERSISTENTCONTACTDISTTHRESHOLD = 0.03f; + + /// Velocity threshold for contact velocity restitution + public final static float RESTITUTIONVELOCITYTHRESHOLD = 1.0f; + + /// Number of iterations when solving the velocity raints of the Sequential Impulse technique + public final static int DEFAULTVELOCITYSOLVERNBITERATIONS = 10; + + /// Number of iterations when solving the position raints of the Sequential Impulse technique + public final static int DEFAULTPOSITIONSOLVERNBITERATIONS = 5; + + /// Time (in seconds) that a body must stay still to be considered sleeping + public final static float DEFAULTTIMEBEFORESLEEP = 1.0f; + + /// A body with a linear velocity smaller than the sleep linear velocity (in m/s) + /// might enter sleeping mode. + public final static float DEFAULTSLEEPLINEARVELOCITY = 0.02f; + + /// A body with angular velocity smaller than the sleep angular velocity (in rad/s) + /// might enter sleeping mode + public final static float DEFAULTSLEEPANGULARVELOCITY = 3.0f * (PI / 180.0f); + + /// 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 final static float DYNAMICTREEAABBGAP = 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 final static float DYNAMICTREEAABBLINGAPMULTIPLIER = 1.7f; + + /// Maximum number of contact manifolds in an overlapping pair that involves two + /// convex collision shapes. + public final static int NBMAXCONTACTMANIFOLDSCONVEXSHAPE = 1; + + /// Maximum number of contact manifolds in an overlapping pair that involves at + /// least one concave collision shape. + public final static int NBMAXCONTACTMANIFOLDSCONCAVESHAPE = 3; +} + diff --git a/src/org/atriaSoft/ephysics/body/Body.java b/src/org/atriaSoft/ephysics/body/Body.java index 1173503..7d275ab 100644 --- a/src/org/atriaSoft/ephysics/body/Body.java +++ b/src/org/atriaSoft/ephysics/body/Body.java @@ -112,7 +112,7 @@ class Body { * @return true if the current element is smaller */ public boolean isLess( Body obj) { - return (this.id < obj.this.id); + return (this.id < obj.id); } /** * @brief Larger than operator @@ -120,7 +120,7 @@ class Body { * @return true if the current element is Bigger */ public boolean isUpper( Body obj) { - return (this.id > obj.this.id); + return (this.id > obj.id); } /** * @brief Equal operator @@ -128,7 +128,7 @@ class Body { * @return true if the curretn element is equal */ public boolean isEqual( Body obj) { - return (this.id == obj.this.id); + return (this.id == obj.id); } /** * @brief Not equal operator @@ -136,6 +136,6 @@ class Body { * @return true if the curretn element is NOT equal */ public boolean isDifferent( Body obj) { - return (this.id != obj.this.id); + return (this.id != obj.id); } } diff --git a/src/org/atriaSoft/ephysics/body/BodyType.java b/src/org/atriaSoft/ephysics/body/BodyType.java new file mode 100644 index 0000000..2097aa0 --- /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.cpp b/src/org/atriaSoft/ephysics/body/CollisionBody.cpp deleted file mode 100644 index 51e158e..0000000 --- a/src/org/atriaSoft/ephysics/body/CollisionBody.cpp +++ /dev/null @@ -1,238 +0,0 @@ -/** @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) - */ -#include -#include -#include - -// We want to use the ReactPhysics3D namespace -using namespace ephysics; - - -CollisionBody::CollisionBody( etk::Transform3D transform, CollisionWorld world, long id): - Body(id), - this.type(DYNAMIC), - this.transform(transform), - this.proxyCollisionShapes(null), - this.numberCollisionShapes(0), - this.contactManifoldsList(null), - this.world(world) { - - Log.debug(" set transform: " << transform); - if (isnan(transform.getPosition().x()) == true) { // check NAN - Log.critical(" set transform: " << transform); - } - if (isinf(transform.getOrientation().z()) == true) { - Log.critical(" set transform: " << transform); - } -} - -CollisionBody::~CollisionBody() { - assert(this.contactManifoldsList == null); - - // Remove all the proxy collision shapes of the body - removeAllCollisionShapes(); -} - -inline void CollisionBody::setType(BodyType type) { - this.type = type; - if (this.type == STATIC) { - // Update the broad-phase state of the body - updateBroadPhaseState(); - } -} - -void CollisionBody::setTransform( etk::Transform3D transform) { - Log.debug(" set transform: " << this.transform << " ==> " << transform); - if (isnan(transform.getPosition().x()) == true) { // check NAN - Log.critical(" set transform: " << this.transform << " ==> " << transform); - } - if (isinf(transform.getOrientation().z()) == true) { - Log.critical(" set transform: " << this.transform << " ==> " << transform); - } - this.transform = transform; - updateBroadPhaseState(); -} - -ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, - etk::Transform3D transform) { - // Create a proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape,transform, float(1)); - // Add it to the list of proxy collision shapes of the body - if (this.proxyCollisionShapes == null) { - this.proxyCollisionShapes = proxyShape; - } else { - proxyShape->this.next = this.proxyCollisionShapes; - this.proxyCollisionShapes = proxyShape; - } - AABB aabb; - collisionShape->computeAABB(aabb, this.transform * transform); - this.world.this.collisionDetection.addProxyCollisionShape(proxyShape, aabb); - this.numberCollisionShapes++; - return proxyShape; -} - -void CollisionBody::removeCollisionShape( ProxyShape* proxyShape) { - ProxyShape* current = this.proxyCollisionShapes; - // If the the first proxy shape is the one to remove - if (current == proxyShape) { - this.proxyCollisionShapes = current->this.next; - if (this.isActive) { - this.world.this.collisionDetection.removeProxyCollisionShape(current); - } - ETKDELETE(ProxyShape, current); - current = null; - this.numberCollisionShapes--; - return; - } - // Look for the proxy shape that contains the collision shape in parameter - while(current->this.next != null) { - // If we have found the collision shape to remove - if (current->this.next == proxyShape) { - // Remove the proxy collision shape - ProxyShape* elementToRemove = current->this.next; - current->this.next = elementToRemove->this.next; - if (this.isActive) { - this.world.this.collisionDetection.removeProxyCollisionShape(elementToRemove); - } - ETKDELETE(ProxyShape, elementToRemove); - elementToRemove = null; - this.numberCollisionShapes--; - return; - } - // Get the next element in the list - current = current->this.next; - } -} - - -void CollisionBody::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 - ProxyShape* nextElement = current->this.next; - if (this.isActive) { - this.world.this.collisionDetection.removeProxyCollisionShape(current); - } - ETKDELETE(ProxyShape, current); - // Get the next element in the list - current = nextElement; - } - this.proxyCollisionShapes = null; -} - - -void CollisionBody::resetContactManifoldsList() { - // Delete the linked list of contact manifolds of that body - ContactManifoldListElement* currentElement = this.contactManifoldsList; - while (currentElement != null) { - ContactManifoldListElement* nextElement = currentElement->next; - // Delete the current element - ETKDELETE(ContactManifoldListElement, currentElement); - currentElement = nextElement; - } - this.contactManifoldsList = null; -} - - -void CollisionBody::updateBroadPhaseState() { - // For all the proxy collision shapes of the body - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - // Update the proxy - updateProxyShapeInBroadPhase(shape); - } -} - - -void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert) { - AABB aabb; - proxyShape->getCollisionShape()->computeAABB(aabb, this.transform * proxyShape->getLocalToBodyTransform()); - this.world.this.collisionDetection.updateProxyCollisionShape(proxyShape, aabb, vec3(0, 0, 0), forceReinsert); -} - - -void CollisionBody::setIsActive(boolean isActive) { - // If the state does not change - if (this.isActive == isActive) { - return; - } - Body::setIsActive(isActive); - // If we have to activate the body - if (isActive == true) { - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, this.transform * shape->this.localToBodyTransform); - this.world.this.collisionDetection.addProxyCollisionShape(shape, aabb); - } - } else { - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - this.world.this.collisionDetection.removeProxyCollisionShape(shape); - } - resetContactManifoldsList(); - } -} - - -void CollisionBody::askForBroadPhaseCollisionCheck() { - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - this.world.this.collisionDetection.askForBroadPhaseCollisionCheck(shape); - } -} - - -int CollisionBody::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->this.isAlreadyInIsland = false; - currentElement = currentElement->next; - nbManifolds++; - } - return nbManifolds; -} - -boolean CollisionBody::testPointInside( vec3 worldPoint) { - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - if (shape->testPointInside(worldPoint)) return true; - } - return false; -} - -boolean CollisionBody::raycast( Ray ray, RaycastInfo raycastInfo) { - if (this.isActive == false) { - return false; - } - boolean isHit = false; - Ray rayTemp(ray); - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - // Test if the ray hits the collision shape - if (shape->raycast(rayTemp, raycastInfo)) { - rayTemp.maxFraction = raycastInfo.hitFraction; - isHit = true; - } - } - return isHit; -} - -AABB CollisionBody::getAABB() { - AABB bodyAABB; - if (this.proxyCollisionShapes == null) { - return bodyAABB; - } - this.proxyCollisionShapes->getCollisionShape()->computeAABB(bodyAABB, this.transform * this.proxyCollisionShapes->getLocalToBodyTransform()); - for (ProxyShape* shape = this.proxyCollisionShapes->this.next; shape != null; shape = shape->this.next) { - AABB aabb; - shape->getCollisionShape()->computeAABB(aabb, this.transform * shape->getLocalToBodyTransform()); - bodyAABB.mergeWithAABB(aabb); - } - return bodyAABB; -} - diff --git a/src/org/atriaSoft/ephysics/body/CollisionBody.hpp b/src/org/atriaSoft/ephysics/body/CollisionBody.hpp deleted file mode 100644 index 8da0d5b..0000000 --- a/src/org/atriaSoft/ephysics/body/CollisionBody.hpp +++ /dev/null @@ -1,209 +0,0 @@ -/** @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) - */ -#pragma once -#include -#include -#include -#include -#include -#include - -namespace ephysics { - struct ContactManifoldListElement; - class ProxyShape; - class CollisionWorld; - /** - * @brief Define the type of the body - */ - 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. - }; - /** - * @brief This class represents a body that is able to collide with others bodies. This class inherits from the Body class. - */ - class CollisionBody : public Body { - protected : - BodyType this.type; //!< Type of body (static, kinematic or dynamic) - etk::Transform3D this.transform; //!< Position and orientation of the body - ProxyShape* this.proxyCollisionShapes; //!< First element of the linked list of proxy collision shapes of this body - int this.numberCollisionShapes; //!< Number of collision shapes - ContactManifoldListElement* this.contactManifoldsList; //!< First element of the linked list of contact manifolds involving this body - CollisionWorld this.world; //!< Reference to the world the body belongs to - /// Private copy-ructor - CollisionBody( CollisionBody body) = delete; - /// Private assignment operator - CollisionBody operator=( CollisionBody body) = delete; - /** - * @brief Reset the contact manifold lists - */ - void resetContactManifoldsList(); - /** - * @brief Remove all the collision shapes - */ - void removeAllCollisionShapes(); - /** - * @brief Update the broad-phase state for this body (because it has moved for instance) - */ - virtual void updateBroadPhaseState() ; - /** - * @brief Update the broad-phase state of a proxy collision shape of the body - */ - void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert = false) ; - /** - * @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved). - */ - void askForBroadPhaseCollisionCheck() ; - /** - * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds. - * This method also returns the number of contact manifolds of the body. - */ - int resetIsAlreadyInIslandAndCountManifolds(); - public : - /** - * @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 - */ - CollisionBody( etk::Transform3D transform, CollisionWorld world, long id); - /** - * @brief Destructor - */ - virtual ~CollisionBody(); - /** - * @brief Return the type of the body - * @return the type of the body (STATIC, KINEMATIC, DYNAMIC) - */ - BodyType getType() { - return this.type; - } - /** - * @brief Set the type of the body - * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC) - */ - virtual void setType(BodyType type); - /** - * @brief Set whether or not the body is active - * @param[in] isActive True if you want to activate the body - */ - virtual void setIsActive(boolean isActive); - /** - * @brief Return the current position and orientation - * @return The current transformation of the body that transforms the local-space of the body into world-space - */ - etk::Transform3D getTransform() { - return this.transform; - } - /** - * @brief Set the current position and orientation - * @param transform The transformation of the body that transforms the local-space of the body into world-space - */ - virtual void setTransform( etk::Transform3D 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 into 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. - */ - ProxyShape* addCollisionShape(CollisionShape* collisionShape, etk::Transform3D transform); - /** - * @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 - */ - virtual void removeCollisionShape( ProxyShape* proxyShape); - /** - * @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 - */ - ContactManifoldListElement* getContactManifoldsList() { - return this.contactManifoldsList; - } - /** - * @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 - */ - boolean testPointInside( vec3 worldPoint) ; - /** - * @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 - */ - boolean raycast( Ray ray, RaycastInfo raycastInfo); - /** - * @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 - */ - AABB getAABB() ; - /** - * @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 - */ - ProxyShape* getProxyShapesList() { - return this.proxyCollisionShapes; - } - /** - * @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 - */ - ProxyShape* getProxyShapesList() { - return this.proxyCollisionShapes; - } - /** - * @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 - */ - vec3 getWorldPoint( vec3 localPoint) { - return this.transform * 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 - */ - vec3 getWorldVector( vec3 localVector) { - return this.transform.getOrientation() * localVector; - } - /** - * @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 - */ - vec3 getLocalPoint( vec3 worldPoint) { - return this.transform.getInverse() * 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 - */ - vec3 getLocalVector( vec3 worldVector) { - return this.transform.getOrientation().getInverse() * worldVector; - } - friend class CollisionWorld; - friend class DynamicsWorld; - friend class CollisionDetection; - friend class BroadPhaseAlgorithm; - friend class ConvexMeshShape; - friend class ProxyShape; - }; -} - diff --git a/src/org/atriaSoft/ephysics/body/CollisionBody.java b/src/org/atriaSoft/ephysics/body/CollisionBody.java new file mode 100644 index 0000000..5808b7f --- /dev/null +++ b/src/org/atriaSoft/ephysics/body/CollisionBody.java @@ -0,0 +1,345 @@ +package org.atriaSoft.ephysics.body; + +import org.atriaSoft.etk.math.Transform3D; + +/** + * @brief This class represents a body that is able to collide with others bodies. This class inherits from the Body class. + */ +class CollisionBody extends Body { + protected BodyType type; //!< Type of body (static, kinematic or dynamic) + protected Transform3D transform; //!< Position and orientation of the body + protected ProxyShape proxyCollisionShapes; //!< First element of the linked list of proxy collision shapes of this body + protected int numberCollisionShapes; //!< Number of collision shapes + protected ContactManifoldListElement* contactManifoldsList; //!< First element of the linked list of contact manifolds involving this body + protected CollisionWorld world; //!< Reference to the world the body belongs to + /** + * @brief Reset the contact manifold lists + */ + protected void resetContactManifoldsList() { + // Delete the linked list of contact manifolds of that body + ContactManifoldListElement* currentElement = this.contactManifoldsList; + while (currentElement != null) { + ContactManifoldListElement* nextElement = currentElement.next; + // Delete the current element + ETKDELETE(ContactManifoldListElement, currentElement); + currentElement = nextElement; + } + this.contactManifoldsList = null; + } + /** + * @brief Remove all the collision shapes + */ + protected 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 + ProxyShape* nextElement = current.this.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(current); + } + ETKDELETE(ProxyShape, current); + // Get the next element in the list + current = nextElement; + } + this.proxyCollisionShapes = null; + } + /** + * @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.this.next) { + // Update the proxy + updateProxyShapeInBroadPhase(shape); + } + } + /** + * @brief Update the broad-phase state of a proxy collision shape of the body + */ + protected void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, boolean forceReinsert = false) { + AABB aabb; + proxyShape.getCollisionShape().computeAABB(aabb, this.transform * proxyShape.getLocalToBodyTransform()); + this.world.collisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3f(0, 0, 0), forceReinsert); + } + /** + * @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.this.next) { + this.world.collisionDetection.askForBroadPhaseCollisionCheck(shape); + } + } + /** + * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds. + * This method also returns the number of contact manifolds of the body. + */ + protected 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.this.isAlreadyInIsland = false; + currentElement = currentElement.next; + nbManifolds++; + } + return nbManifolds; + } + /** + * @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( Transform3D transform, CollisionWorld world, long id) { + super(id); + this.type = DYNAMIC; + this.transform = transform; + this.proxyCollisionShapes = null; + this.numberCollisionShapes = 0; + this.contactManifoldsList = null; + this.world(world); + + Log.debug(" set transform: " + transform); + if (isnan(transform.getPosition().x()) == true) { // check NAN + Log.critical(" set transform: " + transform); + } + if (isinf(transform.getOrientation().z()) == true) { + Log.critical(" set transform: " + transform); + } + } + /** + * @brief Return the type of the body + * @return the type of the body (STATIC, KINEMATIC, DYNAMIC) + */ + public BodyType getType() { + return this.type; + } + /** + * @brief Set the type of the body + * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC) + */ + public void setType(BodyType type) { + this.type = type; + if (this.type == STATIC) { + // Update the broad-phase state of the body + updateBroadPhaseState(); + } + } + /** + * @brief Set whether or not the body is active + * @param[in] isActive True if you want to activate the body + */ + public void setIsActive(boolean isActive) { + // If the state does not change + if (this.isActive == isActive) { + return; + } + Body::setIsActive(isActive); + // If we have to activate the body + if (isActive == true) { + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + AABB aabb; + shape.getCollisionShape().computeAABB(aabb, this.transform * shape.this.localToBodyTransform); + this.world.collisionDetection.addProxyCollisionShape(shape, aabb); + } + } else { + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + this.world.collisionDetection.removeProxyCollisionShape(shape); + } + resetContactManifoldsList(); + } + } + /** + * @brief Return the current position and orientation + * @return The current transformation of the body that transforms the local-space of the body into world-space + */ + public Transform3D getTransform() { + return this.transform; + } + /** + * @brief Set the current position and orientation + * @param transform The transformation of the body that transforms the local-space of the body into world-space + */ + public void setTransform( Transform3D transform) { + Log.debug(" set transform: " + this.transform + " ==> " + transform); + if (isnan(transform.getPosition().x()) == true) { // check NAN + Log.critical(" set transform: " + this.transform + " ==> " + transform); + } + if (isinf(transform.getOrientation().z()) == true) { + Log.critical(" set transform: " + this.transform + " ==> " + transform); + } + this.transform = transform; + updateBroadPhaseState(); + } + /** + * @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 into 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(CollisionShape collisionShape, Transform3D transform) { + // Create a proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = ETKNEW(ProxyShape, this, collisionShape,transform, float(1)); + // Add it to the list of proxy collision shapes of the body + if (this.proxyCollisionShapes == null) { + this.proxyCollisionShapes = proxyShape; + } else { + proxyShape.this.next = this.proxyCollisionShapes; + this.proxyCollisionShapes = proxyShape; + } + AABB aabb; + collisionShape.computeAABB(aabb, this.transform * transform); + this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb); + this.numberCollisionShapes++; + return proxyShape; + } + /** + * @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(ProxyShape proxyShape) { + ProxyShape* current = this.proxyCollisionShapes; + // If the the first proxy shape is the one to remove + if (current == proxyShape) { + this.proxyCollisionShapes = current.this.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(current); + } + ETKDELETE(ProxyShape, current); + current = null; + this.numberCollisionShapes--; + return; + } + // Look for the proxy shape that contains the collision shape in parameter + while(current.this.next != null) { + // If we have found the collision shape to remove + if (current.this.next == proxyShape) { + // Remove the proxy collision shape + ProxyShape* elementToRemove = current.this.next; + current.this.next = elementToRemove.this.next; + if (this.isActive) { + this.world.collisionDetection.removeProxyCollisionShape(elementToRemove); + } + ETKDELETE(ProxyShape, elementToRemove); + elementToRemove = null; + this.numberCollisionShapes--; + return; + } + // Get the next element in the list + current = current.this.next; + } + } + /** + * @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 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( Vector3f worldPoint) { + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + if (shape.testPointInside(worldPoint)) return true; + } + return false; + } + /** + * @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( Ray ray, RaycastInfo raycastInfo) { + if (this.isActive == false) { + return false; + } + boolean isHit = false; + Ray rayTemp(ray); + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + // Test if the ray hits the collision shape + if (shape.raycast(rayTemp, raycastInfo)) { + rayTemp.maxFraction = raycastInfo.hitFraction; + isHit = true; + } + } + return isHit; + } + /** + * @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() { + AABB bodyAABB; + if (this.proxyCollisionShapes == null) { + return bodyAABB; + } + this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform * this.proxyCollisionShapes.getLocalToBodyTransform()); + for (ProxyShape* shape = this.proxyCollisionShapes.this.next; shape != null; shape = shape.this.next) { + AABB aabb; + shape.getCollisionShape().computeAABB(aabb, this.transform * shape.getLocalToBodyTransform()); + bodyAABB.mergeWithAABB(aabb); + } + return bodyAABB; + } + /** + * @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 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 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( Vector3f localPoint) { + return this.transform * 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( Vector3f localVector) { + return this.transform.getOrientation() * localVector; + } + /** + * @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( Vector3f worldPoint) { + return this.transform.getInverse() * 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( Vector3f worldVector) { + return this.transform.getOrientation().getInverse() * worldVector; + } +} + diff --git a/src/org/atriaSoft/ephysics/body/RigidBody.cpp b/src/org/atriaSoft/ephysics/body/RigidBody.cpp deleted file mode 100644 index 0a366d6..0000000 --- a/src/org/atriaSoft/ephysics/body/RigidBody.cpp +++ /dev/null @@ -1,316 +0,0 @@ -/** @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) - */ -#include -#include -#include -#include -#include - -using namespace ephysics; - - -RigidBody::RigidBody( etk::Transform3D transform, CollisionWorld world, long id): - CollisionBody(transform, world, id), - this.initMass(1.0f), - this.centerOfMassLocal(0, 0, 0), - this.centerOfMassWorld(transform.getPosition()), - this.isGravityEnabled(true), - this.linearDamping(0.0f), - this.angularDamping(float(0.0)), - this.jointsList(null) { - // Compute the inverse mass - this.massInverse = 1.0f / this.initMass; -} - -RigidBody::~RigidBody() { - assert(this.jointsList == null); -} - - -void RigidBody::setType(BodyType type) { - if (this.type == type) { - return; - } - CollisionBody::setType(type); - recomputeMassInformation(); - if (this.type == STATIC) { - // Reset the velocity to zero - this.linearVelocity.setZero(); - this.angularVelocity.setZero(); - } - if ( this.type == STATIC - || this.type == 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.getInverse(); - } - 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(); -} - - -void RigidBody::setInertiaTensorLocal( etk::Matrix3x3 inertiaTensorLocal) { - if (this.type != DYNAMIC) { - return; - } - this.inertiaTensorLocal = inertiaTensorLocal; - this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse(); -} - - -void RigidBody::setCenterOfMassLocal( vec3 centerOfMassLocal) { - if (this.type != DYNAMIC) { - return; - } - vec3 oldCenterOfMass = this.centerOfMassWorld; - this.centerOfMassLocal = centerOfMassLocal; - this.centerOfMassWorld = this.transform * this.centerOfMassLocal; - this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); -} - - -void RigidBody::setMass(float mass) { - if (this.type != 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; - } -} - -void RigidBody::removeJointFrothis.jointsList( 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; - ETKDELETE(JointListElement, elementToRemove); - 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; - ETKDELETE(JointListElement, elementToRemove); - elementToRemove = null; - break; - } - currentElement = currentElement->next; - } - } -} - - -ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, - etk::Transform3D transform, - float mass) { - assert(mass > 0.0f); - // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = ETKNEW(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->this.next = this.proxyCollisionShapes; - this.proxyCollisionShapes = proxyShape; - } - // Compute the world-space AABB of the new collision shape - AABB aabb; - collisionShape->computeAABB(aabb, this.transform * transform); - // Notify the collision detection about this new collision shape - this.world.this.collisionDetection.addProxyCollisionShape(proxyShape, aabb); - this.numberCollisionShapes++; - recomputeMassInformation(); - return proxyShape; -} - -void RigidBody::removeCollisionShape( ProxyShape* proxyShape) { - CollisionBody::removeCollisionShape(proxyShape); - recomputeMassInformation(); -} - - -void RigidBody::setLinearVelocity( vec3 linearVelocity) { - if (this.type == STATIC) { - return; - } - this.linearVelocity = linearVelocity; - if (this.linearVelocity.length2() > 0.0f) { - setIsSleeping(false); - } -} - - -void RigidBody::setAngularVelocity( vec3 angularVelocity) { - if (this.type == STATIC) { - return; - } - this.angularVelocity = angularVelocity; - if (this.angularVelocity.length2() > 0.0f) { - setIsSleeping(false); - } -} - -void RigidBody::setIsSleeping(boolean isSleeping) { - if (isSleeping) { - this.linearVelocity.setZero(); - this.angularVelocity.setZero(); - this.externalForce.setZero(); - this.externalTorque.setZero(); - } - Body::setIsSleeping(isSleeping); -} - -void RigidBody::updateTransformWithCenterOfMass() { - // Translate the body according to the translation of the center of mass position - this.transform.setPosition(this.centerOfMassWorld - this.transform.getOrientation() * this.centerOfMassLocal); - if (isnan(this.transform.getPosition().x()) == true) { - Log.critical("updateTransformWithCenterOfMass: " << this.transform); - } - if (isinf(this.transform.getOrientation().z()) == true) { - Log.critical(" set transform: " << this.transform); - } -} - -void RigidBody::setTransform( etk::Transform3D transform) { - Log.debug(" set transform: " << this.transform << " ==> " << transform); - if (isnan(transform.getPosition().x()) == true) { - Log.critical(" set transform: " << this.transform << " ==> " << transform); - } - if (isinf(transform.getOrientation().z()) == true) { - Log.critical(" set transform: " << this.transform << " ==> " << transform); - } - this.transform = transform; - vec3 oldCenterOfMass = this.centerOfMassWorld; - // Compute the new center of mass in world-space coordinates - this.centerOfMassWorld = this.transform * this.centerOfMassLocal; - // Update the linear velocity of the center of mass - this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); - updateBroadPhaseState(); -} - -void RigidBody::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 == STATIC || this.type == KINEMATIC) { - this.centerOfMassWorld = this.transform.getPosition(); - return; - } - assert(this.type == DYNAMIC); - // Compute the total mass of the body - for (ProxyShape* shape = this.proxyCollisionShapes; shape != NULL; shape = shape->this.next) { - this.initMass += shape->getMass(); - this.centerOfMassLocal += shape->getLocalToBodyTransform().getPosition() * 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 - vec3 oldCenterOfMass = this.centerOfMassWorld; - this.centerOfMassLocal *= this.massInverse; - this.centerOfMassWorld = this.transform * this.centerOfMassLocal; - // Compute the total mass and inertia tensor using all the collision shapes - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - // Get the inertia tensor of the collision shape in its local-space - etk::Matrix3x3 inertiaTensor; - shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass()); - // Convert the collision shape inertia tensor into the local-space of the body - etk::Transform3D shapeTransform = shape->getLocalToBodyTransform(); - etk::Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix(); - inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); - // 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. - vec3 offset = shapeTransform.getPosition() - this.centerOfMassLocal; - float offsetSquare = offset.length2(); - vec3 off1 = offset * (-offset.x()); - vec3 off2 = offset * (-offset.y()); - vec3 off3 = offset * (-offset.z()); - etk::Matrix3x3 offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(), - off2.x(), off2.y()+offsetSquare, off2.z(), - off3.x(), off3.y(), off3.z()+offsetSquare); - offsetMatrix *= shape->getMass(); - this.inertiaTensorLocal += inertiaTensor + offsetMatrix; - } - // Compute the local inverse inertia tensor - this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse(); - // Update the linear velocity of the center of mass - this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); -} - - -void RigidBody::updateBroadPhaseState() { - PROFILE("RigidBody::updateBroadPhaseState()"); - DynamicsWorld world = staticcast(this.world); - vec3 displacement = world.this.timeStep * this.linearVelocity; - // For all the proxy collision shapes of the body - for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape->this.next) { - // Recompute the world-space AABB of the collision shape - AABB aabb; - Log.verbose(" : " << aabb.getMin() << " " << aabb.getMax()); - Log.verbose(" this.transform: " << this.transform); - shape->getCollisionShape()->computeAABB(aabb, this.transform *shape->getLocalToBodyTransform()); - Log.verbose(" : " << aabb.getMin() << " " << aabb.getMax()); - // Update the broad-phase state for the proxy collision shape - this.world.this.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); - } -} - - -void RigidBody::applyForceToCenterOfMass( vec3 force) { - if (this.type != DYNAMIC) { - return; - } - if (this.isSleeping) { - setIsSleeping(false); - } - this.externalForce += force; -} - -void RigidBody::applyForce( vec3 force, vec3 point) { - if (this.type != DYNAMIC) { - return; - } - if (this.isSleeping) { - setIsSleeping(false); - } - this.externalForce += force; - this.externalTorque += (point - this.centerOfMassWorld).cross(force); -} - -void RigidBody::applyTorque( vec3 torque) { - if (this.type != DYNAMIC) { - return; - } - if (this.isSleeping) { - setIsSleeping(false); - } - this.externalTorque += torque; -} \ No newline at end of file diff --git a/src/org/atriaSoft/ephysics/body/RigidBody.hpp b/src/org/atriaSoft/ephysics/body/RigidBody.hpp deleted file mode 100644 index 4a7c85d..0000000 --- a/src/org/atriaSoft/ephysics/body/RigidBody.hpp +++ /dev/null @@ -1,294 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include - -namespace ephysics { - - // Class declarations - struct JointListElement; - class Joint; - class DynamicsWorld; - - /** - * @brief 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. - */ - class RigidBody : public CollisionBody { - protected : - float this.initMass; //!< Intial mass of the body - vec3 this.centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin - vec3 this.centerOfMassWorld; //!< Center of mass of the body in world-space coordinates - vec3 this.linearVelocity; //!< Linear velocity of the body - vec3 this.angularVelocity; //!< Angular velocity of the body - vec3 this.externalForce; //!< Current external force on the body - vec3 this.externalTorque; //!< Current external torque on the body - etk::Matrix3x3 this.inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body - etk::Matrix3x3 this.inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body - float this.massInverse; //!< Inverse of the mass of the body - boolean this.isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body - Material this.material; //!< Material properties of the rigid body - float this.linearDamping; //!< Linear velocity damping factor - float this.angularDamping; //!< Angular velocity damping factor - JointListElement* this.jointsList; //!< First element of the linked list of joints involving this body - /// Private copy-ructor - RigidBody( RigidBody body); - /// Private assignment operator - RigidBody operator=( RigidBody body); - /** - * @brief Remove a joint from the joints list - */ - void removeJointFrothis.jointsList( Joint* joint); - /** - * @brief Update the transform of the body after a change of the center of mass - */ - void updateTransformWithCenterOfMass(); - void updateBroadPhaseState() override; - public : - /** - * @brief 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 - */ - RigidBody( etk::Transform3D transform, CollisionWorld world, long id); - /** - * @brief Virtual Destructor - */ - virtual ~RigidBody(); - void setType(BodyType type) override; - /** - * @brief Set the current position and orientation - * @param[in] transform The transformation of the body that transforms the local-space of the body into world-space - */ - void setTransform( etk::Transform3D transform) override; - /** - * @brief Get the mass of the body - * @return The mass (in kilograms) of the body - */ - float getMass() { - return this.initMass; - } - /** - * @brief Get the linear velocity - * @return The linear velocity vector of the body - */ - vec3 getLinearVelocity() { - return this.linearVelocity; - } - /** - * @brief Set the linear velocity of the rigid body. - * @param[in] linearVelocity Linear velocity vector of the body - */ - void setLinearVelocity( vec3 linearVelocity); - /** - * @brief Get the angular velocity of the body - * @return The angular velocity vector of the body - */ - vec3 getAngularVelocity() { - return this.angularVelocity; - } - /** - * @brief Set the angular velocity. - * @param[in] angularVelocity The angular velocity vector of the body - */ - void setAngularVelocity( vec3 angularVelocity); - /** - * @brief Set the variable to know whether or not the body is sleeping - * @param[in] isSleeping New sleeping state of the body - */ - virtual void setIsSleeping(boolean isSleeping); - /** - * @brief Get the local inertia tensor of the body (in local-space coordinates) - * @return The 3x3 inertia tensor matrix of the body - */ - etk::Matrix3x3 getInertiaTensorLocal() { - return this.inertiaTensorLocal; - } - /** - * @brief Set the local inertia tensor of the body (in local-space coordinates) - * @param[in] inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates - */ - void setInertiaTensorLocal( etk::Matrix3x3 inertiaTensorLocal); - /** - * @brief Set the local center of mass of the body (in local-space coordinates) - * @param[in] centerOfMassLocal The center of mass of the body in local-space coordinates - */ - void setCenterOfMassLocal( vec3 centerOfMassLocal); - /** - * @brief Set the mass of the rigid body - * @param[in] mass The mass (in kilograms) of the body - */ - void setMass(float mass); - /** - * @brief Get the inertia tensor in world coordinates. - * The inertia tensor Iw in world coordinates is computed - * with the local inertia tensor Ib in body coordinates - * by Iw = R * Ib * 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 - */ - etk::Matrix3x3 getInertiaTensorWorld() { - // Compute and return the inertia tensor in world coordinates - return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocal * - this.transform.getOrientation().getMatrix().getTranspose(); - } - /** - * @brief Get the inverse of the inertia tensor in world coordinates. - * The inertia tensor Iw in world coordinates is computed with the - * local inverse inertia tensor Ib^-1 in body coordinates - * by Iw = R * Ib^-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 - */ - etk::Matrix3x3 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 - return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocalInverse * - this.transform.getOrientation().getMatrix().getTranspose(); - } - /** - * @brief get the need of gravity appling to this rigid body - * @return True if the gravity is applied to the body - */ - boolean isGravityEnabled() { - return this.isGravityEnabled; - } - /** - * @brief Set the variable to know if the gravity is applied to this rigid body - * @param[in] isEnabled True if you want the gravity to be applied to this body - */ - void enableGravity(boolean isEnabled) { - this.isGravityEnabled = isEnabled; - } - /** - * @brief get a reference to the material properties of the rigid body - * @return A reference to the material of the body - */ - Material getMaterial() { - return this.material; - } - /** - * @brief Set a new material for this rigid body - * @param[in] material The material you want to set to the body - */ - void setMaterial( Material material) { - this.material = material; - } - /** - * @brief Get the linear velocity damping factor - * @return The linear damping factor of this body - */ - float getLinearDamping() { - return this.linearDamping; - } - /** - * @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation. - * @param[in] linearDamping The linear damping factor of this body - */ - void setLinearDamping(float linearDamping) { - assert(linearDamping >= 0.0f); - this.linearDamping = linearDamping; - } - /** - * @brief Get the angular velocity damping factor - * @return The angular damping factor of this body - */ - float getAngularDamping() { - return this.angularDamping; - } - /** - * @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation. - * @param[in] angularDamping The angular damping factor of this body - */ - void setAngularDamping(float angularDamping) { - assert(angularDamping >= 0.0f); - this.angularDamping = angularDamping; - } - /** - * @brief 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 - */ - JointListElement* getJointsList() { - return this.jointsList; - } - /** - * @brief 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 - */ - JointListElement* getJointsList() { - return this.jointsList; - } - /** - * @brief 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[in] force The external force to apply on the center of mass of the body - */ - void applyForceToCenterOfMass( vec3 force); - /** - * @brief 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[in] force The force to apply on the body - * @param[in] point The point where the force is applied (in world-space coordinates) - */ - void applyForce( vec3 force, vec3 point); - /** - * @brief 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[in] torque The external torque to apply on the body - */ - void applyTorque( vec3 torque); - /** - * @brief 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[in] collisionShape 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 into the local-space of the body - * @param[in] 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. - */ - ProxyShape* addCollisionShape(CollisionShape* collisionShape, - etk::Transform3D transform, - float mass); - virtual void removeCollisionShape( ProxyShape* proxyShape) override; - /** - * @brief Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body. - */ - void recomputeMassInformation(); - friend class DynamicsWorld; - friend class ContactSolver; - friend class BallAndSocketJoint; - friend class SliderJoint; - friend class HingeJoint; - friend class FixedJoint; - }; - -} - diff --git a/src/org/atriaSoft/ephysics/body/RigidBody.java b/src/org/atriaSoft/ephysics/body/RigidBody.java new file mode 100644 index 0000000..93a1bdf --- /dev/null +++ b/src/org/atriaSoft/ephysics/body/RigidBody.java @@ -0,0 +1,514 @@ +package org.atriaSoft.ephysics.body; + +import org.atriaSoft.etk.math.Matrix3f; +import org.atriaSoft.etk.math.Transform3D; +import org.atriaSoft.etk.math.Vector3f; + +/** + * @brief 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. + */ +class RigidBody extends CollisionBody { + protected float initMass; //!< Intial mass of the body + protected Vector3f centerOfMassLocal; //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin + protected Vector3f centerOfMassWorld; //!< Center of mass of the body in world-space coordinates + protected Vector3f linearVelocity; //!< Linear velocity of the body + protected Vector3f angularVelocity; //!< Angular velocity of the body + protected Vector3f externalForce; //!< Current external force on the body + protected Vector3f externalTorque; //!< Current external torque on the body + protected Matrix3f inertiaTensorLocal; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body + protected Matrix3f inertiaTensorLocalInverse; //!< Inverse of the inertia tensor of the body + protected float massInverse; //!< Inverse of the mass of the body + protected boolean isGravityEnabled; //!< True if the gravity needs to be applied to this rigid body + protected Material material; //!< Material properties of the rigid body + protected float linearDamping; //!< Linear velocity damping factor + protected float angularDamping; //!< Angular velocity damping factor + protected JointListElement jointsList; //!< First element of the linked list of joints involving this body + /** + * @brief Remove a joint from the joints list + */ + protected void removeJointFrom_jointsList( 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; + ETKDELETE(JointListElement, elementToRemove); + 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; + ETKDELETE(JointListElement, elementToRemove); + elementToRemove = null; + break; + } + currentElement = currentElement.next; + } + } + } + /** + * @brief Update the transform of the body after a change of the center of mass + */ + protected void updateTransformWithCenterOfMass() { + // Translate the body according to the translation of the center of mass position + this.transform.setPosition(this.centerOfMassWorld - this.transform.getOrientation() * this.centerOfMassLocal); + if (isnan(this.transform.getPosition().x()) == true) { + Log.critical("updateTransformWithCenterOfMass: " + this.transform); + } + if (isinf(this.transform.getOrientation().z()) == true) { + Log.critical(" set transform: " + this.transform); + } + } + @Override + protected void updateBroadPhaseState() { + PROFILE("RigidBody::updateBroadPhaseState()"); + DynamicsWorld world = staticcast(this.world); + Vector3f displacement = world.timeStep * this.linearVelocity; + // For all the proxy collision shapes of the body + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + // Recompute the world-space AABB of the collision shape + AABB aabb; + Log.verbose(" : " + aabb.getMin() + " " + aabb.getMax()); + Log.verbose(" this.transform: " + this.transform); + shape.getCollisionShape().computeAABB(aabb, this.transform *shape.getLocalToBodyTransform()); + Log.verbose(" : " + aabb.getMin() + " " + aabb.getMax()); + // Update the broad-phase state for the proxy collision shape + this.world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement); + } + } + /** + * @brief 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( Transform3D transform, CollisionWorld world, long id) { + super(transform, world, id); + this.initMass = 1.0f; + this.centerOfMassLocal = new Vector3f(0, 0, 0; + this.centerOfMassWorld = transform.getPosition().clone(); + this.isGravityEnabled = true; + this.linearDamping = 0.0f; + this.angularDamping = 0.0f; + this.jointsList = null; + // Compute the inverse mass + this.massInverse = 1.0f / this.initMass; + } + @Override + public void setType(BodyType type) { + if (this.type == type) { + return; + } + super.setType(type); + recomputeMassInformation(); + if (this.type == STATIC) { + // Reset the velocity to zero + this.linearVelocity.setZero(); + this.angularVelocity.setZero(); + } + if ( this.type == STATIC + || this.type == 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.getInverse(); + } + 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(); + } + /** + * @brief Set the current position and orientation + * @param[in] transform The transformation of the body that transforms the local-space of the body into world-space + */ + public void setTransform( Transform3D transform) { + Log.debug(" set transform: " + this.transform + " ==> " + transform); + if (isnan(transform.getPosition().x()) == true) { + Log.critical(" set transform: " + this.transform + " ==> " + transform); + } + if (isinf(transform.getOrientation().z()) == true) { + Log.critical(" set transform: " + this.transform + " ==> " + transform); + } + this.transform = transform; + Vector3f oldCenterOfMass = this.centerOfMassWorld; + // Compute the new center of mass in world-space coordinates + this.centerOfMassWorld = this.transform * this.centerOfMassLocal; + // Update the linear velocity of the center of mass + this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); + updateBroadPhaseState(); + } + /** + * @brief Get the mass of the body + * @return The mass (in kilograms) of the body + */ + public float getMass() { + return this.initMass; + } + /** + * @brief Get the linear velocity + * @return The linear velocity vector of the body + */ + public Vector3f getLinearVelocity() { + return this.linearVelocity; + } + /** + * @brief Set the linear velocity of the rigid body. + * @param[in] linearVelocity Linear velocity vector of the body + */ + public void setLinearVelocity( Vector3f linearVelocity) { + if (this.type == STATIC) { + return; + } + this.linearVelocity = linearVelocity; + if (this.linearVelocity.length2() > 0.0f) { + setIsSleeping(false); + } + } + /** + * @brief Get the angular velocity of the body + * @return The angular velocity vector of the body + */ + public Vector3f getAngularVelocity() { + return this.angularVelocity; + } + /** + * @brief Set the angular velocity. + * @param[in] angularVelocity The angular velocity vector of the body + */ + public void setAngularVelocity( Vector3f angularVelocity) { + if (this.type == STATIC) { + return; + } + this.angularVelocity = angularVelocity; + if (this.angularVelocity.length2() > 0.0f) { + setIsSleeping(false); + } + } + /** + * @brief Set the variable to know whether or not the body is sleeping + * @param[in] isSleeping New sleeping state of the body + */ + public void setIsSleeping(boolean isSleeping) { + if (isSleeping) { + this.linearVelocity.setZero(); + this.angularVelocity.setZero(); + this.externalForce.setZero(); + this.externalTorque.setZero(); + } + Body::setIsSleeping(isSleeping); + } + /** + * @brief 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; + } + /** + * @brief Set the local inertia tensor of the body (in local-space coordinates) + * @param[in] inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates + */ + public void setInertiaTensorLocal( Matrix3f inertiaTensorLocal) { + if (this.type != DYNAMIC) { + return; + } + this.inertiaTensorLocal = inertiaTensorLocal; + this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse(); + } + /** + * @brief Set the local center of mass of the body (in local-space coordinates) + * @param[in] centerOfMassLocal The center of mass of the body in local-space coordinates + */ + public void setCenterOfMassLocal( Vector3f centerOfMassLocal) { + if (this.type != DYNAMIC) { + return; + } + Vector3f oldCenterOfMass = this.centerOfMassWorld; + this.centerOfMassLocal = centerOfMassLocal; + this.centerOfMassWorld = this.transform * this.centerOfMassLocal; + this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); + } + /** + * @brief Set the mass of the rigid body + * @param[in] mass The mass (in kilograms) of the body + */ + public void setMass(float mass) { + if (this.type != 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; + } + } + /** + * @brief Get the inertia tensor in world coordinates. + * The inertia tensor Iw in world coordinates is computed + * with the local inertia tensor Ib in body coordinates + * by Iw = R * Ib * 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() * this.inertiaTensorLocal * + this.transform.getOrientation().getMatrix().getTranspose(); + } + /** + * @brief Get the inverse of the inertia tensor in world coordinates. + * The inertia tensor Iw in world coordinates is computed with the + * local inverse inertia tensor Ib^-1 in body coordinates + * by Iw = R * Ib^-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 + return this.transform.getOrientation().getMatrix() * this.inertiaTensorLocalInverse * + this.transform.getOrientation().getMatrix().getTranspose(); + } + /** + * @brief 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; + } + /** + * @brief Set the variable to know if the gravity is applied to this rigid body + * @param[in] isEnabled True if you want the gravity to be applied to this body + */ + public void enableGravity(boolean isEnabled) { + this.isGravityEnabled = isEnabled; + } + /** + * @brief 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; + } + /** + * @brief Set a new material for this rigid body + * @param[in] material The material you want to set to the body + */ + public void setMaterial( Material material) { + this.material = material; + } + /** + * @brief Get the linear velocity damping factor + * @return The linear damping factor of this body + */ + public float getLinearDamping() { + return this.linearDamping; + } + /** + * @brief Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation. + * @param[in] linearDamping The linear damping factor of this body + */ + public void setLinearDamping(float linearDamping) { + assert(linearDamping >= 0.0f); + this.linearDamping = linearDamping; + } + /** + * @brief Get the angular velocity damping factor + * @return The angular damping factor of this body + */ + public float getAngularDamping() { + return this.angularDamping; + } + /** + * @brief Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation. + * @param[in] angularDamping The angular damping factor of this body + */ + public void setAngularDamping(float angularDamping) { + assert(angularDamping >= 0.0f); + this.angularDamping = angularDamping; + } + /** + * @brief 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; + } + /** + * @brief 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; + } + /** + * @brief 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[in] force The external force to apply on the center of mass of the body + */ + public void applyForceToCenterOfMass( Vector3f force) { + if (this.type != DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalForce += force; + } + /** + * @brief 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[in] force The force to apply on the body + * @param[in] point The point where the force is applied (in world-space coordinates) + */ + public void applyForce( Vector3f force, Vector3f point) { + if (this.type != DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalForce += force; + this.externalTorque += (point - this.centerOfMassWorld).cross(force); + } + /** + * @brief 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[in] torque The external torque to apply on the body + */ + public void applyTorque( Vector3f torque) { + if (this.type != DYNAMIC) { + return; + } + if (this.isSleeping) { + setIsSleeping(false); + } + this.externalTorque += torque; + } + /** + * @brief 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[in] collisionShape 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 into the local-space of the body + * @param[in] 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(CollisionShape collisionShape, + Transform3D transform, + float mass) { + assert(mass > 0.0f); + // Create a new proxy collision shape to attach the collision shape to the body + ProxyShape* proxyShape = ETKNEW(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.this.next = this.proxyCollisionShapes; + this.proxyCollisionShapes = proxyShape; + } + // Compute the world-space AABB of the new collision shape + AABB aabb; + collisionShape.computeAABB(aabb, this.transform * transform); + // Notify the collision detection about this new collision shape + this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb); + this.numberCollisionShapes++; + recomputeMassInformation(); + return proxyShape; + } + public void removeCollisionShape(ProxyShape proxyShape) { + CollisionBody::removeCollisionShape(proxyShape); + recomputeMassInformation(); + } + + /** + * @brief 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 == STATIC || this.type == KINEMATIC) { + this.centerOfMassWorld = this.transform.getPosition(); + return; + } + assert(this.type == DYNAMIC); + // Compute the total mass of the body + for (ProxyShape* shape = this.proxyCollisionShapes; shape != NULL; shape = shape.this.next) { + this.initMass += shape.getMass(); + this.centerOfMassLocal += shape.getLocalToBodyTransform().getPosition() * 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 + Vector3f oldCenterOfMass = this.centerOfMassWorld; + this.centerOfMassLocal *= this.massInverse; + this.centerOfMassWorld = this.transform * this.centerOfMassLocal; + // Compute the total mass and inertia tensor using all the collision shapes + for (ProxyShape* shape = this.proxyCollisionShapes; shape != null; shape = shape.this.next) { + // Get the inertia tensor of the collision shape in its local-space + Matrix3f inertiaTensor; + shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass()); + // Convert the collision shape inertia tensor into the local-space of the body + Transform3D shapeTransform = shape.getLocalToBodyTransform(); + Matrix3f rotationMatrix = shapeTransform.getOrientation().getMatrix(); + inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose(); + // 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. + Vector3f offset = shapeTransform.getPosition() - this.centerOfMassLocal; + float offsetSquare = offset.length2(); + Vector3f off1 = offset * (-offset.x()); + Vector3f off2 = offset * (-offset.y()); + Vector3f off3 = offset * (-offset.z()); + Matrix3f offsetMatrix(off1.x()+offsetSquare, off1.y(), off1.z(), + off2.x(), off2.y()+offsetSquare, off2.z(), + off3.x(), off3.y(), off3.z()+offsetSquare); + offsetMatrix *= shape.getMass(); + this.inertiaTensorLocal += inertiaTensor + offsetMatrix; + } + // Compute the local inverse inertia tensor + this.inertiaTensorLocalInverse = this.inertiaTensorLocal.getInverse(); + // Update the linear velocity of the center of mass + this.linearVelocity += this.angularVelocity.cross(this.centerOfMassWorld - oldCenterOfMass); + } + +} + diff --git a/src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp b/src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp index a861049..5f4cf16 100644 --- a/src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp +++ b/src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp @@ -40,7 +40,7 @@ void CollisionDetection::computeCollisionDetection() { computeNarrowPhase(); } -void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, etk::Set shapes1, etk::Set shapes2) { +void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, Set shapes1, Set shapes2) { // Compute the broad-phase collision detection computeBroadPhase(); // Delete all the contact points in the currently overlapping pairs @@ -49,54 +49,54 @@ void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); } -void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, etk::Set shapes1, etk::Set shapes2) { +void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, Set shapes1, Set shapes2) { // For each possible collision pair of bodies - etk::Map::Iterator it; + Map::Iterator it; for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) { - OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); + OverlappingPair* pair = it.second; + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); + assert(shape1.this.broadPhaseID != shape2.this.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.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0 - || shapes2.count(shape2->this.broadPhaseID) == 0 ) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0 - || shapes2.count(shape1->this.broadPhaseID) == 0 ) ) { + && !shapes2.empty() + && ( shapes1.count(shape1.this.broadPhaseID) == 0 + || shapes2.count(shape2.this.broadPhaseID) == 0 ) + && ( shapes1.count(shape2.this.broadPhaseID) == 0 + || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) { continue; } if ( !shapes1.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) { + && shapes2.empty() + && shapes1.count(shape1.this.broadPhaseID) == 0 + && shapes1.count(shape2.this.broadPhaseID) == 0) { continue; } if ( !shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) { + && shapes1.empty() + && shapes2.count(shape1.this.broadPhaseID) == 0 + && shapes2.count(shape2.this.broadPhaseID) == 0) { continue; } // For each contact manifold set of the overlapping pair - ContactManifoldSet manifoldSet = pair->getContactManifoldSet(); + ContactManifoldSet manifoldSet = pair.getContactManifoldSet(); for (int j=0; jgetNbContactPoints(); i++) { - ContactPoint* contactPoint = manifold->getContactPoint(i); + for (int i=0; igetShape1(), manifold->getShape2(), - manifold->getShape1()->getCollisionShape(), - manifold->getShape2()->getCollisionShape(), - contactPoint->getNormal(), - contactPoint->getPenetrationDepth(), - contactPoint->getLocalPointOnBody1(), - contactPoint->getLocalPointOnBody2()); + ContactPointInfo contactInfo(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); + callback.notifyContact(contactInfo); } } } @@ -119,35 +119,35 @@ void CollisionDetection::computeNarrowPhase() { // Clear the set of overlapping pairs in narrow-phase contact this.contactOverlappingPairs.clear(); // For each possible collision pair of bodies - etk::Map::Iterator it; + Map::Iterator it; for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { - OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); + OverlappingPair* pair = it.second; + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); + assert(shape1.this.broadPhaseID != shape2.this.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) || + 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 - ETKDELETE(OverlappingPair, it->second); - it->second = null; + ETKDELETE(OverlappingPair, it.second); + it.second = null; it = this.overlappingPairs.erase(it); continue; } else { ++it; } - CollisionBody* body1 = shape1->getBody(); - CollisionBody* body2 = shape2->getBody(); + CollisionBody* body1 = shape1.getBody(); + CollisionBody* body2 = shape2.getBody(); // Update the contact cache of the overlapping pair - pair->update(); + pair.update(); // Check that at least one body is awake and not static - boolean isBody1Active = !body1->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body1->getType() != STATIC; - boolean isBody2Active = !body2->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->getType() != STATIC; - if (!isBody1Active hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !isBody2Active) { + boolean isBody1Active = !body1.isSleeping() && body1.getType() != STATIC; + boolean isBody2Active = !body2.isSleeping() && body2.getType() != STATIC; + if (!isBody1Active && !isBody2Active) { continue; } // Check if the bodies are in the set of bodies that cannot collide between each other @@ -156,85 +156,85 @@ void CollisionDetection::computeNarrowPhase() { continue; } // Select the narrow phase algorithm to use according to the two collision shapes - CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + CollisionShapeType shape1Type = shape1.getCollisionShape().getType(); + CollisionShapeType shape2Type = shape2.getCollisionShape().getType(); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type]; // 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); + narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); // Create the CollisionShapeInfo objects - CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), - pair, shape1->getCachedCollisionData()); - CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), - pair, shape2->getCachedCollisionData()); + CollisionShapeInfo shape1Info(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), + pair, shape1.getCachedCollisionData()); + CollisionShapeInfo shape2Info(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); + narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this); } // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } -void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, etk::Set shapes1, etk::Set shapes2) { +void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, Set shapes1, Set shapes2) { this.contactOverlappingPairs.clear(); // For each possible collision pair of bodies - etk::Map::Iterator it; + Map::Iterator it; for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { - OverlappingPair* pair = it->second; - ProxyShape* shape1 = pair->getShape1(); - ProxyShape* shape2 = pair->getShape2(); - assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); + OverlappingPair* pair = it.second; + ProxyShape* shape1 = pair.getShape1(); + ProxyShape* shape2 = pair.getShape2(); + assert(shape1.this.broadPhaseID != shape2.this.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.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0 - || shapes2.count(shape2->this.broadPhaseID) == 0 ) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0 - || shapes2.count(shape1->this.broadPhaseID) == 0 ) ) { + && !shapes2.empty() + && ( shapes1.count(shape1.this.broadPhaseID) == 0 + || shapes2.count(shape2.this.broadPhaseID) == 0 ) + && ( shapes1.count(shape2.this.broadPhaseID) == 0 + || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) { ++it; continue; } if ( !shapes1.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) { + && shapes2.empty() + && shapes1.count(shape1.this.broadPhaseID) == 0 + && shapes1.count(shape2.this.broadPhaseID) == 0) { ++it; continue; } if ( !shapes2.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) { + && shapes1.empty() + && shapes2.count(shape1.this.broadPhaseID) == 0 + && shapes2.count(shape2.this.broadPhaseID) == 0) { ++it; continue; } // Check if the collision filtering allows collision between the two shapes and // that the two shapes are still overlapping. Otherwise, we destroy the // overlapping pair - if ( ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 - || (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0 ) + if ( ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0 + || (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0 ) || !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) { // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // Destroy the overlapping pair - ETKDELETE(OverlappingPair, it->second); - it->second = null; + ETKDELETE(OverlappingPair, it.second); + it.second = null; it = this.overlappingPairs.erase(it); continue; } else { ++it; } - CollisionBody* body1 = shape1->getBody(); - CollisionBody* body2 = shape2->getBody(); + CollisionBody* body1 = shape1.getBody(); + CollisionBody* body2 = shape2.getBody(); // Update the contact cache of the overlapping pair - pair->update(); + pair.update(); // Check if the two bodies are allowed to collide, otherwise, we do not test for collision - if (body1->getType() != DYNAMIC hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->getType() != DYNAMIC) { + if (body1.getType() != DYNAMIC && body2.getType() != DYNAMIC) { continue; } longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); @@ -242,48 +242,48 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call continue; } // Check if the two bodies are sleeping, if so, we do no test collision between them - if (body1->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->isSleeping()) { + if (body1.isSleeping() && body2.isSleeping()) { continue; } // Select the narrow phase algorithm to use according to the two collision shapes - CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); - CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); + CollisionShapeType shape1Type = shape1.getCollisionShape().getType(); + CollisionShapeType shape2Type = shape2.getCollisionShape().getType(); NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type]; // 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); + narrowPhaseAlgorithm.setCurrentOverlappingPair(pair); // Create the CollisionShapeInfo objects CollisionShapeInfo shape1Info(shape1, - shape1->getCollisionShape(), - shape1->getLocalToWorldTransform(), + shape1.getCollisionShape(), + shape1.getLocalToWorldTransform(), pair, - shape1->getCachedCollisionData()); + shape1.getCachedCollisionData()); CollisionShapeInfo shape2Info(shape2, - shape2->getCollisionShape(), - shape2->getLocalToWorldTransform(), + shape2.getCollisionShape(), + shape2.getLocalToWorldTransform(), pair, - shape2->getCachedCollisionData()); + shape2.getCachedCollisionData()); TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); // Use the narrow-phase collision detection algorithm to check // if there really is a collision - narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, narrowPhaseCallback); + narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback); } // Add all the contact manifolds (between colliding bodies) to the bodies addAllContactManifoldsToBodies(); } void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { - assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); + assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID); // If the two proxy collision shapes are from the same body, skip it - if (shape1->getBody()->getID() == shape2->getBody()->getID()) { + if (shape1.getBody().getID() == shape2.getBody().getID()) { return; } // Check if the collision filtering allows collision between the two shapes - if ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 - || (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0) { + if ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0 + || (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0) { return; } // Compute the overlapping pair ID @@ -291,27 +291,27 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro // Check if the overlapping pair already exists if (this.overlappingPairs.find(pairID) != this.overlappingPairs.end()) return; // Compute the maximum number of contact manifolds for this pair - int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), - shape2->getCollisionShape()->getType()); + int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1.getCollisionShape().getType(), + shape2.getCollisionShape().getType()); // Create the overlapping pair and add it into the set of overlapping pairs OverlappingPair* newPair = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds); assert(newPair != null); this.overlappingPairs.set(pairID, newPair); // Wake up the two bodies - shape1->getBody()->setIsSleeping(false); - shape2->getBody()->setIsSleeping(false); + shape1.getBody().setIsSleeping(false); + shape2.getBody().setIsSleeping(false); } void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Remove all the overlapping pairs involving this proxy shape - etk::Map::Iterator it; + Map::Iterator it; for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { - if (it->second->getShape1()->this.broadPhaseID == proxyShape->this.broadPhaseID|| - it->second->getShape2()->this.broadPhaseID == proxyShape->this.broadPhaseID) { + if (it.second.getShape1().this.broadPhaseID == proxyShape.this.broadPhaseID|| + it.second.getShape2().this.broadPhaseID == proxyShape.this.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 - ETKDELETE(OverlappingPair, it->second); - it->second = null; + ETKDELETE(OverlappingPair, it.second); + it.second = null; it = this.overlappingPairs.erase(it); } else { ++it; @@ -323,17 +323,17 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) { // If it is the first contact since the pairs are overlapping - if (overlappingPair->getNbContactPoints() == 0) { + if (overlappingPair.getNbContactPoints() == 0) { // Trigger a callback event - if (this.world->this.eventListener != NULL) { - this.world->this.eventListener->beginContact(contactInfo); + if (this.world.this.eventListener != NULL) { + this.world.this.eventListener.beginContact(contactInfo); } } // Create a new contact createContact(overlappingPair, contactInfo); // Trigger a callback event for the new contact - if (this.world->this.eventListener != NULL) { - this.world->this.eventListener->newContact(contactInfo); + if (this.world.this.eventListener != NULL) { + this.world.this.eventListener.newContact(contactInfo); } } @@ -341,46 +341,46 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, Contac // Create a new contact ContactPoint* contact = ETKNEW(ContactPoint, contactInfo); // Add the contact to the contact manifold set of the corresponding overlapping pair - overlappingPair->addContact(contact); + overlappingPair.addContact(contact); // Add the overlapping pair into the set of pairs in contact during narrow-phase - overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), - overlappingPair->getShape2()); + overlappingpairid pairId = OverlappingPair::computeID(overlappingPair.getShape1(), + overlappingPair.getShape2()); this.contactOverlappingPairs.set(pairId, overlappingPair); } void CollisionDetection::addAllContactManifoldsToBodies() { // For each overlapping pairs in contact during the narrow-phase - etk::Map::Iterator it; + Map::Iterator it; for (it = this.contactOverlappingPairs.begin(); it != this.contactOverlappingPairs.end(); ++it) { // Add all the contact manifolds of the pair into the list of contact manifolds // of the two bodies involved in the contact - addContactManifoldToBody(it->second); + addContactManifoldToBody(it.second); } } void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { assert(pair != null); - CollisionBody* body1 = pair->getShape1()->getBody(); - CollisionBody* body2 = pair->getShape2()->getBody(); - ContactManifoldSet manifoldSet = pair->getContactManifoldSet(); + CollisionBody* body1 = pair.getShape1().getBody(); + CollisionBody* body2 = pair.getShape2().getBody(); + ContactManifoldSet manifoldSet = pair.getContactManifoldSet(); // For each contact manifold in the set of manifolds in the pair for (int i=0; igetNbContactPoints() > 0); + assert(contactManifold.getNbContactPoints() > 0); // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - body1->this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body1->this.contactManifoldsList);; + body1.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body1.this.contactManifoldsList);; // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - body2->this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body2->this.contactManifoldsList);; + body2.this.contactManifoldsList = ETKNEW(ContactManifoldListElement, contactManifold, body2.this.contactManifoldsList);; } } void CollisionDetection::clearContactPoints() { // For each overlapping pair - etk::Map::Iterator it; + Map::Iterator it; for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) { - it->second->clearContactPoints(); + it.second.clearContactPoints(); } } @@ -388,17 +388,17 @@ void CollisionDetection::fillInCollisionMatrix() { // For each possible type of collision shape for (int i=0; iselectAlgorithm(i, j); + this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(i, j); } } } EventListener* CollisionDetection::getWorldEventListener() { - return this.world->this.eventListener; + return this.world.this.eventListener; } void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) { - this.collisionCallback->notifyContact(contactInfo); + this.collisionCallback.notifyContact(contactInfo); } NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) { @@ -407,7 +407,7 @@ NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeTy void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { this.collisionDispatch = collisionDispatch; - this.collisionDispatch->init(this); + this.collisionDispatch.init(this); // Fill-in the collision matrix with the new algorithms to use fillInCollisionMatrix(); } @@ -427,14 +427,14 @@ void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBo } void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { - this.broadPhaseAlgorithm.addMovedCollisionShape(shape->this.broadPhaseID); + this.broadPhaseAlgorithm.addMovedCollisionShape(shape.this.broadPhaseID); } -void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, AABB aabb, vec3 displacement, boolean forceReinsert) { +void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, AABB aabb, Vector3f displacement, boolean forceReinsert) { this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } -void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, unsigned short raycastWithCategoryMaskBits) { +void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, int raycastWithCategoryMaskBits) { PROFILE("CollisionDetection::raycast()"); RaycastTest rayCastTest(raycastCallback); // Ask the broad-phase algorithm to call the testRaycastAgainstShape() @@ -444,8 +444,8 @@ void CollisionDetection::raycast(RaycastCallback* raycastCallback, Ray ray, uns boolean CollisionDetection::testAABBOverlap( ProxyShape* shape1, ProxyShape* shape2) { // If one of the shape's body is not active, we return no overlap - if ( !shape1->getBody()->isActive() - || !shape2->getBody()->isActive()) { + if ( !shape1.getBody().isActive() + || !shape2.getBody().isActive()) { return false; } return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); diff --git a/src/org/atriaSoft/ephysics/collision/CollisionDetection.hpp b/src/org/atriaSoft/ephysics/collision/CollisionDetection.java similarity index 58% rename from src/org/atriaSoft/ephysics/collision/CollisionDetection.hpp rename to src/org/atriaSoft/ephysics/collision/CollisionDetection.java index 7771a4e..3db0c04 100644 --- a/src/org/atriaSoft/ephysics/collision/CollisionDetection.hpp +++ b/src/org/atriaSoft/ephysics/collision/CollisionDetection.java @@ -1,32 +1,9 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision; -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ephysics { - - class BroadPhaseAlgorithm; - class CollisionWorld; - class CollisionCallback; - class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { + class TestCollisionBetweenShapesCallback extends NarrowPhaseCallback { private: - CollisionCallback* this.collisionCallback; //!< + CollisionCallback* collisionCallback; //!< public: // Constructor TestCollisionBetweenShapesCallback(CollisionCallback* callback): @@ -34,7 +11,7 @@ namespace ephysics { } // Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, + void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo); }; @@ -44,23 +21,19 @@ namespace ephysics { * collide and then we run a narrow-phase algorithm to compute the * collision contacts between bodies. */ - class CollisionDetection : public NarrowPhaseCallback { + class CollisionDetection extends NarrowPhaseCallback { private : - CollisionDispatch* this.collisionDispatch; //!< Collision Detection Dispatch configuration - DefaultCollisionDispatch this.defaultCollisionDispatch; //!< Default collision dispatch configuration - NarrowPhaseAlgorithm* this.collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use) - CollisionWorld* this.world; //!< Pointer to the physics world - etk::Map this.overlappingPairs; //!< Broad-phase overlapping pairs - etk::Map this.contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection) - BroadPhaseAlgorithm this.broadPhaseAlgorithm; //!< Broad-phase algorithm + CollisionDispatch* collisionDispatch; //!< Collision Detection Dispatch configuration + DefaultCollisionDispatch defaultCollisionDispatch; //!< Default collision dispatch configuration + NarrowPhaseAlgorithm* collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use) + CollisionWorld* world; //!< Pointer to the physics world + Map overlappingPairs; //!< Broad-phase overlapping pairs + Map contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection) + BroadPhaseAlgorithm broadPhaseAlgorithm; //!< Broad-phase algorithm // TODO : Delete this - GJKAlgorithm this.narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm - etk::Set this.noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other - boolean this.isCollisionShapesAdded; //!< True if some collision shapes have been added previously - /// Private copy-ructor - CollisionDetection( CollisionDetection collisionDetection); - /// Private assignment operator - CollisionDetection operator=( CollisionDetection collisionDetection); + GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm + Set noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other + boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously /// Compute the broad-phase collision detection void computeBroadPhase(); /// Compute the narrow-phase collision detection @@ -77,8 +50,6 @@ namespace ephysics { public : /// Constructor CollisionDetection(CollisionWorld* world); - /// Destructor - ~CollisionDetection(); /// Set the collision dispatch configuration void setCollisionDispatch(CollisionDispatch* collisionDispatch); /// Return the Narrow-phase collision detection algorithm to use between two types of shapes @@ -91,7 +62,7 @@ namespace ephysics { /// Update a proxy collision shape (that has moved for instance) void updateProxyCollisionShape(ProxyShape* shape, AABB aabb, - vec3 displacement = vec3(0, 0, 0), + Vector3f displacement = Vector3f(0, 0, 0), boolean forceReinsert = false); /// Add a pair of bodies that cannot collide with each other void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); @@ -105,16 +76,16 @@ namespace ephysics { void computeCollisionDetection(); /// Compute the collision detection void testCollisionBetweenShapes(CollisionCallback* callback, - etk::Set shapes1, - etk::Set shapes2); + Set shapes1, + Set shapes2); /// Report collision between two sets of shapes void reportCollisionBetweenShapes(CollisionCallback* callback, - etk::Set shapes1, - etk::Set shapes2) ; + Set shapes1, + Set shapes2) ; /// Ray casting method void raycast(RaycastCallback* raycastCallback, Ray ray, - unsigned short raycastWithCategoryMaskBits) ; + int raycastWithCategoryMaskBits) ; /// Test if the AABBs of two bodies overlap boolean testAABBOverlap( CollisionBody* body1, CollisionBody* body2) ; @@ -126,14 +97,14 @@ namespace ephysics { void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); /// Compute the narrow-phase collision detection void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, - etk::Set shapes1, - etk::Set shapes2); + Set shapes1, + Set shapes2); /// Return a pointer to the world CollisionWorld* getWorld(); /// Return the world event listener EventListener* getWorldEventListener(); /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) override; + void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) ; /// Create a new contact void createContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo); friend class DynamicsWorld; diff --git a/src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp b/src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp index 91b1890..b4a76be 100644 --- a/src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp +++ b/src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp @@ -11,7 +11,6 @@ #include namespace ephysics { - class OverlappingPair; /** * @brief It regroups different things about a collision shape. This is * used to pass information about a collision shape to a collision algorithm. @@ -21,12 +20,12 @@ namespace ephysics { OverlappingPair* overlappingPair; //!< Broadphase overlapping pair ProxyShape* proxyShape; //!< Proxy shape CollisionShape* collisionShape; //!< Pointer to the collision shape - etk::Transform3D shapeToWorldTransform; //!< etk::Transform3D that maps from collision shape local-space to world-space + Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space void** cachedCollisionData; //!< Cached collision data of the proxy shape /// Constructor CollisionShapeInfo(ProxyShape* proxyCollisionShape, CollisionShape* shape, - etk::Transform3D shapeLocalToWorldTransform, + Transform3D shapeLocalToWorldTransform, OverlappingPair* pair, void** cachedData): overlappingPair(pair), diff --git a/src/org/atriaSoft/ephysics/collision/ContactManifold.hpp b/src/org/atriaSoft/ephysics/collision/ContactManifold.hpp index 02a9808..c8feadf 100644 --- a/src/org/atriaSoft/ephysics/collision/ContactManifold.hpp +++ b/src/org/atriaSoft/ephysics/collision/ContactManifold.hpp @@ -16,9 +16,7 @@ namespace ephysics { int MAXCONTACTPOINTSINMANIFOLD = 4; //!< Maximum number of contacts in the manifold - - class ContactManifold; - + /** * @brief This structure represents a single element of a linked list of contact manifolds */ @@ -54,25 +52,19 @@ namespace ephysics { ContactManifold(ProxyShape* shape1, ProxyShape* shape2, int16t normalDirectionId); - /// Destructor - ~ContactManifold(); - /// DELETE copy-ructor - ContactManifold( ContactManifold contactManifold) = delete; - /// DELETE assignment operator - ContactManifold operator=( ContactManifold contactManifold) = delete; private: - ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact - ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact - ContactPoint* this.contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold - int16t this.normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction) - int this.nbContactPoints; //!< Number of contacts in the cache - vec3 this.frictionVector1; //!< First friction vector of the contact manifold - vec3 this.frictionvec2; //!< Second friction vector of the contact manifold - float this.frictionImpulse1; //!< First friction raint accumulated impulse - float this.frictionImpulse2; //!< Second friction raint accumulated impulse - float this.frictionTwistImpulse; //!< Twist friction raint accumulated impulse - vec3 this.rollingResistanceImpulse; //!< Accumulated rolling resistance impulse - boolean this.isAlreadyInIsland; //!< True if the contact manifold has already been added into an island + ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact + ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact + ContactPoint* contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold + int16t normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction) + int nbContactPoints; //!< Number of contacts in the cache + Vector3f frictionVector1; //!< First friction vector of the contact manifold + Vector3f frictionvec2; //!< Second friction vector of the contact manifold + float frictionImpulse1; //!< First friction raint accumulated impulse + float frictionImpulse2; //!< Second friction raint accumulated impulse + float frictionTwistImpulse; //!< Twist friction raint accumulated impulse + Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse + boolean isAlreadyInIsland; //!< True if the contact manifold has already been added into an island /// Return the index of maximum area int getMaxArea(float area0, float area1, float area2, float area3) ; /** @@ -94,7 +86,7 @@ namespace ephysics { * 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(int indexMaxPenetration, vec3 newPoint) ; + int getIndexToRemove(int indexMaxPenetration, Vector3f newPoint) ; /// Remove a contact point from the manifold void removeContactPoint(int index); /// Return true if the contact manifold has already been added into an island @@ -121,20 +113,20 @@ namespace ephysics { * the contacts with a too large distance between the contact points in the plane orthogonal to the * contact normal. */ - void update( etk::Transform3D transform1, - etk::Transform3D transform2); + void update( Transform3D transform1, + Transform3D transform2); /// Clear the contact manifold void clear(); /// Return the number of contact points in the manifold int getNbContactPoints() ; /// Return the first friction vector at the center of the contact manifold - vec3 getFrictionVector1() ; + Vector3f getFrictionVector1() ; /// set the first friction vector at the center of the contact manifold - void setFrictionVector1( vec3 frictionVector1); + void setFrictionVector1( Vector3f frictionVector1); /// Return the second friction vector at the center of the contact manifold - vec3 getFrictionvec2() ; + Vector3f getFrictionvec2() ; /// set the second friction vector at the center of the contact manifold - void setFrictionvec2( vec3 frictionvec2); + void setFrictionvec2( Vector3f frictionvec2); /// Return the first friction accumulated impulse float getFrictionImpulse1() ; /// Set the first friction accumulated impulse @@ -148,16 +140,13 @@ namespace ephysics { /// Set the friction twist accumulated impulse void setFrictionTwistImpulse(float frictionTwistImpulse); /// Set the accumulated rolling resistance impulse - void setRollingResistanceImpulse( vec3 rollingResistanceImpulse); + void setRollingResistanceImpulse( Vector3f rollingResistanceImpulse); /// Return a contact point of the manifold ContactPoint* getContactPoint(int index) ; /// Return the normalized averaged normal vector - vec3 getAverageContactNormal() ; + Vector3f getAverageContactNormal() ; /// Return the largest depth of all the contact points - float getLargestContactDepth() ; - friend class DynamicsWorld; - friend class Island; - friend class CollisionBody; + float getLargestContactDepth(); }; } diff --git a/src/org/atriaSoft/ephysics/collision/ContactManifold.cpp b/src/org/atriaSoft/ephysics/collision/ContactManifold.java similarity index 71% rename from src/org/atriaSoft/ephysics/collision/ContactManifold.cpp rename to src/org/atriaSoft/ephysics/collision/ContactManifold.java index 8486d66..ce54593 100644 --- a/src/org/atriaSoft/ephysics/collision/ContactManifold.cpp +++ b/src/org/atriaSoft/ephysics/collision/ContactManifold.java @@ -1,18 +1,8 @@ -/** @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) - */ -#include - -using namespace ephysics; +package org.atriaSoft.ephysics.collision; ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - short normalDirectionId): + int normalDirectionId): this.shape1(shape1), this.shape2(shape2), this.normalDirectionId(normalDirectionId), @@ -33,7 +23,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) { for (int i=0; igetWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2(); + float distance = (this.contactPoints[i].getWorldPointOnBody1() - contact.getWorldPointOnBody1()).length2(); if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) { // Delete the new contact ETKDELETE(ContactPoint, contact); @@ -44,7 +34,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) { // If the contact manifold is full if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) { int indexMaxPenetration = getIndexOfDeepestPenetration(contact); - int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); + int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1()); removeContactPoint(indexToRemove); } // Add the new contact point in the manifold @@ -67,30 +57,30 @@ void ContactManifold::removeContactPoint(int index) { this.nbContactPoints--; } -void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D transform2) { +void ContactManifold::update( Transform3D transform1, Transform3D transform2) { if (this.nbContactPoints == 0) { return; } // Update the world coordinates and penetration depth of the contact points in the manifold for (int i=0; isetWorldPointOnBody1(transform1 * this.contactPoints[i]->getLocalPointOnBody1()); - this.contactPoints[i]->setWorldPointOnBody2(transform2 * this.contactPoints[i]->getLocalPointOnBody2()); - this.contactPoints[i]->setPenetrationDepth((this.contactPoints[i]->getWorldPointOnBody1() - this.contactPoints[i]->getWorldPointOnBody2()).dot(this.contactPoints[i]->getNormal())); + this.contactPoints[i].setWorldPointOnBody1(transform1 * this.contactPoints[i].getLocalPointOnBody1()); + this.contactPoints[i].setWorldPointOnBody2(transform2 * this.contactPoints[i].getLocalPointOnBody2()); + this.contactPoints[i].setPenetrationDepth((this.contactPoints[i].getWorldPointOnBody1() - this.contactPoints[i].getWorldPointOnBody2()).dot(this.contactPoints[i].getNormal())); } float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD; // Remove the contact points that don't represent very well the contact manifold for (int i=staticcast(this.nbContactPoints)-1; i>=0; i--) { assert(i < staticcast(this.nbContactPoints)); // Compute the distance between contact points in the normal direction - float distanceNormal = -this.contactPoints[i]->getPenetrationDepth(); + float distanceNormal = -this.contactPoints[i].getPenetrationDepth(); // If the contacts points are too far from each other in the normal direction if (distanceNormal > squarePersistentContactThreshold) { removeContactPoint(i); } else { // Compute the distance of the two contact points in the plane // orthogonal to the contact normal - vec3 projOfPoint1 = this.contactPoints[i]->getWorldPointOnBody1() + this.contactPoints[i]->getNormal() * distanceNormal; - vec3 projDifference = this.contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; + Vector3f projOfPoint1 = this.contactPoints[i].getWorldPointOnBody1() + this.contactPoints[i].getNormal() * distanceNormal; + Vector3f projDifference = this.contactPoints[i].getWorldPointOnBody2() - projOfPoint1; // If the orthogonal distance is larger than the valid distance // threshold, we remove the contact if (projDifference.length2() > squarePersistentContactThreshold) { @@ -103,12 +93,12 @@ void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D tra int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) { assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD); int indexMaxPenetrationDepth = -1; - float maxPenetrationDepth = newContact->getPenetrationDepth(); + float maxPenetrationDepth = newContact.getPenetrationDepth(); // For each contact in the cache for (int i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = this.contactPoints[i]->getPenetrationDepth(); + if (this.contactPoints[i].getPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = this.contactPoints[i].getPenetrationDepth(); indexMaxPenetrationDepth = i; } } @@ -116,7 +106,7 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) { return indexMaxPenetrationDepth; } -int ContactManifold::getIndexToRemove(int indexMaxPenetration, vec3 newPoint) { +int ContactManifold::getIndexToRemove(int indexMaxPenetration, Vector3f newPoint) { assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD); float area0 = 0.0f; // Area with contact 1,2,3 and newPoint float area1 = 0.0f; // Area with contact 0,2,3 and newPoint @@ -124,30 +114,30 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, vec3 newPoint) float area3 = 0.0f; // Area with contact 0,1,2 and newPoint if (indexMaxPenetration != 0) { // Compute the area - vec3 vector1 = newPoint - this.contactPoints[1]->getLocalPointOnBody1(); - vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1(); - vec3 crossProduct = vector1.cross(vector2); + Vector3f vector1 = newPoint - this.contactPoints[1].getLocalPointOnBody1(); + Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1(); + Vector3f crossProduct = vector1.cross(vector2); area0 = crossProduct.length2(); } if (indexMaxPenetration != 1) { // Compute the area - vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1(); - vec3 crossProduct = vector1.cross(vector2); + Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1(); + Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1(); + Vector3f crossProduct = vector1.cross(vector2); area1 = crossProduct.length2(); } if (indexMaxPenetration != 2) { // Compute the area - vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1(); - vec3 crossProduct = vector1.cross(vector2); + Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1(); + Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1(); + Vector3f crossProduct = vector1.cross(vector2); area2 = crossProduct.length2(); } if (indexMaxPenetration != 3) { // Compute the area - vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); - vec3 vector2 = this.contactPoints[2]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1(); - vec3 crossProduct = vector1.cross(vector2); + Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1(); + Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1(); + Vector3f crossProduct = vector1.cross(vector2); area3 = crossProduct.length2(); } // Return the index of the contact to remove @@ -204,12 +194,12 @@ ProxyShape* ContactManifold::getShape2() { // Return a pointer to the first body of the contact manifold CollisionBody* ContactManifold::getBody1() { - return this.shape1->getBody(); + return this.shape1.getBody(); } // Return a pointer to the second body of the contact manifold CollisionBody* ContactManifold::getBody2() { - return this.shape2->getBody(); + return this.shape2.getBody(); } // Return the normal direction Id @@ -223,22 +213,22 @@ int ContactManifold::getNbContactPoints() { } // Return the first friction vector at the center of the contact manifold - vec3 ContactManifold::getFrictionVector1() { + Vector3f ContactManifold::getFrictionVector1() { return this.frictionVector1; } // set the first friction vector at the center of the contact manifold -void ContactManifold::setFrictionVector1( vec3 frictionVector1) { +void ContactManifold::setFrictionVector1( Vector3f frictionVector1) { this.frictionVector1 = frictionVector1; } // Return the second friction vector at the center of the contact manifold - vec3 ContactManifold::getFrictionvec2() { + Vector3f ContactManifold::getFrictionvec2() { return this.frictionvec2; } // set the second friction vector at the center of the contact manifold -void ContactManifold::setFrictionvec2( vec3 frictionvec2) { +void ContactManifold::setFrictionvec2( Vector3f frictionvec2) { this.frictionvec2 = frictionvec2; } @@ -273,7 +263,7 @@ void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) { } // Set the accumulated rolling resistance impulse -void ContactManifold::setRollingResistanceImpulse( vec3 rollingResistanceImpulse) { +void ContactManifold::setRollingResistanceImpulse( Vector3f rollingResistanceImpulse) { this.rollingResistanceImpulse = rollingResistanceImpulse; } @@ -289,10 +279,10 @@ boolean ContactManifold::isAlreadyInIsland() { } // Return the normalized averaged normal vector -vec3 ContactManifold::getAverageContactNormal() { - vec3 averageNormal; +Vector3f ContactManifold::getAverageContactNormal() { + Vector3f averageNormal; for (int i=0; igetNormal(); + averageNormal += this.contactPoints[i].getNormal(); } return averageNormal.safeNormalized(); } @@ -301,7 +291,7 @@ vec3 ContactManifold::getAverageContactNormal() { float ContactManifold::getLargestContactDepth() { float largestDepth = 0.0f; for (int i=0; igetPenetrationDepth(); + float depth = this.contactPoints[i].getPenetrationDepth(); if (depth > largestDepth) { largestDepth = depth; } diff --git a/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp b/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp index 59b7770..1f1c33d 100644 --- a/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp +++ b/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp @@ -26,14 +26,14 @@ ContactManifoldSet::~ContactManifoldSet() { void ContactManifoldSet::addContactPoint(ContactPoint* contact) { // Compute an Id corresponding to the normal direction (using a cubemap) - int16t normalDirectionId = computeCubemapNormalId(contact->getNormal()); + int16t 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); + this.manifolds[0].addContactPoint(contact); + assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0); for (int i=0; igetNbContactPoints() > 0); + assert(this.manifolds[i].getNbContactPoints() > 0); } return; } @@ -45,17 +45,17 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { // 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); + 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); + this.manifolds[this.nbManifolds-1].addContactPoint(contact); for (int i=0; igetNbContactPoints() > 0); + assert(this.manifolds[i].getNbContactPoints() > 0); } return; } @@ -63,10 +63,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { // 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(); + float minDepth = contact.getPenetrationDepth(); assert(this.nbManifolds == this.nbMaxManifolds); for (int i=0; igetLargestContactDepth(); + float depth = this.manifolds[i].getLargestContactDepth(); if (depth < minDepth) { minDepth = depth; smallestDepthIndex = i; @@ -80,15 +80,15 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { contact = null; return; } - assert(smallestDepthIndex >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj smallestDepthIndex < this.nbManifolds); + 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); + this.manifolds[this.nbManifolds-1].addContactPoint(contact); + assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0); for (int i=0; igetNbContactPoints() > 0); + assert(this.manifolds[i].getNbContactPoints() > 0); } return; } @@ -96,24 +96,24 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) { int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) { // Return the Id of the manifold with the same normal direction id (if exists) for (int i=0; igetNormalDirectionId()) { + if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) { return i; } } return -1; } -int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) { +int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal) { assert(normal.length2() > FLTEPSILON); int faceNo; float u, v; float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z())); - vec3 normalScaled = normal / max; - if (normalScaled.x() >= normalScaled.y() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj normalScaled.x() >= normalScaled.z()) { + Vector3f normalScaled = normal / 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() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj normalScaled.y() >= normalScaled.z()) { + } else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) { faceNo = normalScaled.y() > 0 ? 2 : 3; u = normalScaled.x(); v = normalScaled.z(); @@ -137,10 +137,10 @@ int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) { void ContactManifoldSet::update() { for (int i=this.nbManifolds-1; i>=0; i--) { // Update the contact manifold - this.manifolds[i]->update(this.shape1->getBody()->getTransform() * this.shape1->getLocalToBodyTransform(), - this.shape2->getBody()->getTransform() * this.shape2->getLocalToBodyTransform()); + this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(), + this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform()); // Remove the contact manifold if has no contact points anymore - if (this.manifolds[i]->getNbContactPoints() == 0) { + if (this.manifolds[i].getNbContactPoints() == 0) { removeManifold(i); } } @@ -161,7 +161,7 @@ void ContactManifoldSet::createManifold(int16t normalDirectionId) { void ContactManifoldSet::removeManifold(int index) { assert(this.nbManifolds > 0); - assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds); + assert(index >= 0 && index < this.nbManifolds); // Delete the new contact ETKDELETE(ContactManifold, this.manifolds[index]); this.manifolds[index] = null; @@ -184,14 +184,14 @@ int ContactManifoldSet::getNbContactManifolds() { } ContactManifold* ContactManifoldSet::getContactManifold(int index) { - assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds); + assert(index >= 0 && index < this.nbManifolds); return this.manifolds[index]; } int ContactManifoldSet::getTotalNbContactPoints() { int nbPoints = 0; for (int i=0; igetNbContactPoints(); + nbPoints += this.manifolds[i].getNbContactPoints(); } return nbPoints; } diff --git a/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.hpp b/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.java similarity index 65% rename from src/org/atriaSoft/ephysics/collision/ContactManifoldSet.hpp rename to src/org/atriaSoft/ephysics/collision/ContactManifoldSet.java index 9bf3d1c..5b3e8e7 100644 --- a/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.hpp +++ b/src/org/atriaSoft/ephysics/collision/ContactManifoldSet.java @@ -1,16 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision; -#include - -namespace ephysics { int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3; // Maximum number of contact manifolds in the set int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap /** @@ -21,13 +10,13 @@ namespace ephysics { */ class ContactManifoldSet { private: - int this.nbMaxManifolds; //!< Maximum number of contact manifolds in the set - int this.nbManifolds; //!< Current number of contact manifolds in the set - ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact - ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact - ContactManifold* this.manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set + int nbMaxManifolds; //!< Maximum number of contact manifolds in the set + int nbManifolds; //!< Current number of contact manifolds in the set + ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact + ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact + ContactManifold* manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set /// Create a new contact manifold and add it to the set - void createManifold(short normalDirectionId); + void createManifold(int normalDirectionId); /// Remove a contact manifold from the set void removeManifold(int index); // Return the index of the contact manifold with a similar average normal. @@ -35,14 +24,12 @@ namespace ephysics { // Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets) // Each face of the cube is divided into 4x4 buckets. This method maps the // normal vector into of the of the bucket and returns a unique Id for the bucket - int16t computeCubemapNormalId( vec3 normal) ; + int16t computeCubemapNormalId( Vector3f normal) ; public: /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, int nbMaxManifolds); - /// Destructor - ~ContactManifoldSet(); /// Return the first proxy shape ProxyShape* getShape1() ; /// Return the second proxy shape diff --git a/src/org/atriaSoft/ephysics/collision/ProxyShape.cpp b/src/org/atriaSoft/ephysics/collision/ProxyShape.cpp index e38abf4..f49e903 100644 --- a/src/org/atriaSoft/ephysics/collision/ProxyShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/ProxyShape.cpp @@ -14,10 +14,10 @@ using namespace ephysics; /** * @param body Pointer to the parent body * @param shape Pointer to the collision shape - * @param transform etk::Transform3Dation from collision shape local-space to body local-space + * @param transform Transform3Dation from collision shape local-space to body local-space * @param mass Mass of the collision shape (in kilograms) */ -ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, etk::Transform3D transform, float mass) +ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, Transform3D transform, float mass) :this.body(body), this.collisionShape(shape), this.localToBodyTransform(transform), this.mass(mass), this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL), this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) { @@ -37,10 +37,10 @@ ProxyShape::~ProxyShape() { * @param worldPoint Point to test in world-space coordinates * @return True if the point is inside the collision shape */ -boolean ProxyShape::testPointInside( vec3 worldPoint) { - etk::Transform3D localToWorld = this.body->getTransform() * this.localToBodyTransform; - vec3 localPoint = localToWorld.getInverse() * worldPoint; - return this.collisionShape->testPointInside(localPoint, this); +boolean ProxyShape::testPointInside( Vector3f worldPoint) { + Transform3D localToWorld = this.body.getTransform() * this.localToBodyTransform; + Vector3f localPoint = localToWorld.getInverse() * worldPoint; + return this.collisionShape.testPointInside(localPoint, this); } // Raycast method with feedback information @@ -53,16 +53,16 @@ boolean ProxyShape::testPointInside( vec3 worldPoint) { boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) { // If the corresponding body is not active, it cannot be hit by rays - if (!this.body->isActive()) return false; + if (!this.body.isActive()) return false; // Convert the ray into the local-space of the collision shape - etk::Transform3D localToWorldTransform = getLocalToWorldTransform(); - etk::Transform3D worldToLocalTransform = localToWorldTransform.getInverse(); + Transform3D localToWorldTransform = getLocalToWorldTransform(); + Transform3D worldToLocalTransform = localToWorldTransform.getInverse(); Ray rayLocal(worldToLocalTransform * ray.point1, worldToLocalTransform * ray.point2, ray.maxFraction); - boolean isHit = this.collisionShape->raycast(rayLocal, raycastInfo, this); + boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this); if (isHit == true) { // Convert the raycast info into world-space raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; @@ -122,19 +122,19 @@ void ProxyShape::setUserData(void* userData) { * @return The transformation that transforms the local-space of the collision shape * to the local-space of the parent body */ - etk::Transform3D ProxyShape::getLocalToBodyTransform() { + Transform3D ProxyShape::getLocalToBodyTransform() { return this.localToBodyTransform; } // Set the local to parent body transform -void ProxyShape::setLocalToBodyTransform( etk::Transform3D transform) { +void ProxyShape::setLocalToBodyTransform( Transform3D transform) { this.localToBodyTransform = transform; - this.body->setIsSleeping(false); + this.body.setIsSleeping(false); // Notify the body that the proxy shape has to be updated in the broad-phase - this.body->updateProxyShapeInBroadPhase(this, true); + this.body.updateProxyShapeInBroadPhase(this, true); } // Return the local to world transform @@ -142,8 +142,8 @@ void ProxyShape::setLocalToBodyTransform( etk::Transform3D transform) { * @return The transformation that transforms the local-space of the collision * shape to the world-space */ - etk::Transform3D ProxyShape::getLocalToWorldTransform() { - return this.body->this.transform * this.localToBodyTransform; + Transform3D ProxyShape::getLocalToWorldTransform() { + return this.body.this.transform * this.localToBodyTransform; } // Return the next proxy shape in the linked list of proxy shapes @@ -166,7 +166,7 @@ ProxyShape* ProxyShape::getNext() { /** * @return The collision category bits mask of the proxy shape */ -unsigned short ProxyShape::getCollisionCategoryBits() { +int ProxyShape::getCollisionCategoryBits() { return this.collisionCategoryBits; } @@ -174,7 +174,7 @@ unsigned short ProxyShape::getCollisionCategoryBits() { /** * @param collisionCategoryBits The collision category bits mask of the proxy shape */ -void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) { +void ProxyShape::setCollisionCategoryBits(int collisionCategoryBits) { this.collisionCategoryBits = collisionCategoryBits; } @@ -182,7 +182,7 @@ void ProxyShape::setCollisionCategoryBits(unsigned short collisionCategoryBits) /** * @return The bits mask that specifies with which collision category this shape will collide */ -unsigned short ProxyShape::getCollideWithMaskBits() { +int ProxyShape::getCollideWithMaskBits() { return this.collideWithMaskBits; } @@ -190,7 +190,7 @@ unsigned short ProxyShape::getCollideWithMaskBits() { /** * @param collideWithMaskBits The bits mask that specifies with which collision category this shape will collide */ -void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { +void ProxyShape::setCollideWithMaskBits(int collideWithMaskBits) { this.collideWithMaskBits = collideWithMaskBits; } @@ -198,21 +198,21 @@ void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) { /** * @return The local scaling vector */ -vec3 ProxyShape::getLocalScaling() { - return this.collisionShape->getScaling(); +Vector3f ProxyShape::getLocalScaling() { + return this.collisionShape.getScaling(); } // Set the local scaling vector of the collision shape /** * @param scaling The new local scaling vector */ -void ProxyShape::setLocalScaling( vec3 scaling) { +void ProxyShape::setLocalScaling( Vector3f scaling) { // Set the local scaling of the collision shape - this.collisionShape->setLocalScaling(scaling); + this.collisionShape.setLocalScaling(scaling); - this.body->setIsSleeping(false); + this.body.setIsSleeping(false); // Notify the body that the proxy shape has to be updated in the broad-phase - this.body->updateProxyShapeInBroadPhase(this, true); + this.body.updateProxyShapeInBroadPhase(this, true); } diff --git a/src/org/atriaSoft/ephysics/collision/ProxyShape.hpp b/src/org/atriaSoft/ephysics/collision/ProxyShape.java similarity index 60% rename from src/org/atriaSoft/ephysics/collision/ProxyShape.hpp rename to src/org/atriaSoft/ephysics/collision/ProxyShape.java index 4dc5a64..e4dd853 100644 --- a/src/org/atriaSoft/ephysics/collision/ProxyShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/ProxyShape.java @@ -1,17 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision; -#include -#include - -namespace ephysics { /** * @breif The CollisionShape instances are supposed to be unique for memory optimization. For instance, * consider two rigid bodies with the same sphere collision shape. In this situation, we will have @@ -23,14 +11,14 @@ namespace ephysics { */ class ProxyShape { protected: - CollisionBody* this.body; //!< Pointer to the parent body - CollisionShape* this.collisionShape; //!< Internal collision shape - etk::Transform3D this.localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time) - float this.mass; //!< Mass (in kilogramms) of the corresponding collision shape - ProxyShape* this.next; //!< Pointer to the next proxy shape of the body (linked list) - int this.broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree) - void* this.cachedCollisionData; //!< Cached collision data - void* this.userData; //!< Pointer to user data + CollisionBody* body; //!< Pointer to the parent body + CollisionShape* collisionShape; //!< Internal collision shape + Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time) + float mass; //!< Mass (in kilogramms) of the corresponding collision shape + ProxyShape* next; //!< Pointer to the next proxy shape of the body (linked list) + int broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree) + void* cachedCollisionData; //!< Cached collision data + void* userData; //!< 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 @@ -39,29 +27,21 @@ namespace ephysics { * categories of shapes collide with each other and do not collide with * other categories. */ - unsigned short this.collisionCategoryBits; + int collisionCategoryBits; /** * @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. */ - unsigned short this.collideWithMaskBits; - /// Private copy-ructor - ProxyShape( ProxyShape) = delete; - /// Private assignment operator - ProxyShape operator=( ProxyShape) = delete; + int collideWithMaskBits; public: /// Constructor ProxyShape(CollisionBody* body, CollisionShape* shape, - etk::Transform3D transform, + Transform3D transform, float mass); - - /// Destructor - virtual ~ProxyShape(); - /// Return the collision shape - CollisionShape* getCollisionShape() ; + CollisionShape* getCollisionShape() ; /// Return the parent body CollisionBody* getBody() ; @@ -76,31 +56,31 @@ namespace ephysics { void setUserData(void* userData); /// Return the local to parent body transform - etk::Transform3D getLocalToBodyTransform() ; + Transform3D getLocalToBodyTransform() ; /// Set the local to parent body transform - void setLocalToBodyTransform( etk::Transform3D transform); + void setLocalToBodyTransform( Transform3D transform); /// Return the local to world transform - etk::Transform3D getLocalToWorldTransform() ; + Transform3D getLocalToWorldTransform() ; /// Return true if a point is inside the collision shape - boolean testPointInside( vec3 worldPoint); + boolean testPointInside( Vector3f worldPoint); /// Raycast method with feedback information boolean raycast( Ray ray, RaycastInfo raycastInfo); /// Return the collision bits mask - unsigned short getCollideWithMaskBits() ; + int getCollideWithMaskBits() ; /// Set the collision bits mask - void setCollideWithMaskBits(unsigned short collideWithMaskBits); + void setCollideWithMaskBits(int collideWithMaskBits); /// Return the collision category bits - unsigned short getCollisionCategoryBits() ; + int getCollisionCategoryBits() ; /// Set the collision category bits - void setCollisionCategoryBits(unsigned short collisionCategoryBits); + void setCollisionCategoryBits(int collisionCategoryBits); /// Return the next proxy shape in the linked list of proxy shapes ProxyShape* getNext(); @@ -112,10 +92,10 @@ namespace ephysics { void** getCachedCollisionData(); /// Return the local scaling vector of the collision shape - vec3 getLocalScaling() ; + Vector3f getLocalScaling() ; /// Set the local scaling vector of the collision shape - virtual void setLocalScaling( vec3 scaling); + void setLocalScaling( Vector3f scaling); friend class OverlappingPair; friend class CollisionBody; diff --git a/src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp b/src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp index 495cf05..1511054 100644 --- a/src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp +++ b/src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp @@ -16,14 +16,14 @@ float RaycastTest::raycastAgainstShape(ProxyShape* shape, Ray ray) { // Ray casting test against the collision shape RaycastInfo raycastInfo; - boolean isHit = shape->raycast(ray, raycastInfo); + 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 userCallback->notifyRaycastHit(raycastInfo); + return userCallback.notifyRaycastHit(raycastInfo); } return ray.maxFraction; diff --git a/src/org/atriaSoft/ephysics/collision/RaycastInfo.hpp b/src/org/atriaSoft/ephysics/collision/RaycastInfo.java similarity index 74% rename from src/org/atriaSoft/ephysics/collision/RaycastInfo.hpp rename to src/org/atriaSoft/ephysics/collision/RaycastInfo.java index 40e0715..530a33c 100644 --- a/src/org/atriaSoft/ephysics/collision/RaycastInfo.hpp +++ b/src/org/atriaSoft/ephysics/collision/RaycastInfo.java @@ -1,20 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision; -#include -#include - -namespace ephysics { - class CollisionBody; - class ProxyShape; - class CollisionShape; /** * @brief It contains the information about a raycast hit. */ @@ -25,8 +10,8 @@ namespace ephysics { /// Private assignment operator RaycastInfo operator=( RaycastInfo) = delete; public: - vec3 worldPoint; //!< Hit point in world-space coordinates - vec3 worldNormal; //!< Surface normal at hit point in world-space coordinates + Vector3f worldPoint; //!< Hit point in world-space coordinates + Vector3f worldNormal; //!< Surface normal at hit point in world-space coordinates float hitFraction; //!< 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) int meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise) int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise) @@ -40,9 +25,6 @@ namespace ephysics { proxyShape(null) { } - - /// Destructor - virtual ~RaycastInfo() = default; }; /** @@ -53,8 +35,6 @@ namespace ephysics { */ class RaycastCallback { public: - /// Destructor - virtual ~RaycastCallback() = default; /** * @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 @@ -70,7 +50,7 @@ namespace ephysics { * @param[in] raycastInfo Information about the raycast hit * @return Value that controls the continuation of the ray after a hit */ - virtual float notifyRaycastHit( RaycastInfo raycastInfo)=0; + float notifyRaycastHit( RaycastInfo raycastInfo)=0; }; struct RaycastTest { diff --git a/src/org/atriaSoft/ephysics/collision/TriangleMesh.hpp b/src/org/atriaSoft/ephysics/collision/TriangleMesh.java similarity index 60% rename from src/org/atriaSoft/ephysics/collision/TriangleMesh.hpp rename to src/org/atriaSoft/ephysics/collision/TriangleMesh.java index 1e956d4..0288c98 100644 --- a/src/org/atriaSoft/ephysics/collision/TriangleMesh.hpp +++ b/src/org/atriaSoft/ephysics/collision/TriangleMesh.java @@ -1,16 +1,5 @@ -/** @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) - */ -#pragma once -#include -#include +package org.atriaSoft.ephysics.collision; -namespace ephysics { /** * @brief Represents a mesh made of triangles. A TriangleMesh contains * one or several parts. Each part is a set of triangles represented in a @@ -20,16 +9,12 @@ namespace ephysics { */ class TriangleMesh { protected: - etk::Vector this.triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part) + Vector triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part) public: /** * @brief Constructor */ TriangleMesh(); - /** - * @brief Virtualisation of Destructor - */ - virtual ~TriangleMesh() = default; /** * @brief Add a subpart of the mesh */ diff --git a/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp b/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp index f00137e..047be8a 100644 --- a/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp +++ b/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp @@ -9,25 +9,25 @@ #include -ephysics::TriangleVertexArray::TriangleVertexArray( etk::Vector vertices, etk::Vector triangles): +ephysics::TriangleVertexArray::TriangleVertexArray( Vector vertices, Vector triangles): this.vertices(vertices), this.triangles(triangles) { } -sizet ephysics::TriangleVertexArray::getNbVertices() { +long ephysics::TriangleVertexArray::getNbVertices() { return this.vertices.size(); } -sizet ephysics::TriangleVertexArray::getNbTriangles() { +long ephysics::TriangleVertexArray::getNbTriangles() { return this.triangles.size()/3; } - etk::Vector ephysics::TriangleVertexArray::getVertices() { + Vector ephysics::TriangleVertexArray::getVertices() { return this.vertices; } - etk::Vector ephysics::TriangleVertexArray::getIndices() { + Vector ephysics::TriangleVertexArray::getIndices() { return this.triangles; } diff --git a/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.hpp b/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.java similarity index 60% rename from src/org/atriaSoft/ephysics/collision/TriangleVertexArray.hpp rename to src/org/atriaSoft/ephysics/collision/TriangleVertexArray.java index 7c66039..2f5fdc4 100644 --- a/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.hpp +++ b/src/org/atriaSoft/ephysics/collision/TriangleVertexArray.java @@ -1,21 +1,9 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision; -#include -#include - -namespace ephysics { class Triangle { public: - vec3 value[3]; - vec3 operator[] (sizet id) { + Vector3f value[3]; + Vector3f operator[] (long id) { return value[id]; } }; @@ -30,36 +18,36 @@ namespace ephysics { */ class TriangleVertexArray { protected: - etk::Vector this.vertices; //!< Vertice list - etk::Vector this.triangles; //!< List of triangle (3 pos for each triangle) + Vector vertices; //!< Vertice list + Vector triangles; //!< List of triangle (3 pos for each triangle) public: /** * @brief Constructor * @param[in] vertices List Of all vertices * @param[in] triangles List of all linked points */ - TriangleVertexArray( etk::Vector vertices, - etk::Vector triangles); + TriangleVertexArray( Vector vertices, + Vector triangles); /** * @brief Get the number of vertices * @return Number of vertices */ - sizet getNbVertices() ; + long getNbVertices() ; /** * @brief Get the number of triangle * @return Number of triangles */ - sizet getNbTriangles() ; + long getNbTriangles() ; /** * @brief Get The table of the vertices * @return reference on the vertices */ - etk::Vector getVertices() ; + Vector getVertices() ; /** * @brief Get The table of the triangle indice * @return reference on the triangle indice */ - etk::Vector getIndices() ; + Vector getIndices() ; /** * @brief Get a triangle at the specific ID * @return Buffer of 3 points diff --git a/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp index b94c15f..337a537 100644 --- a/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -41,7 +41,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) { // If less than the quarter of allocated elements of the non-static shapes IDs array // are used, we release some allocated memory - if ((this.numberMovedShapes - this.numberNonUsedMovedShapes) < this.numberAllocatedMovedShapes / 4 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj + if ((this.numberMovedShapes - this.numberNonUsedMovedShapes) < this.numberAllocatedMovedShapes / 4 && this.numberAllocatedMovedShapes > 8) { this.numberAllocatedMovedShapes /= 2; @@ -75,14 +75,14 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, AABB a // Add the collision shape into the dynamic AABB tree and get its broad-phase ID int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape); // Set the broad-phase ID of the proxy shape - proxyShape->this.broadPhaseID = nodeId; + proxyShape.this.broadPhaseID = nodeId; // Add the collision shape into the array of bodies that have moved (or have been created) // during the last simulation step - addMovedCollisionShape(proxyShape->this.broadPhaseID); + addMovedCollisionShape(proxyShape.this.broadPhaseID); } void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { - int broadPhaseID = proxyShape->this.broadPhaseID; + int broadPhaseID = proxyShape.this.broadPhaseID; // Remove the collision shape from the dynamic AABB tree this.dynamicAABBTree.removeObject(broadPhaseID); // Remove the collision shape into the array of shapes that have moved (or have been created) @@ -92,9 +92,9 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, AABB aabb, - vec3 displacement, + Vector3f displacement, boolean forceReinsert) { - int broadPhaseID = proxyShape->this.broadPhaseID; + int broadPhaseID = proxyShape.this.broadPhaseID; assert(broadPhaseID >= 0); // Update the dynamic AABB tree according to the movement of the collision shape boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); @@ -107,7 +107,7 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, } } -static boolean sortFunction( etk::Pair pair1, etk::Pair pair2) { +static boolean sortFunction( Pair pair1, Pair pair2) { if (pair1.first < pair2.first) { return true; } @@ -137,19 +137,19 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { return; } // Add the new potential pair into the array of potential overlapping pairs - this.potentialPairs.pushBack(etk::makePair(etk::min(it, nodeId), etk::max(it, nodeId) )); + this.potentialPairs.pushBack(makePair(min(it, nodeId), max(it, nodeId) )); }); } // 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 - etk::algorithm::quickSort(this.potentialPairs, sortFunction); + algorithm::quickSort(this.potentialPairs, sortFunction); // 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 - etk::Pair pair = (this.potentialPairs[iii]); + Pair pair = (this.potentialPairs[iii]); ++iii; // Get the two collision shapes of the pair ProxyShape* shape1 = staticcast(this.dynamicAABBTree.getNodeDataPointer(pair.first)); @@ -159,7 +159,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() { // Skip the duplicate overlapping pairs while (iii < this.potentialPairs.size()) { // Get the next pair - etk::Pair nextPair = this.potentialPairs[iii]; + Pair nextPair = this.potentialPairs[iii]; // If the next pair is different from the previous one, we stop skipping pairs if ( nextPair.first != pair.first || nextPair.second != pair.second) { @@ -175,7 +175,7 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) { // Get the proxy shape from the node ProxyShape* proxyShape = staticcast(this.dynamicAABBTree.getNodeDataPointer(nodeId)); // Check if the raycast filtering mask allows raycast against this shape - if ((this.raycastWithCategoryMaskBits proxyShape->getCollisionCategoryBits()) != 0) { + 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 @@ -187,15 +187,15 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) { boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1, ProxyShape* shape2) { // Get the two AABBs of the collision shapes - AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1->this.broadPhaseID); - AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2->this.broadPhaseID); + AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.this.broadPhaseID); + AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2.this.broadPhaseID); // Check if the two AABBs are overlapping return aabb1.testCollision(aabb2); } void BroadPhaseAlgorithm::raycast( Ray ray, RaycastTest raycastTest, - unsigned short raycastWithCategoryMaskBits) { + int raycastWithCategoryMaskBits) { PROFILE("BroadPhaseAlgorithm::raycast()"); BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); diff --git a/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java similarity index 69% rename from src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp rename to src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java index 905f219..d5f3b8f 100644 --- a/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp +++ b/src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java @@ -1,23 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.broadphase; -#include -#include -#include -#include -#include - -namespace ephysics { - - class CollisionDetection; - class BroadPhaseAlgorithm; // TODO : remove this as callback ... DynamicAABBTreeOverlapCallback { /** @@ -27,12 +9,12 @@ namespace ephysics { class BroadPhaseRaycastCallback { private : DynamicAABBTree this.dynamicAABBTree; - unsigned short this.raycastWithCategoryMaskBits; + int this.raycastWithCategoryMaskBits; RaycastTest this.raycastTest; public: // Constructor BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree, - unsigned short raycastWithCategoryMaskBits, + int raycastWithCategoryMaskBits, RaycastTest raycastTest): this.dynamicAABBTree(dynamicAABBTree), this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits), @@ -53,8 +35,8 @@ namespace ephysics { class BroadPhaseAlgorithm { protected : DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree - etk::Vector this.movedShapes; //!< 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. - etk::Vector> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) + Vector this.movedShapes; //!< 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. + Vector> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) CollisionDetection this.collisionDetection; //!< Reference to the collision detection object /// Private copy-ructor BroadPhaseAlgorithm( BroadPhaseAlgorithm obj); @@ -64,7 +46,7 @@ namespace ephysics { /// Constructor BroadPhaseAlgorithm(CollisionDetection collisionDetection); /// Destructor - virtual ~BroadPhaseAlgorithm(); + ~BroadPhaseAlgorithm(); /// Add a proxy collision shape into the broad-phase collision detection void addProxyCollisionShape(ProxyShape* proxyShape, AABB aabb); /// Remove a proxy collision shape from the broad-phase collision detection @@ -72,7 +54,7 @@ namespace ephysics { /// Notify the broad-phase that a collision shape has moved and need to be updated void updateProxyCollisionShape(ProxyShape* proxyShape, AABB aabb, - vec3 displacement, + Vector3f displacement, boolean forceReinsert = false); /// 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. @@ -87,7 +69,7 @@ namespace ephysics { /// Ray casting method void raycast( Ray ray, RaycastTest raycastTest, - unsigned short raycastWithCategoryMaskBits) ; + int raycastWithCategoryMaskBits) ; }; } diff --git a/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp b/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp index b879aae..00df7f9 100644 --- a/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp +++ b/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp @@ -85,7 +85,7 @@ int DynamicAABBTree::allocateNode() { // Release a node void DynamicAABBTree::releaseNode(int nodeID) { assert(this.numberNodes > 0); - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].height >= 0); this.nodes[nodeID].nextNodeID = this.freeNodeID; this.nodes[nodeID].height = -1; @@ -98,7 +98,7 @@ int DynamicAABBTree::addObjectInternal( AABB aabb) { // Get the next available node (or allocate new ones if necessary) int nodeID = allocateNode(); // Create the fat aabb to use in the tree - vec3 gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); + Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); this.nodes[nodeID].aabb.setMin(aabb.getMin() - gap); this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap); // Set the height of the node in the tree @@ -113,7 +113,7 @@ int DynamicAABBTree::addObjectInternal( AABB aabb) { // Remove an object from the tree void DynamicAABBTree::removeObject(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].isLeaf()); // Remove the node from the tree removeLeafNode(nodeID); @@ -128,43 +128,43 @@ void DynamicAABBTree::removeObject(int nodeID) { /// 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). -boolean DynamicAABBTree::updateObject(int nodeID, AABB newAABB, vec3 displacement, bool forceReinsert) { +boolean DynamicAABBTree::updateObject(int nodeID, AABB newAABB, Vector3f displacement, bool forceReinsert) { PROFILE("DynamicAABBTree::updateObject()"); - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].isLeaf()); assert(this.nodes[nodeID].height >= 0); - Log.verbose(" compare : " << this.nodes[nodeID].aabb.this.minCoordinates << " " << this.nodes[nodeID].aabb.this.maxCoordinates); - Log.verbose(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates); + Log.verbose(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates); + Log.verbose(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates); // If the new AABB is still inside the fat AABB of the node if ( forceReinsert == false - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.nodes[nodeID].aabb.contains(newAABB)) { + && this.nodes[nodeID].aabb.contains(newAABB)) { return false; } // If the new AABB is outside the fat AABB, we remove the corresponding node removeLeafNode(nodeID); // Compute the fat AABB by inflating the AABB with a ant gap this.nodes[nodeID].aabb = newAABB; - vec3 gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); - this.nodes[nodeID].aabb.this.minCoordinates -= gap; - this.nodes[nodeID].aabb.this.maxCoordinates += gap; + Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); + this.nodes[nodeID].aabb.minCoordinates -= gap; + this.nodes[nodeID].aabb.maxCoordinates += gap; // Inflate the fat AABB in direction of the linear motion of the AABB if (displacement.x() < 0.0f) { - this.nodes[nodeID].aabb.this.minCoordinates.setX(this.nodes[nodeID].aabb.this.minCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x()); + this.nodes[nodeID].aabb.minCoordinates.setX(this.nodes[nodeID].aabb.minCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x()); } else { - this.nodes[nodeID].aabb.this.maxCoordinates.setX(this.nodes[nodeID].aabb.this.maxCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x()); + this.nodes[nodeID].aabb.maxCoordinates.setX(this.nodes[nodeID].aabb.maxCoordinates.x() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.x()); } if (displacement.y() < 0.0f) { - this.nodes[nodeID].aabb.this.minCoordinates.setY(this.nodes[nodeID].aabb.this.minCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y()); + this.nodes[nodeID].aabb.minCoordinates.setY(this.nodes[nodeID].aabb.minCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y()); } else { - this.nodes[nodeID].aabb.this.maxCoordinates.setY(this.nodes[nodeID].aabb.this.maxCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y()); + this.nodes[nodeID].aabb.maxCoordinates.setY(this.nodes[nodeID].aabb.maxCoordinates.y() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.y()); } if (displacement.z() < 0.0f) { - this.nodes[nodeID].aabb.this.minCoordinates.setZ(this.nodes[nodeID].aabb.this.minCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z()); + this.nodes[nodeID].aabb.minCoordinates.setZ(this.nodes[nodeID].aabb.minCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z()); } else { - this.nodes[nodeID].aabb.this.maxCoordinates.setZ(this.nodes[nodeID].aabb.this.maxCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z()); + this.nodes[nodeID].aabb.maxCoordinates.setZ(this.nodes[nodeID].aabb.maxCoordinates.z() + DYNAMICTREEAABBLINGAPMULTIPLIER *displacement.z()); } - Log.error(" compare : " << this.nodes[nodeID].aabb.this.minCoordinates << " " << this.nodes[nodeID].aabb.this.maxCoordinates); - Log.error(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates); + Log.error(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates); + Log.error(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates); if (this.nodes[nodeID].aabb.contains(newAABB) == false) { //Log.critical("ERROR"); } @@ -222,7 +222,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { } // 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 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj costS < costRight) { + if (costS < costLeft && costS < costRight) { break; } // It is cheaper to go down into a child of the current node, choose the best child @@ -273,7 +273,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { assert(leftChild != TreeNode::NULLTREENODE); assert(rightChild != TreeNode::NULLTREENODE); // Recompute the height of the node in the tree - this.nodes[currentNodeID].height = etk::max(this.nodes[leftChild].height, + this.nodes[currentNodeID].height = max(this.nodes[leftChild].height, this.nodes[rightChild].height) + 1; assert(this.nodes[currentNodeID].height > 0); // Recompute the AABB of the node @@ -285,7 +285,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) { // Remove a leaf node from the tree void DynamicAABBTree::removeLeafNode(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].isLeaf()); // If we are removing the root node (root node is a leaf in this case) if (this.rootNodeID == nodeID) { @@ -324,7 +324,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) { // Recompute the AABB and the height of the current node this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb, this.nodes[rightChildID].aabb); - this.nodes[currentNodeID].height = etk::max(this.nodes[leftChildID].height, + this.nodes[currentNodeID].height = max(this.nodes[leftChildID].height, this.nodes[rightChildID].height) + 1; assert(this.nodes[currentNodeID].height > 0); currentNodeID = this.nodes[currentNodeID].parentID; @@ -344,123 +344,123 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { assert(nodeID != TreeNode::NULLTREENODE); TreeNode* nodeA = this.nodes + nodeID; // If the node is a leaf or the height of A's sub-tree is less than 2 - if (nodeA->isLeaf() || nodeA->height < 2) { + if (nodeA.isLeaf() || nodeA.height < 2) { // Do not perform any rotation return nodeID; } // Get the two children nodes - int nodeBID = nodeA->children[0]; - int nodeCID = nodeA->children[1]; - assert(nodeBID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeBID < this.numberAllocatedNodes); - assert(nodeCID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeCID < this.numberAllocatedNodes); + int nodeBID = nodeA.children[0]; + int nodeCID = nodeA.children[1]; + assert(nodeBID >= 0 && nodeBID < this.numberAllocatedNodes); + assert(nodeCID >= 0 && nodeCID < this.numberAllocatedNodes); TreeNode* nodeB = this.nodes + nodeBID; TreeNode* nodeC = this.nodes + nodeCID; // Compute the factor of the left and right sub-trees - int balanceFactor = nodeC->height - nodeB->height; + int balanceFactor = nodeC.height - nodeB.height; // If the right node C is 2 higher than left node B if (balanceFactor > 1) { - assert(!nodeC->isLeaf()); - int nodeFID = nodeC->children[0]; - int nodeGID = nodeC->children[1]; - assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes); - assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes); + assert(!nodeC.isLeaf()); + int nodeFID = nodeC.children[0]; + int nodeGID = nodeC.children[1]; + assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes); TreeNode* nodeF = this.nodes + nodeFID; TreeNode* nodeG = this.nodes + nodeGID; - nodeC->children[0] = nodeID; - nodeC->parentID = nodeA->parentID; - nodeA->parentID = nodeCID; - if (nodeC->parentID != TreeNode::NULLTREENODE) { - if (this.nodes[nodeC->parentID].children[0] == nodeID) { - this.nodes[nodeC->parentID].children[0] = nodeCID; + nodeC.children[0] = nodeID; + nodeC.parentID = nodeA.parentID; + nodeA.parentID = nodeCID; + if (nodeC.parentID != TreeNode::NULLTREENODE) { + if (this.nodes[nodeC.parentID].children[0] == nodeID) { + this.nodes[nodeC.parentID].children[0] = nodeCID; } else { - assert(this.nodes[nodeC->parentID].children[1] == nodeID); - this.nodes[nodeC->parentID].children[1] = nodeCID; + assert(this.nodes[nodeC.parentID].children[1] == nodeID); + this.nodes[nodeC.parentID].children[1] = nodeCID; } } else { this.rootNodeID = nodeCID; } - assert(!nodeC->isLeaf()); - assert(!nodeA->isLeaf()); + 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) { - nodeC->children[1] = nodeFID; - nodeA->children[1] = nodeGID; - nodeG->parentID = nodeID; + if (nodeF.height > nodeG.height) { + nodeC.children[1] = nodeFID; + nodeA.children[1] = nodeGID; + nodeG.parentID = nodeID; // Recompute the AABB of node A and C - nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); - nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); // Recompute the height of node A and C - nodeA->height = etk::max(nodeB->height, nodeG->height) + 1; - nodeC->height = etk::max(nodeA->height, nodeF->height) + 1; - assert(nodeA->height > 0); - assert(nodeC->height > 0); + nodeA.height = max(nodeB.height, nodeG.height) + 1; + nodeC.height = 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 - nodeC->children[1] = nodeGID; - nodeA->children[1] = nodeFID; - nodeF->parentID = nodeID; + nodeC.children[1] = nodeGID; + nodeA.children[1] = nodeFID; + nodeF.parentID = nodeID; // Recompute the AABB of node A and C - nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); - nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); + nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb); + nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); // Recompute the height of node A and C - nodeA->height = etk::max(nodeB->height, nodeF->height) + 1; - nodeC->height = etk::max(nodeA->height, nodeG->height) + 1; - assert(nodeA->height > 0); - assert(nodeC->height > 0); + nodeA.height = max(nodeB.height, nodeF.height) + 1; + nodeC.height = max(nodeA.height, nodeG.height) + 1; + assert(nodeA.height > 0); + assert(nodeC.height > 0); } // Return the new root of the sub-tree return nodeCID; } // If the left node B is 2 higher than right node C if (balanceFactor < -1) { - assert(!nodeB->isLeaf()); - int nodeFID = nodeB->children[0]; - int nodeGID = nodeB->children[1]; - assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes); - assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes); + assert(!nodeB.isLeaf()); + int nodeFID = nodeB.children[0]; + int nodeGID = nodeB.children[1]; + assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes); + assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes); TreeNode* nodeF = this.nodes + nodeFID; TreeNode* nodeG = this.nodes + nodeGID; - nodeB->children[0] = nodeID; - nodeB->parentID = nodeA->parentID; - nodeA->parentID = nodeBID; - if (nodeB->parentID != TreeNode::NULLTREENODE) { - if (this.nodes[nodeB->parentID].children[0] == nodeID) { - this.nodes[nodeB->parentID].children[0] = nodeBID; + nodeB.children[0] = nodeID; + nodeB.parentID = nodeA.parentID; + nodeA.parentID = nodeBID; + if (nodeB.parentID != TreeNode::NULLTREENODE) { + if (this.nodes[nodeB.parentID].children[0] == nodeID) { + this.nodes[nodeB.parentID].children[0] = nodeBID; } else { - assert(this.nodes[nodeB->parentID].children[1] == nodeID); - this.nodes[nodeB->parentID].children[1] = nodeBID; + assert(this.nodes[nodeB.parentID].children[1] == nodeID); + this.nodes[nodeB.parentID].children[1] = nodeBID; } } else { this.rootNodeID = nodeBID; } - assert(!nodeB->isLeaf()); - assert(!nodeA->isLeaf()); + 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) { - nodeB->children[1] = nodeFID; - nodeA->children[0] = nodeGID; - nodeG->parentID = nodeID; + if (nodeF.height > nodeG.height) { + nodeB.children[1] = nodeFID; + nodeA.children[0] = nodeGID; + nodeG.parentID = nodeID; // Recompute the AABB of node A and B - nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); - nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb); // Recompute the height of node A and B - nodeA->height = etk::max(nodeC->height, nodeG->height) + 1; - nodeB->height = etk::max(nodeA->height, nodeF->height) + 1; - assert(nodeA->height > 0); - assert(nodeB->height > 0); + nodeA.height = max(nodeC.height, nodeG.height) + 1; + nodeB.height = 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 - nodeB->children[1] = nodeGID; - nodeA->children[0] = nodeFID; - nodeF->parentID = nodeID; + nodeB.children[1] = nodeGID; + nodeA.children[0] = nodeFID; + nodeF.parentID = nodeID; // Recompute the AABB of node A and B - nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); - nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); + nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb); + nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb); // Recompute the height of node A and B - nodeA->height = etk::max(nodeC->height, nodeF->height) + 1; - nodeB->height = etk::max(nodeA->height, nodeG->height) + 1; - assert(nodeA->height > 0); - assert(nodeB->height > 0); + nodeA.height = max(nodeC.height, nodeF.height) + 1; + nodeB.height = max(nodeA.height, nodeG.height) + 1; + assert(nodeA.height > 0); + assert(nodeB.height > 0); } // Return the new root of the sub-tree return nodeBID; @@ -470,7 +470,7 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) { } /// Report all shapes overlapping with the AABB given in parameter. -void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function callback) { +void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, Function callback) { if (callback == null) { Log.error("call with null callback"); return; @@ -489,23 +489,23 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Functi // Get the corresponding node TreeNode* nodeToVisit = this.nodes + nodeIDToVisit; // If the AABB in parameter overlaps with the AABB of the node to visit - if (aabb.testCollision(nodeToVisit->aabb)) { + if (aabb.testCollision(nodeToVisit.aabb)) { // If the node is a leaf - if (nodeToVisit->isLeaf()) { + if (nodeToVisit.isLeaf()) { // Notify the broad-phase about a new potential overlapping pair callback(nodeIDToVisit); } else { // If the node is not a leaf // We need to visit its children - stack.push(nodeToVisit->children[0]); - stack.push(nodeToVisit->children[1]); + stack.push(nodeToVisit.children[0]); + stack.push(nodeToVisit.children[1]); } } } } // Ray casting method -void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function callback) { +void DynamicAABBTree::raycast( ephysics::Ray ray, Function callback) { PROFILE("DynamicAABBTree::raycast()"); if (callback == null) { Log.error("call with null callback"); @@ -527,11 +527,11 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Functionaabb.testRayIntersect(rayTemp) == false) { + if (node.aabb.testRayIntersect(rayTemp) == false) { continue; } // If the node is a leaf of the tree - if (node->isLeaf()) { + if (node.isLeaf()) { // Call the callback that will raycast again the broad-phase shape float hitFraction = callback(nodeID, rayTemp); // If the user returned a hitFraction of zero, it means that @@ -551,8 +551,8 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Functionchildren[0]); - stack.push(node->children[1]); + stack.push(node.children[0]); + stack.push(node.children[1]); } } } @@ -564,20 +564,20 @@ boolean TreeNode::isLeaf() { // Return the fat AABB corresponding to a given node ID AABB DynamicAABBTree::getFatAABB(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); return this.nodes[nodeID].aabb; } // Return the pointer to the data array of a given leaf node of the tree int* DynamicAABBTree::getNodeDataInt(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].isLeaf()); return this.nodes[nodeID].dataInt; } // Return the pointer to the data pointer of a given leaf node of the tree void* DynamicAABBTree::getNodeDataPointer(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); assert(this.nodes[nodeID].isLeaf()); return this.nodes[nodeID].dataPointer; } @@ -615,7 +615,7 @@ void DynamicAABBTree::check() { int freeNodeID = this.freeNodeID; // Check the free nodes while(freeNodeID != TreeNode::NULLTREENODE) { - assert(0 <= freeNodeID hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj freeNodeID < this.numberAllocatedNodes); + assert(0 <= freeNodeID && freeNodeID < this.numberAllocatedNodes); freeNodeID = this.nodes[freeNodeID].nextNodeID; nbFreeNodes++; } @@ -633,26 +633,26 @@ void DynamicAABBTree::checkNode(int nodeID) { } // Get the children nodes TreeNode* pNode = this.nodes + nodeID; - assert(!pNode->isLeaf()); - int leftChild = pNode->children[0]; - int rightChild = pNode->children[1]; - assert(pNode->height >= 0); - assert(pNode->aabb.getVolume() > 0); + assert(!pNode.isLeaf()); + int leftChild = pNode.children[0]; + int rightChild = pNode.children[1]; + assert(pNode.height >= 0); + assert(pNode.aabb.getVolume() > 0); // If the current node is a leaf - if (pNode->isLeaf()) { + if (pNode.isLeaf()) { // Check that there are no children assert(leftChild == TreeNode::NULLTREENODE); assert(rightChild == TreeNode::NULLTREENODE); - assert(pNode->height == 0); + assert(pNode.height == 0); } else { // Check that the children node IDs are valid - assert(0 <= leftChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj leftChild < this.numberAllocatedNodes); - assert(0 <= rightChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj rightChild < this.numberAllocatedNodes); + assert(0 <= leftChild && leftChild < this.numberAllocatedNodes); + assert(0 <= rightChild && rightChild < this.numberAllocatedNodes); // Check that the children nodes have the correct parent node assert(this.nodes[leftChild].parentID == nodeID); assert(this.nodes[rightChild].parentID == nodeID); // Check the height of node - int height = 1 + etk::max(this.nodes[leftChild].height, this.nodes[rightChild].height); + int height = 1 + max(this.nodes[leftChild].height, this.nodes[rightChild].height); assert(this.nodes[nodeID].height == height); // Check the AABB of the node AABB aabb; @@ -672,17 +672,17 @@ int DynamicAABBTree::computeHeight() { // Compute the height of a given node in the tree int DynamicAABBTree::computeHeight(int nodeID) { - assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); + assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes); TreeNode* node = this.nodes + nodeID; // If the node is a leaf, its height is zero - if (node->isLeaf()) { + if (node.isLeaf()) { return 0; } // Compute the height of the left and right sub-tree - int leftHeight = computeHeight(node->children[0]); - int rightHeight = computeHeight(node->children[1]); + int leftHeight = computeHeight(node.children[0]); + int rightHeight = computeHeight(node.children[1]); // Return the height of the node - return 1 + etk::max(leftHeight, rightHeight); + return 1 + max(leftHeight, rightHeight); } #endif diff --git a/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.hpp b/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.java similarity index 81% rename from src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.hpp rename to src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.java index 293ef42..cc30b6b 100644 --- a/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.hpp +++ b/src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.java @@ -1,19 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.broadphase; -#include -#include -#include -#include - -namespace ephysics { // TODO: to replace this, create a Tree template (multiple child) or TreeRedBlack /** * @brief It represents a node of the dynamic AABB tree. @@ -85,7 +71,7 @@ namespace ephysics { /// Constructor DynamicAABBTree(float extraAABBGap = 0.0f); /// Destructor - virtual ~DynamicAABBTree(); + ~DynamicAABBTree(); /// Add an object into the tree (where node data are two integers) int addObject( AABB aabb, int data1, int data2); /// Add an object into the tree (where node data is a pointer) @@ -93,7 +79,7 @@ namespace ephysics { /// Remove an object from the tree void removeObject(int nodeID); /// Update the dynamic tree after an object has moved. - boolean updateObject(int nodeID, AABB newAABB, vec3 displacement, bool forceReinsert = false); + boolean updateObject(int nodeID, AABB newAABB, Vector3f displacement, bool forceReinsert = false); /// Return the fat AABB corresponding to a given node ID AABB getFatAABB(int nodeID) ; /// Return the pointer to the data array of a given leaf node of the tree @@ -101,9 +87,9 @@ namespace ephysics { /// Return the data pointer of a given leaf node of the tree void* getNodeDataPointer(int nodeID) ; /// Report all shapes overlapping with the AABB given in parameter. - void reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function callback) ; + void reportAllShapesOverlappingWithAABB( AABB aabb, Function callback) ; /// Ray casting method - void raycast( Ray ray, etk::Function callback) ; + void raycast( Ray ray, Function callback) ; /// Compute the height of the tree int computeHeight(); /// Return the root AABB of the tree diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.hpp deleted file mode 100644 index d9eefd9..0000000 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.hpp +++ /dev/null @@ -1,37 +0,0 @@ -/** @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) - */ -#pragma once - -#include - -namespace ephysics { - /** - * @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. - */ - class CollisionDispatch { - public: - /// Constructor - CollisionDispatch() {} - /// Destructor - virtual ~CollisionDispatch() = default; - /// Initialize the collision dispatch configuration - virtual void init(CollisionDetection* collisionDetection) { - // Nothing to do ... - } - /// Select and return the narrow-phase collision detection algorithm to - /// use between two types of collision shapes. - virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, - int shape2Type) = 0; - }; - -} - - 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..9408709 --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.java @@ -0,0 +1,21 @@ +package org.atriaSoft.ephysics.collision.narrowphase; + + /** + * @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. + */ + class CollisionDispatch { + public: + /// Initialize the collision dispatch configuration + void init(CollisionDetection* collisionDetection) { + // Nothing to do ... + } + /// Select and return the narrow-phase collision detection algorithm to + /// use between two types of collision shapes. + NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type) = 0; + }; + +} + + diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp index 4f6590d..151def5 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp @@ -27,7 +27,7 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info, ConvexShape* convexShape; ConcaveShape* concaveShape; // Collision shape 1 is convex, collision shape 2 is concave - if (shape1Info.collisionShape->isConvex()) { + if (shape1Info.collisionShape.isConvex()) { convexProxyShape = shape1Info.proxyShape; convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape); concaveProxyShape = shape2Info.proxyShape; @@ -48,48 +48,48 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info, convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); // Compute the convex shape AABB in the local-space of the convex shape AABB aabb; - convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); + convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform()); // If smooth mesh collision is enabled for the concave mesh - if (concaveShape->getIsSmoothMeshCollisionEnabled()) { - etk::Vector contactPoints; + if (concaveShape.getIsSmoothMeshCollisionEnabled()) { + Vector contactPoints; SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback); // Call the convex vs triangle callback for each triangle of the concave shape - concaveShape->testAllTriangles(convexVsTriangleCallback, aabb); + 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); + concaveShape.testAllTriangles(convexVsTriangleCallback, aabb); } } -void ConvexVsTriangleCallback::testTriangle( vec3* trianglePoints) { +void ConvexVsTriangleCallback::testTriangle( Vector3f* trianglePoints) { // Create a triangle collision shape - float margin = this.concaveShape->getTriangleMargin(); + float margin = this.concaveShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); // Select the collision algorithm to use between the triangle and the convex shape - NarrowPhaseAlgorithm* algo = this.collisionDetection->getCollisionAlgorithm(triangleShape.getType(), this.convexShape->getType()); + 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); + algo.setCurrentOverlappingPair(this.overlappingPair); // Create the CollisionShapeInfo objects CollisionShapeInfo shapeConvexInfo(this.convexProxyShape, this.convexShape, - this.convexProxyShape->getLocalToWorldTransform(), + this.convexProxyShape.getLocalToWorldTransform(), this.overlappingPair, - this.convexProxyShape->getCachedCollisionData()); + this.convexProxyShape.getCachedCollisionData()); CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape, triangleShape, - this.concaveProxyShape->getLocalToWorldTransform(), + this.concaveProxyShape.getLocalToWorldTransform(), this.overlappingPair, - this.concaveProxyShape->getCachedCollisionData()); + this.concaveProxyShape.getCachedCollisionData()); // Use the collision algorithm to test collision between the triangle and the other convex shape - algo->testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback); + algo.testCollision(shapeConvexInfo, shapeConcaveInfo, this.narrowPhaseCallback); } static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactInfo contact2) { @@ -97,17 +97,17 @@ static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactI } void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, - etk::Vector contactPoints, + Vector contactPoints, NarrowPhaseCallback* callback) { // Set with the triangle vertices already processed to void further contacts with same triangle - etk::Vector> processTriangleVertices; + Vector> processTriangleVertices; // Sort the list of narrow-phase contacts according to their penetration depth - etk::algorithm::quickSort(contactPoints, sortFunction); + algorithm::quickSort(contactPoints, sortFunction); // For each contact point (from smaller penetration depth to larger) - etk::Vector::Iterator it; + Vector::Iterator it; for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { SmoothMeshContactInfo info = *it; - vec3 contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; // Compute the barycentric coordinates of the point in the triangle float u, v, w; computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], @@ -129,21 +129,21 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl } // If it is a vertex contact if (nbZeros == 2) { - vec3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]); + 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); + callback.notifyContact(overlappingPair, info.contactInfo); } } else if (nbZeros == 1) { // If it is an edge contact - vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); - vec3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]); + Vector3f contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); + 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) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj + if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { // Keep the contact as it is and report it - callback->notifyContact(overlappingPair, info.contactInfo); + callback.notifyContact(overlappingPair, info.contactInfo); } } else { // If it is a face contact @@ -151,19 +151,19 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl ProxyShape* firstShape; ProxyShape* secondShape; if (info.isFirstShapeTriangle) { - firstShape = overlappingPair->getShape1(); - secondShape = overlappingPair->getShape2(); + firstShape = overlappingPair.getShape1(); + secondShape = overlappingPair.getShape2(); } else { - firstShape = overlappingPair->getShape2(); - secondShape = overlappingPair->getShape1(); + firstShape = overlappingPair.getShape2(); + secondShape = overlappingPair.getShape1(); } // We use the triangle normal as the contact normal - vec3 a = info.triangleVertices[1] - info.triangleVertices[0]; - vec3 b = info.triangleVertices[2] - info.triangleVertices[0]; - vec3 localNormal = a.cross(b); - newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; - vec3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; - vec3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; + Vector3f a = info.triangleVertices[1] - info.triangleVertices[0]; + Vector3f b = info.triangleVertices[2] - info.triangleVertices[0]; + Vector3f localNormal = a.cross(b); + newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation() * localNormal; + Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; + Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform() * firstLocalPoint; newContactInfo.normal.normalize(); if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { newContactInfo.normal = -newContactInfo.normal; @@ -171,16 +171,16 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl // 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 - etk::Transform3D worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); + Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().getInverse(); if (info.isFirstShapeTriangle) { - vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; + Vector3f newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; } else { - vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; + Vector3f newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; } // Report the contact - callback->notifyContact(overlappingPair, newContactInfo); + callback.notifyContact(overlappingPair, newContactInfo); } // Add the three vertices of the triangle to the set of processed // triangle vertices @@ -190,14 +190,14 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl } } -boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector> processTriangleVertices, vec3 vertex) { - /* TODO : etk::Vector> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good... +boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( Vector> processTriangleVertices, Vector3f vertex) { + /* TODO : Vector> 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.equalrange(key); for (auto it = range.first; it != range.second; ++it) { - if ( vertex.x() == it->second.x() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.y() == it->second.y() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.z() == it->second.z()) { + if ( vertex.x() == it.second.x() + && vertex.y() == it.second.y() + && vertex.z() == it.second.z()) { return true; } } @@ -206,8 +206,8 @@ boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::VectorgetType() == TRIANGLE) { - assert(contactInfo.collisionShape2->getType() != TRIANGLE); + if (contactInfo.collisionShape1.getType() == TRIANGLE) { + assert(contactInfo.collisionShape2.getType() != TRIANGLE); TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape1); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); + 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() == TRIANGLE); + assert(contactInfo.collisionShape2.getType() == TRIANGLE); TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape2); - triangleVertices[0] = triangleShape->getVertex(0); - triangleVertices[1] = triangleShape->getVertex(1); - triangleVertices[2] = triangleShape->getVertex(2); + triangleVertices[0] = triangleShape.getVertex(0); + triangleVertices[1] = triangleShape.getVertex(1); + triangleVertices[2] = triangleShape.getVertex(2); isFirstShapeTriangle = false; } SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java similarity index 71% rename from src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java index 98b10b6..6489cc7 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java @@ -1,25 +1,12 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.narrowphase; -#include -#include -#include - -namespace ephysics { /** * @brief 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 : public TriangleCallback { + class ConvexVsTriangleCallback extends TriangleCallback { protected: CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback @@ -57,7 +44,7 @@ namespace ephysics { this.concaveProxyShape = concaveProxyShape; } /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle( vec3* trianglePoints); + void testTriangle( Vector3f* trianglePoints); }; /** @@ -68,13 +55,13 @@ namespace ephysics { public: ContactPointInfo contactInfo; boolean isFirstShapeTriangle; - vec3 triangleVertices[3]; + Vector3f triangleVertices[3]; /// Constructor SmoothMeshContactInfo( ContactPointInfo contact, boolean firstShapeTriangle, - vec3 trianglePoint1, - vec3 trianglePoint2, - vec3 trianglePoint3): + Vector3f trianglePoint1, + Vector3f trianglePoint2, + Vector3f trianglePoint3): contactInfo(contact) { isFirstShapeTriangle = firstShapeTriangle; triangleVertices[0] = trianglePoint1; @@ -82,7 +69,7 @@ namespace ephysics { triangleVertices[2] = trianglePoint3; } SmoothMeshContactInfo() { - // TODO: add it for etk::Vector + // TODO: add it for Vector } }; @@ -99,17 +86,17 @@ namespace ephysics { * 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 : public NarrowPhaseCallback { + class SmoothCollisionNarrowPhaseCallback extends NarrowPhaseCallback { private: - etk::Vector this.contactPoints; + Vector this.contactPoints; public: // Constructor - SmoothCollisionNarrowPhaseCallback(etk::Vector contactPoints): + SmoothCollisionNarrowPhaseCallback(Vector contactPoints): this.contactPoints(contactPoints) { } /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo); + void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo); }; /** @@ -118,7 +105,7 @@ namespace ephysics { * to use the GJK collision detection algorithm to compute the collision between * the convex shape and each of the triangles in the concave shape. */ - class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { + class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm { protected : /// Private copy-ructor ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm); @@ -126,20 +113,20 @@ namespace ephysics { ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm); /// Process the concave triangle mesh collision using the smooth mesh collision algorithm void processSmoothMeshCollision(OverlappingPair* overlappingPair, - etk::Vector contactPoints, + Vector contactPoints, NarrowPhaseCallback* narrowPhaseCallback); /// Add a triangle vertex into the set of processed triangles - void addProcessedVertex(etk::Vector> processTriangleVertices, vec3 vertex) { - processTriangleVertices.pushBack(etk::makePair(int(vertex.x() * vertex.y() * vertex.z()), vertex)); + void addProcessedVertex(Vector> processTriangleVertices, Vector3f vertex) { + processTriangleVertices.pushBack(makePair(int(vertex.x() * vertex.y() * vertex.z()), vertex)); } /// Return true if the vertex is in the set of already processed vertices - boolean hasVertexBeenProcessed( etk::Vector> processTriangleVertices, - vec3 vertex) ; + boolean hasVertexBeenProcessed( Vector> processTriangleVertices, + Vector3f vertex) ; public : /// Constructor ConcaveVsConvexAlgorithm(); /// Compute a contact info if the two bounding volume collide - virtual void testCollision( CollisionShapeInfo shape1Info, + void testCollision( CollisionShapeInfo shape1Info, CollisionShapeInfo shape2Info, NarrowPhaseCallback* narrowPhaseCallback); }; diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp index 2fe455a..8d529cd 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -30,15 +30,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t CollisionShapeType shape1Type = staticcast(type1); CollisionShapeType shape2Type = staticcast(type2); // Sphere vs Sphere algorithm - if (shape1Type == SPHERE hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shape2Type == SPHERE) { + if (shape1Type == SPHERE && shape2Type == SPHERE) { return this.sphereVsSphereAlgorithm; } else if ( ( !CollisionShape::isConvex(shape1Type) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape2Type) ) + && CollisionShape::isConvex(shape2Type) ) || ( !CollisionShape::isConvex(shape2Type) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape1Type) ) ) { + && CollisionShape::isConvex(shape1Type) ) ) { // Concave vs Convex algorithm return this.concaveVsConvexAlgorithm; - } else if (CollisionShape::isConvex(shape1Type) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape2Type)) { + } else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) { // Convex vs Convex algorithm (GJK algorithm) return this.GJKAlgorithm; } else { diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp deleted file mode 100644 index fa1bac6..0000000 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp +++ /dev/null @@ -1,37 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include -#include - -namespace ephysics { - /** - * @brief 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. - */ - class DefaultCollisionDispatch : public CollisionDispatch { - protected: - SphereVsSphereAlgorithm this.sphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm - ConcaveVsConvexAlgorithm this.concaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm - GJKAlgorithm this.GJKAlgorithm; //!< GJK Algorithm - public: - /** - * @brief Constructor - */ - DefaultCollisionDispatch(); - void init(CollisionDetection* collisionDetection) override; - NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override; - }; -} - - 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..1df92bb --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java @@ -0,0 +1,23 @@ +package org.atriaSoft.ephysics.collision.narrowphase; + + /** + * @brief 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. + */ + class DefaultCollisionDispatch extends CollisionDispatch { + protected: + SphereVsSphereAlgorithm this.sphereVsSphereAlgorithm; //!< Sphere vs Sphere collision algorithm + ConcaveVsConvexAlgorithm this.concaveVsConvexAlgorithm; //!< Concave vs Convex collision algorithm + GJKAlgorithm this.GJKAlgorithm; //!< GJK Algorithm + public: + /** + * @brief Constructor + */ + DefaultCollisionDispatch(); + void init(CollisionDetection* collisionDetection) ; + NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) ; + }; +} + + diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp index c5eeb6c..5321c72 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -21,24 +21,24 @@ EPAAlgorithm::~EPAAlgorithm() { } -int EPAAlgorithm::isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4) { +int EPAAlgorithm::isOriginInTetrahedron( Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) { // Check vertex 1 - vec3 normal1 = (p2-p1).cross(p3-p1); + Vector3f normal1 = (p2-p1).cross(p3-p1); if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) { return 4; } // Check vertex 2 - vec3 normal2 = (p4-p2).cross(p3-p2); + Vector3f normal2 = (p4-p2).cross(p3-p2); if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) { return 1; } // Check vertex 3 - vec3 normal3 = (p4-p3).cross(p1-p3); + Vector3f normal3 = (p4-p3).cross(p1-p3); if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) { return 2; } // Check vertex 4 - vec3 normal4 = (p2-p4).cross(p1-p4); + Vector3f normal4 = (p2-p4).cross(p1-p4); if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) { return 3; } @@ -48,32 +48,32 @@ int EPAAlgorithm::isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4) void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, CollisionShapeInfo shape1Info, - etk::Transform3D transform1, + Transform3D transform1, CollisionShapeInfo shape2Info, - etk::Transform3D transform2, - vec3 vector, + Transform3D transform2, + Vector3f vector, NarrowPhaseCallback* narrowPhaseCallback) { PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape.isConvex()); + assert(shape2Info.collisionShape.isConvex()); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - vec3 suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates - vec3 suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates - vec3 points[MAXSUPPORTPOINTS]; // Current points + Vector3f suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates + Vector3f suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates + Vector3f points[MAXSUPPORTPOINTS]; // Current points TrianglesStore triangleStore; // Store the triangles - etk::Set triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist + Set triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist triangleHeap.setComparator([](TriangleEPA * face1, TriangleEPA * face2) { - return (face1->getDistSquare() < face2->getDistSquare()); + return (face1.getDistSquare() < face2.getDistSquare()); }); - // etk::Transform3D a point from local space of body 2 to local + // 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) - etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; + Transform3D body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 - etk::Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); + Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation(); // Get the simplex computed previously by the GJK algorithm int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); // Compute the tolerance @@ -98,32 +98,32 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, // ruct the polytope are the three support points in those three directions // v1, v2 and v3. // Direction of the segment - vec3 d = (points[1] - points[0]).safeNormalized(); + Vector3f d = (points[1] - points[0]).safeNormalized(); // Choose the coordinate axis from the minimal absolute component of the vector d int minAxis = d.absolute().getMinAxis(); // Compute sin(60) float sin60 = float(sqrt(3.0)) * 0.5f; // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 - etk::Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5); + Quaternion rotationQuat(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5); // Compute the vector v1, v2, v3 - vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2)); - vec3 v2 = rotationQuat * v1; - vec3 v3 = rotationQuat * v2; + Vector3f v1 = d.cross(Vector3f(minAxis == 0, minAxis == 1, minAxis == 2)); + Vector3f v2 = rotationQuat * v1; + Vector3f v3 = rotationQuat * v2; // Compute the support point in the direction of v1 - suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); + suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); suppPointsB[2] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); + shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); points[2] = suppPointsA[2] - suppPointsB[2]; // Compute the support point in the direction of v2 - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); + suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); + shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); points[3] = suppPointsA[3] - suppPointsB[3]; // Compute the support point in the direction of v3 - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); + suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); + shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); points[4] = suppPointsA[4] - 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 @@ -166,9 +166,9 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3); TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); // If the ructed tetrahedron is not correct - if (!((face0 != NULL) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0 hjkhjkhjkhkj face1->getDistSquare() > 0.0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0 hjkhjkhjkhkj face3->getDistSquare() > 0.0)) { + if (!((face0 != NULL) && (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL) + && face0.getDistSquare() > 0.0 hjkhjkhjkhkj face1.getDistSquare() > 0.0 + && face2.getDistSquare() > 0.0 hjkhjkhjkhkj face3.getDistSquare() > 0.0)) { return; } // Associate the edges of neighbouring triangle faces @@ -203,17 +203,17 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, // where "n" is the normal of the triangle. Then, we use only the // tetrahedron that contains the origin. // Compute the normal of the triangle - vec3 v1 = points[1] - points[0]; - vec3 v2 = points[2] - points[0]; - vec3 n = v1.cross(v2); + Vector3f v1 = points[1] - points[0]; + Vector3f v2 = points[2] - points[0]; + Vector3f n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); + suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsB[3] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); + shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); + suppPointsA[4] = shape1.getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); suppPointsB[4] = body2Tobody1 * - shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); + shape2.getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); points[4] = suppPointsA[4] - suppPointsB[4]; TriangleEPA* face0 = null; TriangleEPA* face1 = null; @@ -245,13 +245,13 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, } // If the ructed tetrahedron is not correct if (!( face0 != null - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1 != null - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2 != null - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3 != null - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1->getDistSquare() > 0.0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3->getDistSquare() > 0.0) ) { + && face1 != null + && face2 != null + && face3 != null + && face0.getDistSquare() > 0.0 + && face1.getDistSquare() > 0.0 + && face2.getDistSquare() > 0.0 + && face3.getDistSquare() > 0.0) ) { return; } // Associate the edges of neighbouring triangle faces @@ -281,11 +281,11 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, triangle = triangleHeap[0]; triangleHeap.popFront(); Log.info("rm from heap:"); - for (sizet iii=0; iiigetDistSquare()); + for (long iii=0; iiigetIsObsolete()) { + if (!triangle.getIsObsolete()) { // If we have reached the maximum number of support points if (nbVertices == MAXSUPPORTPOINTS) { assert(false); @@ -293,28 +293,28 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, } // 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 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData); + suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData); + suppPointsB[nbVertices] = body2Tobody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-triangle.getClosestPoint()), shape2CachedCollisionData); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; int indexNewVertex = nbVertices; nbVertices++; // Update the upper bound of the penetration depth - float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); - Log.info(" point=" << points[indexNewVertex]); - Log.info("close point=" << triangle->getClosestPoint()); - Log.info(" ==>" << wDotv); + float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint()); + Log.info(" point=" + points[indexNewVertex]); + Log.info("close point=" + triangle.getClosestPoint()); + Log.info(" ==>" + wDotv); if (wDotv < 0.0) { - Log.error("depth penetration error " << wDotv); + Log.error("depth penetration error " + wDotv); continue; } - EPHYASSERT(wDotv >= 0.0, "depth penetration error " << wDotv); - float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); + EPHYASSERT(wDotv >= 0.0, "depth penetration error " + wDotv); + float wDotVSquare = wDotv * wDotv / triangle.getDistSquare(); if (wDotVSquare < upperBoundSquarePenDepth) { upperBoundSquarePenDepth = wDotVSquare; } // Compute the error - float error = wDotv - triangle->getDistSquare(); - if (error <= etk::max(tolerance, RELERRORSQUARE * wDotv) || + float error = wDotv - triangle.getDistSquare(); + if (error <= max(tolerance, RELERRORSQUARE * wDotv) || points[indexNewVertex] == points[(*triangle)[0]] || points[indexNewVertex] == points[(*triangle)[1]] || points[indexNewVertex] == points[(*triangle)[2]]) { @@ -323,8 +323,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, // 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. - sizet i = triangleStore.getNbTriangles(); - if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { + long i = triangleStore.getNbTriangles(); + if (!triangle.computeSilhouette(points, indexNewVertex, triangleStore)) { break; } // Add all the new triangle faces computed with the silhouette algorithm @@ -336,12 +336,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, } } } while( triangleHeap.size() > 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); + && triangleHeap[0].getDistSquare() <= upperBoundSquarePenDepth); // Compute the contact info - vector = transform1.getOrientation() * triangle->getClosestPoint(); - vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); - vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); - vec3 normal = vector.safeNormalized(); + vector = transform1.getOrientation() * triangle.getClosestPoint(); + Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA); + Vector3f pBLocal = body2Tobody1.getInverse() * triangle.computeClosestPointOfObject(suppPointsB); + Vector3f normal = vector.safeNormalized(); float penetrationDepth = vector.length(); EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0"); if (normal.length2() < FLTEPSILON) { @@ -349,5 +349,5 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, } // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); } diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java similarity index 65% rename from src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java index d60186a..e2743ab 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java @@ -1,23 +1,5 @@ -/** @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) - */ -#pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include +package org.atriaSoft.ephysics.collision.narrowphase.EPA; -namespace ephysics { /// Maximum number of support points of the polytope int MAXSUPPORTPOINTS = 100; /// Maximum number of facets of the polytope @@ -44,31 +26,29 @@ namespace ephysics { EPAAlgorithm operator=( EPAAlgorithm algorithm); /// Add a triangle face in the candidate triangle heap void addFaceCandidate(TriangleEPA* triangle, - etk::Set heap, + Set heap, float 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 - if ( triangle->isClosestPointInternalToTriangle() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangle->getDistSquare() <= upperBoundSquarePenDepth) { + if ( triangle.isClosestPointInternalToTriangle() + && triangle.getDistSquare() <= upperBoundSquarePenDepth) { // Add the triangle face to the list of candidates heap.add(triangle); Log.info("add in heap:"); - for (sizet iii=0; iiigetDistSquare()); + for (long iii=0; iii= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); + assert(index >= 0 && index < 3); } EdgeEPA::EdgeEPA( EdgeEPA obj): - this.ownerTriangle(obj.this.ownerTriangle), - this.index(obj.this.index) { + this.ownerTriangle(obj.ownerTriangle), + this.index(obj.index) { } -EdgeEPA::EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj): +EdgeEPA::EdgeEPA(EdgeEPA&& obj): this.ownerTriangle(null) { - etk::swap(this.ownerTriangle, obj.this.ownerTriangle); - etk::swap(this.index, obj.this.index); + swap(this.ownerTriangle, obj.ownerTriangle); + swap(this.index, obj.index); } int EdgeEPA::getSourceVertexIndex() { @@ -44,12 +44,12 @@ int EdgeEPA::getTargetVertexIndex() { return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)]; } -boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex, +boolean EdgeEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex, TrianglesStore triangleStore) { // If the edge has not already been visited - if (!this.ownerTriangle->getIsObsolete()) { + if (!this.ownerTriangle.getIsObsolete()) { // If the triangle of this edge is not visible from the given point - if (!this.ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { + if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) { TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); @@ -61,12 +61,12 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex, return false; } else { // The current triangle is visible and therefore obsolete - this.ownerTriangle->setIsObsolete(true); + this.ownerTriangle.setIsObsolete(true); int backup = triangleStore.getNbTriangles(); - if(!this.ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->this.index)).computeSilhouette(vertices, + if(!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { - this.ownerTriangle->setIsObsolete(false); + this.ownerTriangle.setIsObsolete(false); TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex()); @@ -76,10 +76,10 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex, return true; } return false; - } else if (!this.ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->this.index)).computeSilhouette(vertices, + } else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) { - this.ownerTriangle->setIsObsolete(false); + this.ownerTriangle.setIsObsolete(false); triangleStore.resize(backup); TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java similarity index 59% rename from src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java index 9580aaf..592a85e 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java @@ -1,17 +1,5 @@ -/** @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) - */ -#pragma once -#include +package org.atriaSoft.ephysics.collision.narrowphase.EPA; -namespace ephysics { -class TriangleEPA; -class TrianglesStore; /** * @brief Class EdgeEPA * This class represents an edge of the current polytope in the EPA algorithm. @@ -31,7 +19,7 @@ class EdgeEPA { /// Copy-ructor EdgeEPA( EdgeEPA obj); /// Move-ructor - EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj); + EdgeEPA(EdgeEPA obj); /// Return the pointer to the owner triangle TriangleEPA* getOwnerTriangle() { return this.ownerTriangle; @@ -45,17 +33,17 @@ class EdgeEPA { /// Return the index of the target vertex of the edge int getTargetVertexIndex() ; /// Execute the recursive silhouette algorithm from this edge - boolean computeSilhouette( vec3* vertices, int index, TrianglesStore triangleStore); + boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore); /// Assignment operator EdgeEPA operator=( EdgeEPA obj) { - this.ownerTriangle = obj.this.ownerTriangle; - this.index = obj.this.index; + this.ownerTriangle = obj.ownerTriangle; + this.index = obj.index; return *this; } /// Move operator - EdgeEPA operator=(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj) { - etk::swap(this.ownerTriangle, obj.this.ownerTriangle); - etk::swap(this.index, obj.this.index); + EdgeEPA operator=(EdgeEPA obj) { + swap(this.ownerTriangle, obj.ownerTriangle); + swap(this.index, obj.index); return *this; } }; diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp index 44ce827..fa260d9 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp @@ -34,10 +34,10 @@ TriangleEPA::~TriangleEPA() { } -boolean TriangleEPA::computeClosestPoint( vec3* vertices) { - vec3 p0 = vertices[this.indicesVertices[0]]; - vec3 v1 = vertices[this.indicesVertices[1]] - p0; - vec3 v2 = vertices[this.indicesVertices[2]] - p0; +boolean TriangleEPA::computeClosestPoint( Vector3f* vertices) { + Vector3f p0 = vertices[this.indicesVertices[0]]; + Vector3f v1 = vertices[this.indicesVertices[1]] - p0; + Vector3f v2 = vertices[this.indicesVertices[2]] - p0; float v1Dotv1 = v1.dot(v1); float v1Dotv2 = v1.dot(v2); float v2Dotv2 = v2.dot(v2); @@ -61,9 +61,9 @@ boolean TriangleEPA::computeClosestPoint( vec3* vertices) { boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) { if ( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) { - edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1; - edge1.getOwnerTriangle()->this.adjacentEdges[edge1.getIndex()] = edge0; + && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) { + edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1; + edge1.getOwnerTriangle().this.adjacentEdges[edge1.getIndex()] = edge0; return true; } return false; @@ -71,20 +71,20 @@ boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) { void ephysics::halfLink( EdgeEPA edge0, EdgeEPA edge1) { assert( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); - edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1; + && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); + edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1; } -boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex, +boolean TriangleEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex, TrianglesStore triangleStore) { 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 - boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj - this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj + 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; @@ -92,7 +92,7 @@ boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex, for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) { TriangleEPA* triangle = triangleStore[i]; - halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); + halfLink(triangle.getAdjacentEdge(1), EdgeEPA(triangle, 1)); if (!link(EdgeEPA(triangle, 0), EdgeEPA(triangleStore[j], 2))) { return false; } diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java similarity index 56% rename from src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java index 9b6bd1b..159cc19 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java @@ -1,16 +1,5 @@ - /** @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) - */ -#pragma once -#include -#include -#include -namespace ephysics { +package org.atriaSoft.ephysics.collision.narrowphase.EPA; + boolean link( EdgeEPA edge0, EdgeEPA edge1); void halfLink( EdgeEPA edge0, EdgeEPA edge1); /** @@ -23,40 +12,40 @@ namespace ephysics { EdgeEPA this.adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles) boolean this.isObsolete; //!< True if the triangle face is visible from the new support point float this.determinant; //!< Determinant - vec3 this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle + Vector3f this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle float this.lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2 float this.lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2 float this.distSquare; //!< Square distance of the point closest point v to the origin public: /// Private copy-ructor TriangleEPA( TriangleEPA triangle) { - this.indicesVertices[0] = triangle.this.indicesVertices[0]; - this.indicesVertices[1] = triangle.this.indicesVertices[1]; - this.indicesVertices[2] = triangle.this.indicesVertices[2]; - this.adjacentEdges[0] = triangle.this.adjacentEdges[0]; - this.adjacentEdges[1] = triangle.this.adjacentEdges[1]; - this.adjacentEdges[2] = triangle.this.adjacentEdges[2]; - this.isObsolete = triangle.this.isObsolete; - this.determinant = triangle.this.determinant; - this.closestPoint = triangle.this.closestPoint; - this.lambda1 = triangle.this.lambda1; - this.lambda2 = triangle.this.lambda2; - this.distSquare = triangle.this.distSquare; + 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; } /// Private assignment operator TriangleEPA operator=( TriangleEPA triangle) { - this.indicesVertices[0] = triangle.this.indicesVertices[0]; - this.indicesVertices[1] = triangle.this.indicesVertices[1]; - this.indicesVertices[2] = triangle.this.indicesVertices[2]; - this.adjacentEdges[0] = triangle.this.adjacentEdges[0]; - this.adjacentEdges[1] = triangle.this.adjacentEdges[1]; - this.adjacentEdges[2] = triangle.this.adjacentEdges[2]; - this.isObsolete = triangle.this.isObsolete; - this.determinant = triangle.this.determinant; - this.closestPoint = triangle.this.closestPoint; - this.lambda1 = triangle.this.lambda1; - this.lambda2 = triangle.this.lambda2; - this.distSquare = triangle.this.distSquare; + 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; } /// Constructor @@ -65,16 +54,14 @@ namespace ephysics { TriangleEPA(int v1, int v2, int v3); /// Constructor void set(int v1, int v2, int v3); - /// Destructor - ~TriangleEPA(); /// Return an adjacent edge of the triangle EdgeEPA getAdjacentEdge(int index) { - assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); + assert(index >= 0 && index < 3); return this.adjacentEdges[index]; } /// Set an adjacent edge of the triangle void setAdjacentEdge(int index, EdgeEPA edge) { - assert(index >=0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); + assert(index >=0 && index < 3); this.adjacentEdges[index] = edge; } /// Return the square distance of the closest point to origin @@ -90,23 +77,23 @@ namespace ephysics { return this.isObsolete; } /// Return the point closest to the origin - vec3 getClosestPoint() { + Vector3f getClosestPoint() { return this.closestPoint; } // Return true if the closest point on affine hull is inside the triangle boolean isClosestPointInternalToTriangle() { - return (this.lambda1 >= 0.0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant); + return (this.lambda1 >= 0.0 && this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant); } /// Return true if the triangle is visible from a given vertex - boolean isVisibleFromVertex( vec3* vertices, int index) { - vec3 closestToVert = vertices[index] - this.closestPoint; + boolean isVisibleFromVertex( Vector3f* vertices, int index) { + Vector3f closestToVert = vertices[index] - this.closestPoint; return (this.closestPoint.dot(closestToVert) > 0.0); } /// Compute the point v closest to the origin of this triangle - boolean computeClosestPoint( vec3* vertices); + boolean computeClosestPoint( Vector3f* vertices); /// Compute the point of an object closest to the origin - vec3 computeClosestPointOfObject( vec3* supportPointsOfObject) { - vec3 p0 = supportPointsOfObject[this.indicesVertices[0]]; + Vector3f computeClosestPointOfObject( Vector3f* supportPointsOfObject) { + Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]]; return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) + this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - p0)); } @@ -121,10 +108,10 @@ namespace ephysics { /// 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 considered as being a candidate face in the future. - boolean computeSilhouette( vec3* vertices, int index, TrianglesStore triangleStore); + boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore); /// Access operator int operator[](int pos) { - assert(pos >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj pos <3); + 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 diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java similarity index 52% rename from src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java index 49c63d2..7a79b71 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java @@ -1,28 +1,12 @@ -/** @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) - */ -#pragma once -#include -#include -#include +package org.atriaSoft.ephysics.collision.narrowphase.EPA; -namespace ephysics { int MAXTRIANGLES = 200; // Maximum number of triangles /** * @brief This class stores several triangles of the polytope in the EPA algorithm. */ class TrianglesStore { private: - etk::Array this.triangles; //!< Triangles - /// Private copy-ructor - TrianglesStore( TrianglesStore triangleStore) = delete; - /// Private assignment operator - TrianglesStore operator=( TrianglesStore triangleStore) = delete; + Array this.triangles; //!< Triangles public: /// Constructor TrianglesStore() = default; @@ -31,13 +15,13 @@ namespace ephysics { this.triangles.clear(); } /// Return the number of triangles - sizet getNbTriangles() { + long getNbTriangles() { return this.triangles.size(); } /// Set the number of triangles void resize(int backup) { if (backup > this.triangles.size()) { - Log.error("RESIZE BIGGER : " << backup << " > " << this.triangles.size()); + Log.error("RESIZE BIGGER : " + backup + " > " + this.triangles.size()); } this.triangles.resize(backup); } @@ -46,14 +30,14 @@ namespace ephysics { return this.triangles.back(); } /// Create a new triangle - TriangleEPA* newTriangle( vec3* vertices, int v0, int v1, int v2) { + TriangleEPA* newTriangle( Vector3f* vertices, int v0, int v1, int v2) { // If we have not reached the maximum number of triangles if (this.triangles.size() < MAXTRIANGLES) { TriangleEPA tmp(v0, v1, v2); if (!tmp.computeClosestPoint(vertices)) { return null; } - this.triangles.pushBack(etk::move(tmp)); + this.triangles.pushBack(move(tmp)); return this.triangles.back(); } // We are at the limit (internal) diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp index c920b37..241461f 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,50 +26,50 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, CollisionShapeInfo shape2Info, NarrowPhaseCallback* narrowPhaseCallback) { PROFILE("GJKAlgorithm::testCollision()"); - vec3 suppA; // Support point of object A - vec3 suppB; // Support point of object B - vec3 w; // Support point of Minkowski difference A-B - vec3 pA; // Closest point of object A - vec3 pB; // Closest point of object B + Vector3f suppA; // Support point of object A + Vector3f suppB; // Support point of object B + Vector3f w; // Support point of Minkowski difference A-B + Vector3f pA; // Closest point of object A + Vector3f pB; // Closest point of object B float vDotw; float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape.isConvex()); + assert(shape2Info.collisionShape.isConvex()); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; // Get the local-space to world-space transforms - etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; - etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; - // etk::Transform3D a point from local space of body 2 to local + Transform3D transform1 = shape1Info.shapeToWorldTransform; + Transform3D transform2 = shape2Info.shapeToWorldTransform; + // 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) - etk::Transform3D body2Tobody1 = transform1.getInverse() * transform2; + Transform3D body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 - etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Initialize the margin (sum of margins of both objects) - float margin = shape1->getMargin() + shape2->getMargin(); + float margin = shape1.getMargin() + shape2.getMargin(); float marginSquare = margin * margin; assert(margin > 0.0); // Create a simplex set Simplex simplex; // Get the previous point V (last cached separating axis) - vec3 v = this.currentOverlappingPair->getCachedSeparatingAxis(); + Vector3f v = this.currentOverlappingPair.getCachedSeparatingAxis(); // Initialize the upper bound for the square distance float distSquare = FLTMAX; do { // Compute the support points for original objects (without margins) A and B - suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); - suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); + suppA = shape1.getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); + suppB = body2Tobody1 * shape2.getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; vDotw = v.dot(w); // If the enlarge objects (with margins) do not intersect - if (vDotw > 0.0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vDotw * vDotw > distSquare * marginSquare) { + if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - this.currentOverlappingPair->setCachedSeparatingAxis(v); + this.currentOverlappingPair.setCachedSeparatingAxis(v); // No intersection, we return return; } @@ -81,17 +81,17 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + pA = (pA - (shape1.getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v); // Compute the contact info - vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); + Vector3f normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) if (penetrationDepth <= 0.0) return; // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); // There is an intersection, therefore we return return; } @@ -105,10 +105,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + pA = (pA - (shape1.getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v); // Compute the contact info - vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); + Vector3f normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -117,7 +117,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); // There is an intersection, therefore we return return; } @@ -130,10 +130,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + pA = (pA - (shape1.getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v); // Compute the contact info - vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); + Vector3f normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -142,7 +142,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); // There is an intersection, therefore we return return; } @@ -161,10 +161,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // object with the margins float dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + pA = (pA - (shape1.getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v); // Compute the contact info - vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); + Vector3f normal = transform1.getOrientation() * (-v.safeNormalized()); float penetrationDepth = margin - dist; // Reject the contact if the penetration depth is negative (due too numerical errors) @@ -173,11 +173,11 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, // Create the contact info object ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); // There is an intersection, therefore we return return; } - } while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * + } while(!simplex.isFull() && distSquare > FLTEPSILON * 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 @@ -188,35 +188,35 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info, } void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info, - etk::Transform3D transform1, + Transform3D transform1, CollisionShapeInfo shape2Info, - etk::Transform3D transform2, + Transform3D transform2, NarrowPhaseCallback* narrowPhaseCallback, - vec3 v) { + Vector3f v) { PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); Simplex simplex; - vec3 suppA; - vec3 suppB; - vec3 w; + Vector3f suppA; + Vector3f suppB; + Vector3f w; float vDotw; float distSquare = FLTMAX; float prevDistSquare; - assert(shape1Info.collisionShape->isConvex()); - assert(shape2Info.collisionShape->isConvex()); + assert(shape1Info.collisionShape.isConvex()); + assert(shape2Info.collisionShape.isConvex()); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; - // etk::Transform3D a point from local space of body 2 to local space + // Transform3D a point from local space of body 2 to local space // of body 1 (the GJK algorithm is done in local space of body 1) - etk::Transform3D body2ToBody1 = transform1.getInverse() * transform2; + Transform3D body2ToBody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local space of body 1 into local space of body 2 - etk::Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * + Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); do { // Compute the support points for the enlarged object A and B - suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); - suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); + suppA = shape1.getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); + suppB = body2ToBody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; vDotw = v.dot(w); @@ -239,7 +239,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) { return; } - } while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * + } while(!simplex.isFull() && distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint()); // Give the simplex computed with GJK algorithm to the EPA algorithm // which will compute the correct penetration depth and contact points @@ -249,24 +249,24 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo v, narrowPhaseCallback); } -boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { - vec3 suppA; // Support point of object A - vec3 w; // Support point of Minkowski difference A-B +boolean GJKAlgorithm::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { + Vector3f suppA; // Support point of object A + Vector3f w; // Support point of Minkowski difference A-B float prevDistSquare; - assert(proxyShape->getCollisionShape()->isConvex()); - ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); + assert(proxyShape.getCollisionShape().isConvex()); + ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape()); + void** shapeCachedCollisionData = proxyShape.getCachedCollisionData(); // Support point of object B (object B is a single point) - vec3 suppB(localPoint); + Vector3f suppB(localPoint); // Create a simplex set Simplex simplex; // Initial supporting direction - vec3 v(1, 1, 1); + Vector3f v(1, 1, 1); // Initialize the upper bound for the square distance float distSquare = FLTMAX; do { // Compute the support points for original objects (without margins) A and B - suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); + suppA = shape.getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; // Add the new support point to the simplex @@ -288,38 +288,38 @@ boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape) return false; } } while( !simplex.isFull() - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint()); + && distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint()); // The point is inside the collision shape return true; } boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) { - assert(proxyShape->getCollisionShape()->isConvex()); - ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape()); - void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); - vec3 suppA; // Current lower bound point on the ray (starting at ray's origin) - vec3 suppB; // Support point on the collision shape + assert(proxyShape.getCollisionShape().isConvex()); + ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape()); + void** 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 float machineEpsilonSquare = FLTEPSILON * FLTEPSILON; float epsilon = float(0.0001); // Convert the ray origin and direction into the local-space of the collision shape - vec3 rayDirection = ray.point2 - ray.point1; + Vector3f rayDirection = ray.point2 - ray.point1; // If the points of the segment are two close, return no hit if (rayDirection.length2() < machineEpsilonSquare) return false; - vec3 w; + Vector3f w; // Create a simplex set Simplex simplex; - vec3 n(0.0f, float(0.0), float(0.0)); + Vector3f n(0.0f, float(0.0), float(0.0)); float lambda = 0.0f; suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) - suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); - vec3 v = suppA - suppB; + suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); + Vector3f v = suppA - suppB; float vDotW, vDotR; float distSquare = v.length2(); int nbIterations = 0; // GJK Algorithm loop - while (distSquare > epsilon hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nbIterations < MAXITERATIONSGJKRAYCAST) { + while (distSquare > epsilon && nbIterations < MAXITERATIONSGJKRAYCAST) { // Compute the support points - suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); + suppB = shape.getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); w = suppA - suppB; vDotW = v.dot(w); if (vDotW > float(0)) { @@ -353,18 +353,18 @@ boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo rayc return false; } // Compute the closet points of both objects (without the margins) - vec3 pointA; - vec3 pointB; + Vector3f pointA; + Vector3f pointB; 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.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 = vec3(float(0), float(0), float(0)); + raycastInfo.worldNormal = Vector3f(float(0), float(0), float(0)); } return true; } diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java similarity index 75% rename from src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java index 9754f25..a479720 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java @@ -1,19 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.narrowphase.GJK; -#include -#include -#include -#include - -namespace ephysics { float RELERROR = float(1.0e-3); float RELERRORSQUARE = RELERROR * RELERROR; int MAXITERATIONSGJKRAYCAST = 32; @@ -32,7 +18,7 @@ namespace ephysics { * Polytope Algorithm) to compute the correct penetration depth between the * enlarged objects. */ - class GJKAlgorithm : public NarrowPhaseAlgorithm { + class GJKAlgorithm extends NarrowPhaseAlgorithm { private : EPAAlgorithm this.algoEPA; //!< EPA Algorithm /// Private copy-ructor @@ -45,18 +31,16 @@ namespace ephysics { /// 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. void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info, - etk::Transform3D transform1, + Transform3D transform1, CollisionShapeInfo shape2Info, - etk::Transform3D transform2, + Transform3D transform2, NarrowPhaseCallback* narrowPhaseCallback, - vec3 v); + Vector3f v); public : /// Constructor GJKAlgorithm(); - /// Destructor - ~GJKAlgorithm(); /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection) { + void init(CollisionDetection* collisionDetection) { NarrowPhaseAlgorithm::init(collisionDetection); this.algoEPA.init(); }; @@ -69,11 +53,11 @@ namespace ephysics { /// algorithm on the enlarged object to obtain a simplex polytope that contains the /// origin, they we give that simplex polytope to the EPA algorithm which will compute /// the correct penetration depth and contact points between the enlarged objects. - virtual void testCollision( CollisionShapeInfo shape1Info, + void testCollision( CollisionShapeInfo shape1Info, CollisionShapeInfo shape2Info, NarrowPhaseCallback* narrowPhaseCallback); /// Use the GJK Algorithm to find if a point is inside a convex collision shape - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape); + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape); /// 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". diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp index 7218e74..e8660d5 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp @@ -18,7 +18,7 @@ Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) { /// 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 -void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) { +void Simplex::addPoint( Vector3f point, Vector3f suppPointA, Vector3f suppPointB) { assert(!isFull()); mLastFound = 0; @@ -28,7 +28,7 @@ void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) { // the current simplex while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { mLastFound++; - mLastFoundBit <<= 1; + mLastFoundBit += 1; } assert(mLastFound < 4); @@ -50,14 +50,14 @@ void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) { } // Return true if the point is in the simplex -boolean Simplex::isPointInSimplex( vec3 point) { +boolean Simplex::isPointInSimplex( Vector3f point) { int i; Bits bit; // For each four possible points in the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + for (i=0, bit = 0x1; i<4; i++, bit += 1) { // Check if the current point is in the simplex - if (overlap(mAllBits, bit) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point == mPoints[i]) return true; + if (overlap(mAllBits, bit) && point == mPoints[i]) return true; } return false; @@ -69,7 +69,7 @@ void Simplex::updateCache() { Bits bit; // For each of the four possible points of the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { + for (i=0, bit = 0x1; i<4; i++, bit += 1) { // If the current points is in the simplex if (overlap(mBitsCurrentSimplex, bit)) { @@ -86,22 +86,22 @@ void Simplex::updateCache() { } // Return the points of the simplex -int Simplex::getSimplex(vec3* suppPointsA, vec3* suppPointsB, - vec3* points) { +int Simplex::getSimplex(Vector3f* suppPointsA, Vector3f* suppPointsB, + Vector3f* points) { int nbVertices = 0; int i; Bits bit; // For each four point in the possible simplex - for (i=0, bit=0x1; i<4; i++, bit <<=1) { + for (i=0, bit=0x1; i<4; i++, bit +=1) { // If the current point is in the simplex if (overlap(mBitsCurrentSimplex, bit)) { // Store the points - suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; - suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; - points[nbVertices] = this->mPoints[nbVertices]; + suppPointsA[nbVertices] = this.mSuppPointsA[nbVertices]; + suppPointsB[nbVertices] = this.mSuppPointsB[nbVertices]; + points[nbVertices] = this.mPoints[nbVertices]; nbVertices++; } @@ -121,7 +121,7 @@ void Simplex::computeDeterminants() { Bits bitI; // For each possible four points in the simplex set - for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { + for (i=0, bitI = 0x1; i<4; i++, bitI += 1) { // If the current point is in the simplex if (overlap(mBitsCurrentSimplex, bitI)) { @@ -134,7 +134,7 @@ void Simplex::computeDeterminants() { int j; Bits bitJ; - for (j=0, bitJ = 0x1; j 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) - */ -#pragma once - -// Libraries -#include -#include - -/// ReactPhysics3D namespace -namespace ephysics { - -// Type definitions -typedef int Bits; +package org.atriaSoft.ephysics.collision.narrowphase.GJK; // Class Simplex /** @@ -34,7 +16,7 @@ class Simplex { // -------------------- Attributes -------------------- // /// Current points - vec3 mPoints[4]; + Vector3f mPoints[4]; /// pointsLengthSquare[i] = (points[i].length)^2 float mPointsLengthSquare[4]; @@ -43,13 +25,13 @@ class Simplex { float mMaxLengthSquare; /// Support points of object A in local coordinates - vec3 mSuppPointsA[4]; + Vector3f mSuppPointsA[4]; /// Support points of object B in local coordinates - vec3 mSuppPointsB[4]; + Vector3f mSuppPointsB[4]; /// diff[i][j] contains points[i] - points[j] - vec3 mDiffLength[4][4]; + Vector3f mDiffLength[4][4]; /// Cached determinant values float mDet[16][4]; @@ -59,16 +41,16 @@ class Simplex { /// 4 bits that identify the current points of the simplex /// For instance, 0101 means that points[1] and points[3] are in the simplex - Bits mBitsCurrentSimplex; + int mBitsCurrentSimplex; /// Number between 1 and 4 that identify the last found support point - Bits mLastFound; + int mLastFound; - /// Position of the last found support point (lastFoundBit = 0x1 << lastFound) - Bits mLastFoundBit; + /// Position of the last found support point (lastFoundBit = 0x1 + lastFound) + int mLastFoundBit; /// allBits = bitsCurrentSimplex | lastFoundBit; - Bits mAllBits; + int mAllBits; // -------------------- Methods -------------------- // @@ -97,7 +79,7 @@ class Simplex { void computeDeterminants(); /// Return the closest point "v" in the convex hull of a subset of points - vec3 computeClosestPointForSubset(Bits subset); + Vector3f computeClosestPointForSubset(Bits subset); public: @@ -113,29 +95,29 @@ class Simplex { boolean isEmpty() ; /// Return the points of the simplex - int getSimplex(vec3* mSuppPointsA, vec3* mSuppPointsB, - vec3* mPoints) ; + int getSimplex(Vector3f* mSuppPointsA, Vector3f* mSuppPointsB, + Vector3f* mPoints) ; /// Return the maximum squared length of a point float getMaxLengthSquareOfAPoint() ; /// Add a new support point of (A-B) into the simplex. - void addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB); + void addPoint( Vector3f point, Vector3f suppPointA, Vector3f suppPointB); /// Return true if the point is in the simplex - boolean isPointInSimplex( vec3 point) ; + boolean isPointInSimplex( Vector3f point) ; /// Return true if the set is affinely dependent boolean isAffinelyDependent() ; /// Backup the closest point - void backupClosestPointInSimplex(vec3 point); + void backupClosestPointInSimplex(Vector3f point); /// Compute the closest points "pA" and "pB" of object A and B. - void computeClosestPointsOfAandB(vec3 pA, vec3 pB) ; + void computeClosestPointsOfAandB(Vector3f pA, Vector3f pB) ; /// Compute the closest point to the origin of the current simplex. - boolean computeClosestPoint(vec3 v); + boolean computeClosestPoint(Vector3f v); }; // Return true if some bits of "a" overlap with bits of "b" diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java similarity index 52% rename from src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp rename to src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java index 5abb43f..227957b 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java @@ -1,21 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.narrowphase; -#include -#include -#include -#include - -namespace ephysics { - - class CollisionDetection; /** * @brief It is the base class for a narrow-phase collision @@ -23,9 +7,8 @@ namespace ephysics { */ class NarrowPhaseCallback { public: - virtual ~NarrowPhaseCallback() = default; /// Called by a narrow-phase collision algorithm when a new contact has been found - virtual void notifyContact(OverlappingPair* overlappingPair, + void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) = 0; }; @@ -39,21 +22,15 @@ namespace ephysics { protected : CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision - /// Private copy-ructor - NarrowPhaseAlgorithm( NarrowPhaseAlgorithm algorithm) = delete; - /// Private assignment operator - NarrowPhaseAlgorithm operator=( NarrowPhaseAlgorithm algorithm) = delete; public : /// Constructor NarrowPhaseAlgorithm(); - /// Destructor - virtual ~NarrowPhaseAlgorithm() = default; /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection); + void init(CollisionDetection* collisionDetection); /// Set the current overlapping pair of bodies void setCurrentOverlappingPair(OverlappingPair* overlappingPair); /// Compute a contact info if the two bounding volume collide - virtual void testCollision( CollisionShapeInfo shape1Info, + void testCollision( CollisionShapeInfo shape1Info, CollisionShapeInfo shape2Info, NarrowPhaseCallback* narrowPhaseCallback) = 0; }; diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 9a45810..417e3d0 100644 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -21,20 +21,20 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape); ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape); // Get the local-space to world-space transforms - etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; - etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; + Transform3D transform1 = shape1Info.shapeToWorldTransform; + Transform3D transform2 = shape2Info.shapeToWorldTransform; // Compute the distance between the centers - vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); + Vector3f vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); // Compute the sum of the radius - float sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius(); + float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius(); // If the sphere collision shapes intersect if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { - vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); - vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); - vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized(); - vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized(); - float penetrationDepth = sumRadius - etk::sqrt(squaredDistanceBetweenCenters); + Vector3f centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); + Vector3f centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); + Vector3f intersectionOnBody1 = sphereShape1.getRadius() * centerSphere2InBody1LocalSpace.safeNormalized(); + Vector3f intersectionOnBody2 = sphereShape2.getRadius() * centerSphere1InBody2LocalSpace.safeNormalized(); + float penetrationDepth = sumRadius - sqrt(squaredDistanceBetweenCenters); // Create the contact info object ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape, @@ -46,6 +46,6 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI intersectionOnBody1, intersectionOnBody2); // Notify about the new contact - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo); } } diff --git a/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp b/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp deleted file mode 100644 index 1790f54..0000000 --- a/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp +++ /dev/null @@ -1,42 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include - -namespace ephysics { - /** - * @brief It is used to compute the narrow-phase collision detection - * between two sphere collision shapes. - */ - class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm { - protected : - SphereVsSphereAlgorithm( SphereVsSphereAlgorithm) = delete; - SphereVsSphereAlgorithm operator=( SphereVsSphereAlgorithm) = delete; - public : - /** - * @brief Constructor - */ - SphereVsSphereAlgorithm(); - /** - * @brief Destructor - */ - virtual ~SphereVsSphereAlgorithm() = default; - /** - * @brief Compute a contact info if the two bounding volume collide - */ - virtual void testCollision( CollisionShapeInfo shape1Info, - CollisionShapeInfo shape2Info, - NarrowPhaseCallback* narrowPhaseCallback); - }; -} - - 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..a8b3aa0 --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java @@ -0,0 +1,29 @@ +package org.atriaSoft.ephysics.collision.narrowphase; + + /** + * @brief It is used to compute the narrow-phase collision detection + * between two sphere collision shapes. + */ + class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm { + protected : + SphereVsSphereAlgorithm( SphereVsSphereAlgorithm) = delete; + SphereVsSphereAlgorithm operator=( SphereVsSphereAlgorithm) = delete; + public : + /** + * @brief Constructor + */ + SphereVsSphereAlgorithm(); + /** + * @brief Destructor + */ + ~SphereVsSphereAlgorithm() = default; + /** + * @brief Compute a contact info if the two bounding volume collide + */ + void testCollision( CollisionShapeInfo shape1Info, + CollisionShapeInfo shape2Info, + NarrowPhaseCallback* narrowPhaseCallback); + }; +} + + diff --git a/src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp b/src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp index 858a9d2..b30f517 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp @@ -20,50 +20,50 @@ AABB::AABB(): } -AABB::AABB( vec3 minCoordinates, vec3 maxCoordinates): +AABB::AABB( Vector3f minCoordinates, Vector3f maxCoordinates): this.minCoordinates(minCoordinates), this.maxCoordinates(maxCoordinates) { } AABB::AABB( AABB aabb): - this.minCoordinates(aabb.this.minCoordinates), - this.maxCoordinates(aabb.this.maxCoordinates) { + this.minCoordinates(aabb.minCoordinates), + this.maxCoordinates(aabb.maxCoordinates) { } void AABB::mergeWithAABB( AABB aabb) { - this.minCoordinates.setX(etk::min(this.minCoordinates.x(), aabb.this.minCoordinates.x())); - this.minCoordinates.setY(etk::min(this.minCoordinates.y(), aabb.this.minCoordinates.y())); - this.minCoordinates.setZ(etk::min(this.minCoordinates.z(), aabb.this.minCoordinates.z())); - this.maxCoordinates.setX(etk::max(this.maxCoordinates.x(), aabb.this.maxCoordinates.x())); - this.maxCoordinates.setY(etk::max(this.maxCoordinates.y(), aabb.this.maxCoordinates.y())); - this.maxCoordinates.setZ(etk::max(this.maxCoordinates.z(), aabb.this.maxCoordinates.z())); + this.minCoordinates.setX(min(this.minCoordinates.x(), aabb.minCoordinates.x())); + this.minCoordinates.setY(min(this.minCoordinates.y(), aabb.minCoordinates.y())); + this.minCoordinates.setZ(min(this.minCoordinates.z(), aabb.minCoordinates.z())); + this.maxCoordinates.setX(max(this.maxCoordinates.x(), aabb.maxCoordinates.x())); + this.maxCoordinates.setY(max(this.maxCoordinates.y(), aabb.maxCoordinates.y())); + this.maxCoordinates.setZ(max(this.maxCoordinates.z(), aabb.maxCoordinates.z())); } void AABB::mergeTwoAABBs( AABB aabb1, AABB aabb2) { - this.minCoordinates.setX(etk::min(aabb1.this.minCoordinates.x(), aabb2.this.minCoordinates.x())); - this.minCoordinates.setY(etk::min(aabb1.this.minCoordinates.y(), aabb2.this.minCoordinates.y())); - this.minCoordinates.setZ(etk::min(aabb1.this.minCoordinates.z(), aabb2.this.minCoordinates.z())); - this.maxCoordinates.setX(etk::max(aabb1.this.maxCoordinates.x(), aabb2.this.maxCoordinates.x())); - this.maxCoordinates.setY(etk::max(aabb1.this.maxCoordinates.y(), aabb2.this.maxCoordinates.y())); - this.maxCoordinates.setZ(etk::max(aabb1.this.maxCoordinates.z(), aabb2.this.maxCoordinates.z())); + this.minCoordinates.setX(min(aabb1.minCoordinates.x(), aabb2.minCoordinates.x())); + this.minCoordinates.setY(min(aabb1.minCoordinates.y(), aabb2.minCoordinates.y())); + this.minCoordinates.setZ(min(aabb1.minCoordinates.z(), aabb2.minCoordinates.z())); + this.maxCoordinates.setX(max(aabb1.maxCoordinates.x(), aabb2.maxCoordinates.x())); + this.maxCoordinates.setY(max(aabb1.maxCoordinates.y(), aabb2.maxCoordinates.y())); + this.maxCoordinates.setZ(max(aabb1.maxCoordinates.z(), aabb2.maxCoordinates.z())); } boolean AABB::contains( AABB aabb) { boolean isInside = true; - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.x() <= aabb.this.minCoordinates.x(); - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.y() <= aabb.this.minCoordinates.y(); - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.z() <= aabb.this.minCoordinates.z(); - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.x() >= aabb.this.maxCoordinates.x(); - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.y() >= aabb.this.maxCoordinates.y(); - isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.z() >= aabb.this.maxCoordinates.z(); + 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; } -AABB AABB::createAABBForTriangle( vec3* trianglePoints) { - vec3 minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); - vec3 maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); +AABB AABB::createAABBForTriangle( Vector3f* trianglePoints) { + Vector3f minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); + Vector3f maxCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); if (trianglePoints[1].x() < minCoords.x()) { minCoords.setX(trianglePoints[1].x()); } @@ -104,21 +104,21 @@ AABB AABB::createAABBForTriangle( vec3* trianglePoints) { } boolean AABB::testRayIntersect( Ray ray) { - vec3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - vec3 e = this.maxCoordinates - this.minCoordinates; - vec3 d = point2 - ray.point1; - vec3 m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates; + Vector3f point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); + Vector3f e = this.maxCoordinates - this.minCoordinates; + Vector3f d = point2 - ray.point1; + Vector3f m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates; // Test if the AABB face normals are separating axis - float adx = etk::abs(d.x()); - if (etk::abs(m.x()) > e.x() + adx) { + float adx = abs(d.x()); + if (abs(m.x()) > e.x() + adx) { return false; } - float ady = etk::abs(d.y()); - if (etk::abs(m.y()) > e.y() + ady) { + float ady = abs(d.y()); + if (abs(m.y()) > e.y() + ady) { return false; } - float adz = etk::abs(d.z()); - if (etk::abs(m.z()) > e.z() + adz) { + float adz = abs(d.z()); + if (abs(m.z()) > e.z() + adz) { return false; } // Add in an epsilon term to counteract arithmetic errors when segment is @@ -129,50 +129,50 @@ boolean AABB::testRayIntersect( Ray ray) { adz += epsilon; // Test if the cross products between face normals and ray direction are // separating axis - if (etk::abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) { + if (abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) { return false; } - if (etk::abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) { + if (abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) { return false; } - if (etk::abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) { + if (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; } -vec3 AABB::getExtent() { +Vector3f AABB::getExtent() { return this.maxCoordinates - this.minCoordinates; } void AABB::inflate(float dx, float dy, float dz) { - this.maxCoordinates += vec3(dx, dy, dz); - this.minCoordinates -= vec3(dx, dy, dz); + this.maxCoordinates += Vector3f(dx, dy, dz); + this.minCoordinates -= Vector3f(dx, dy, dz); } boolean AABB::testCollision( AABB aabb) { - if ( this.maxCoordinates.x() < aabb.this.minCoordinates.x() - || aabb.this.maxCoordinates.x() < this.minCoordinates.x()) { + if ( this.maxCoordinates.x() < aabb.minCoordinates.x() + || aabb.maxCoordinates.x() < this.minCoordinates.x()) { return false; } - if ( this.maxCoordinates.y() < aabb.this.minCoordinates.y() - || aabb.this.maxCoordinates.y() < this.minCoordinates.y()) { + if ( this.maxCoordinates.y() < aabb.minCoordinates.y() + || aabb.maxCoordinates.y() < this.minCoordinates.y()) { return false; } - if ( this.maxCoordinates.z() < aabb.this.minCoordinates.z() - || aabb.this.maxCoordinates.z() < this.minCoordinates.z()) { + if ( this.maxCoordinates.z() < aabb.minCoordinates.z() + || aabb.maxCoordinates.z() < this.minCoordinates.z()) { return false; } return true; } float AABB::getVolume() { - vec3 diff = this.maxCoordinates - this.minCoordinates; + Vector3f diff = this.maxCoordinates - this.minCoordinates; return (diff.x() * diff.y() * diff.z()); } -boolean AABB::testCollisionTriangleAABB( vec3* trianglePoints) { +boolean AABB::testCollisionTriangleAABB( Vector3f* trianglePoints) { if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) { return false; } @@ -194,16 +194,16 @@ boolean AABB::testCollisionTriangleAABB( vec3* trianglePoints) { return true; } -boolean AABB::contains( vec3 point) { - return point.x() >= this.minCoordinates.x() - FLTEPSILON hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.x() <= this.maxCoordinates.x() + FLTEPSILON - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON; +boolean AABB::contains( Vector3f point) { + return point.x() >= this.minCoordinates.x() - FLTEPSILON && point.x() <= this.maxCoordinates.x() + FLTEPSILON + && point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON + && point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON; } AABB AABB::operator=( AABB aabb) { if (this != aabb) { - this.minCoordinates = aabb.this.minCoordinates; - this.maxCoordinates = aabb.this.maxCoordinates; + this.minCoordinates = aabb.minCoordinates; + this.maxCoordinates = aabb.maxCoordinates; } return *this; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/AABB.hpp b/src/org/atriaSoft/ephysics/collision/shapes/AABB.java similarity index 82% rename from src/org/atriaSoft/ephysics/collision/shapes/AABB.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/AABB.java index 0a7f97b..585dcea 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/AABB.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/AABB.java @@ -1,14 +1,5 @@ -/** @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) - */ -#pragma once -#include -namespace ephysics { +package org.atriaSoft.ephysics.collision.shapes; + /** * @brief Represents a bounding volume of type "Axis Aligned * Bounding Box". It's a box where all the edges are always aligned @@ -18,9 +9,9 @@ namespace ephysics { class AABB { private : /// Minimum world coordinates of the AABB on the x,y and z axis - vec3 this.minCoordinates; + Vector3f minCoordinates; /// Maximum world coordinates of the AABB on the x,y and z axis - vec3 this.maxCoordinates; + Vector3f maxCoordinates; public : /** * @brief default contructor @@ -31,7 +22,7 @@ namespace ephysics { * @param[in] minCoordinates Minimum coordinates * @param[in] maxCoordinates Maximum coordinates */ - AABB( vec3 minCoordinates, vec3 maxCoordinates); + AABB( Vector3f minCoordinates, Vector3f maxCoordinates); /** * @brief Copy-contructor * @param[in] aabb the object to copy @@ -41,42 +32,42 @@ namespace ephysics { * @brief Get the center point of the AABB box * @return The 3D position of the center */ - vec3 getCenter() { + Vector3f getCenter() { return (this.minCoordinates + this.maxCoordinates) * 0.5f; } /** * @brief Get the minimum coordinates of the AABB * @return The 3d minimum coordonates */ - vec3 getMin() { + Vector3f getMin() { return this.minCoordinates; } /** * @brief Set the minimum coordinates of the AABB * @param[in] min The 3d minimum coordonates */ - void setMin( vec3 min) { + void setMin( Vector3f min) { this.minCoordinates = min; } /** * @brief Return the maximum coordinates of the AABB * @return The 3d maximum coordonates */ - vec3 getMax() { + Vector3f getMax() { return this.maxCoordinates; } /** * @brief Set the maximum coordinates of the AABB * @param[in] max The 3d maximum coordonates */ - void setMax( vec3 max) { + void setMax( Vector3f max) { this.maxCoordinates = max; } /** * @brief Get the size of the AABB in the three dimension x, y and z * @return the AABB 3D size */ - vec3 getExtent() ; + Vector3f getExtent() ; /** * @brief Inflate each side of the AABB by a given size * @param[in] dx Inflate X size @@ -119,13 +110,13 @@ namespace ephysics { * @param[in] point Point to check. * @return true The point in contained inside */ - boolean contains( vec3 point) ; + boolean contains( Vector3f point) ; /** * @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 */ - boolean testCollisionTriangleAABB( vec3* trianglePoints) ; + boolean testCollisionTriangleAABB( Vector3f* trianglePoints) ; /** * @brief check if the ray intersects the AABB * This method use the line vs AABB raycasting technique described in @@ -139,15 +130,14 @@ namespace ephysics { * @param[in] trianglePoints List of 3 point od a triangle * @return An AABB box */ - static AABB createAABBForTriangle( vec3* trianglePoints); + static AABB createAABBForTriangle( Vector3f* trianglePoints); /** * @brief Assignment operator * @param[in] aabb The other box to compare * @return reference on this */ AABB operator=( AABB aabb); - friend class DynamicAABBTree; }; -} \ No newline at end of file +} diff --git a/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp index 7812eaf..0da139e 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp @@ -14,17 +14,17 @@ using namespace ephysics; -BoxShape::BoxShape( vec3 extent, float margin): +BoxShape::BoxShape( Vector3f extent, float margin): ConvexShape(BOX, margin), - this.extent(extent - vec3(margin, margin, margin)) { - assert(extent.x() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.x() > margin); - assert(extent.y() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.y() > margin); - assert(extent.z() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.z() > margin); + this.extent(extent - Vector3f(margin, margin, margin)) { + assert(extent.x() > 0.0f && extent.x() > margin); + assert(extent.y() > 0.0f && extent.y() > margin); + assert(extent.z() > 0.0f && extent.z() > margin); } -void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void BoxShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { float factor = (1.0f / float(3.0)) * mass; - vec3 realExtent = this.extent + vec3(this.margin, this.margin, this.margin); + Vector3f realExtent = this.extent + Vector3f(this.margin, this.margin, this.margin); float xSquare = realExtent.x() * realExtent.x(); float ySquare = realExtent.y() * realExtent.y(); float zSquare = realExtent.z() * realExtent.z(); @@ -34,15 +34,15 @@ void BoxShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { } boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - vec3 rayDirection = ray.point2 - ray.point1; + Vector3f rayDirection = ray.point2 - ray.point1; float tMin = FLTMIN; float tMax = FLTMAX; - vec3 normalDirection(0,0,0); - vec3 currentNormal(0,0,0); + Vector3f normalDirection(0,0,0); + Vector3f currentNormal(0,0,0); // For each of the three slabs for (int iii=0; iii<3; ++iii) { // If ray is parallel to the slab - if (etk::abs(rayDirection[iii]) < FLTEPSILON) { + if (abs(rayDirection[iii]) < FLTEPSILON) { // If the ray's origin is not inside the slab, there is no hit if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) { return false; @@ -58,7 +58,7 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh // Swap t1 and t2 if need so that t1 is intersection with near plane and // t2 with far plane if (t1 > t2) { - etk::swap(t1, t2); + swap(t1, t2); currentNormal = -currentNormal; } // Compute the intersection of the of slab intersection interval with previous slabs @@ -66,7 +66,7 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh tMin = t1; normalDirection = currentNormal; } - tMax = etk::min(tMax, t2); + tMax = min(tMax, t2); // If tMin is larger than the maximum raycasting fraction, we return no hit if (tMin > ray.maxFraction) { return false; @@ -82,12 +82,12 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh || tMin > ray.maxFraction) { return false; } - if (normalDirection == vec3(0,0,0)) { + if (normalDirection == Vector3f(0,0,0)) { return false; } // The ray intersects the three slabs, we compute the hit point - vec3 localHitPoint = ray.point1 + tMin * rayDirection; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + tMin * rayDirection; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = tMin; raycastInfo.worldPoint = localHitPoint; @@ -95,39 +95,39 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh return true; } -vec3 BoxShape::getExtent() { - return this.extent + vec3(this.margin, this.margin, this.margin); +Vector3f BoxShape::getExtent() { + return this.extent + Vector3f(this.margin, this.margin, this.margin); } -void BoxShape::setLocalScaling( vec3 scaling) { +void BoxShape::setLocalScaling( Vector3f scaling) { this.extent = (this.extent / this.scaling) * scaling; CollisionShape::setLocalScaling(scaling); } -void BoxShape::getLocalBounds(vec3 min, vec3 max) { +void BoxShape::getLocalBounds(Vector3f min, Vector3f max) { // Maximum bounds - max = this.extent + vec3(this.margin, this.margin, this.margin); + max = this.extent + Vector3f(this.margin, this.margin, this.margin); // Minimum bounds min = -max; } -sizet BoxShape::getSizeInBytes() { +long BoxShape::getSizeInBytes() { return sizeof(BoxShape); } -vec3 BoxShape::getLocalSupportPointWithoutMargin( vec3 direction, +Vector3f BoxShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { - return vec3(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(), + return 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()); } -boolean BoxShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean BoxShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { return ( localPoint.x() < this.extent[0] - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.x() > -this.extent[0] - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.extent[1] - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.extent[1] - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() < this.extent[2] - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() > -this.extent[2]); + && localPoint.x() > -this.extent[0] + && localPoint.y() < this.extent[1] + && localPoint.y() > -this.extent[1] + && localPoint.z() < this.extent[2] + && localPoint.z() > -this.extent[2]); } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.hpp deleted file mode 100644 index 9a19b89..0000000 --- a/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.hpp +++ /dev/null @@ -1,58 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include - -namespace ephysics { - -/** - * @brief It 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 - * ructor of the box shape. Otherwise, it is recommended to use the - * default margin distance by not using the "margin" parameter in the ructor. - */ -class BoxShape : public ConvexShape { - public: - /** - * @brief Constructor - * @param extent The vector with the three extents of the box (in meters) - * @param margin The collision margin (in meters) around the collision shape - */ - BoxShape( vec3 extent, float margin = OBJECTMARGIN); - /// DELETE copy-ructor - BoxShape( BoxShape shape) = delete; - /// DELETE assignment operator - BoxShape operator=( BoxShape shape) = delete; - /** - * @brief Return the extents of the box - * @return The vector with the three extents of the box shape (in meters) - */ - vec3 getExtent() ; - void setLocalScaling( vec3 scaling) override; - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; - protected: - vec3 this.extent; //!< Extent sizes of the box in the x, y and z direction - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; -}; - -} 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..ae19ecf --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/shapes/BoxShape.java @@ -0,0 +1,41 @@ +package org.atriaSoft.ephysics.collision.shapes; + + +/** + * @brief It 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 + * ructor of the box shape. Otherwise, it is recommended to use the + * default margin distance by not using the "margin" parameter in the ructor. + */ +class BoxShape extends ConvexShape { + public: + /** + * @brief Constructor + * @param extent The vector with the three extents of the box (in meters) + * @param margin The collision margin (in meters) around the collision shape + */ + BoxShape( Vector3f extent, float margin = OBJECTMARGIN); + /** + * @brief Return the extents of the box + * @return The vector with the three extents of the box shape (in meters) + */ + Vector3f getExtent(); + void setLocalScaling( Vector3f scaling); + void getLocalBounds(Vector3f min, Vector3f max); + void computeLocalInertiaTensor(Matrix3f tensor, float mass); + protected: + Vector3f extent; //!< Extent sizes of the box in the x, y and z direction + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData); + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape); + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape); + long getSizeInBytes(); +}; + +} diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp index 4351fc6..0e7d2ef 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp @@ -19,7 +19,7 @@ CapsuleShape::CapsuleShape(float radius, float height): assert(height > 0.0f); } -void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void CapsuleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1 float height = this.halfHeight + this.halfHeight; float radiusSquare = this.margin * this.margin; @@ -37,39 +37,39 @@ void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) 0.0, 0.0, IxxAndzz); } -boolean CapsuleShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean CapsuleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { float diffYCenterSphere1 = localPoint.y() - this.halfHeight; float diffYCenterSphere2 = localPoint.y() + this.halfHeight; float xSquare = localPoint.x() * localPoint.x(); float zSquare = localPoint.z() * localPoint.z(); 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 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj - localPoint.y() < this.halfHeight hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight) || + return ((xSquare + zSquare) < squareRadius && + localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight) || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; } boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - vec3 n = ray.point2 - ray.point1; + Vector3f n = ray.point2 - ray.point1; float epsilon = float(0.01); - vec3 p(float(0), -this.halfHeight, float(0)); - vec3 q(float(0), this.halfHeight, float(0)); - vec3 d = q - p; - vec3 m = ray.point1 - p; + Vector3f p(float(0), -this.halfHeight, float(0)); + Vector3f q(float(0), this.halfHeight, float(0)); + Vector3f d = q - p; + Vector3f m = ray.point1 - p; float t; float mDotD = m.dot(d); float nDotD = n.dot(d); float dDotD = d.dot(d); // Test if the segment is outside the cylinder - float vec1DotD = (ray.point1 - vec3(0.0f, -this.halfHeight - this.margin, float(0.0))).dot(d); + float vec1DotD = (ray.point1 - Vector3f(0.0f, -this.halfHeight - this.margin, float(0.0))).dot(d); if ( vec1DotD < 0.0f - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD < float(0.0)) { + && vec1DotD + nDotD < float(0.0)) { return false; } float ddotDExtraCaps = float(2.0) * this.margin * d.y(); if ( vec1DotD > dDotD + ddotDExtraCaps - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD > dDotD + ddotDExtraCaps) { + && vec1DotD + nDotD > dDotD + ddotDExtraCaps) { return false; } float nDotN = n.dot(n); @@ -78,7 +78,7 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro float k = m.dot(m) - this.margin * this.margin; float c = dDotD * k - mDotD * mDotD; // If the ray is parallel to the capsule axis - if (etk::abs(a) < epsilon) { + if (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; @@ -87,28 +87,28 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro // If the ray intersects with the "p" endcap of the capsule if (mDotD < 0.0f) { // Check intersection between the ray and the "p" sphere endcap of the capsule - vec3 hitLocalPoint; + Vector3f hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - vec3 normalDirection = hitLocalPoint - p; + Vector3f normalDirection = hitLocalPoint - p; raycastInfo.worldNormal = normalDirection; return true; } return false; } else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder // Check intersection between the ray and the "q" sphere endcap of the capsule - vec3 hitLocalPoint; + Vector3f hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - vec3 normalDirection = hitLocalPoint - q; + Vector3f normalDirection = hitLocalPoint - q; raycastInfo.worldNormal = normalDirection; return true; } @@ -125,33 +125,33 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro return false; } // Compute the smallest root (first intersection along the ray) - float t0 = t = (-b - etk::sqrt(discriminant)) / a; + float t0 = t = (-b - sqrt(discriminant)) / a; // If the intersection is outside the finite cylinder of the capsule on "p" endcap side float value = mDotD + t * nDotD; if (value < 0.0f) { // Check intersection between the ray and the "p" sphere endcap of the capsule - vec3 hitLocalPoint; + Vector3f hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - vec3 normalDirection = hitLocalPoint - p; + Vector3f normalDirection = hitLocalPoint - p; raycastInfo.worldNormal = normalDirection; return true; } return false; } else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side // Check intersection between the ray and the "q" sphere endcap of the capsule - vec3 hitLocalPoint; + Vector3f hitLocalPoint; float hitFraction; if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = hitFraction; raycastInfo.worldPoint = hitLocalPoint; - vec3 normalDirection = hitLocalPoint - q; + Vector3f normalDirection = hitLocalPoint - q; raycastInfo.worldNormal = normalDirection; return true; } @@ -164,31 +164,31 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 v = localHitPoint - p; - vec3 w = (v.dot(d) / d.length2()) * d; - vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized(); + Vector3f v = localHitPoint - p; + Vector3f w = (v.dot(d) / d.length2()) * d; + Vector3f normalDirection = (localHitPoint - (p + w)).safeNormalized(); raycastInfo.worldNormal = normalDirection; return true; } -boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1, - vec3 point2, - vec3 sphereCenter, +boolean CapsuleShape::raycastWithSphereEndCap( Vector3f point1, + Vector3f point2, + Vector3f sphereCenter, float maxFraction, - vec3 hitLocalPoint, + Vector3f hitLocalPoint, float hitFraction) { - vec3 m = point1 - sphereCenter; + Vector3f m = point1 - sphereCenter; float c = m.dot(m) - this.margin * this.margin; // If the origin of the ray is inside the sphere, we return no intersection if (c < 0.0f) { return false; } - vec3 rayDirection = point2 - point1; + Vector3f rayDirection = point2 - point1; 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 intersection @@ -204,7 +204,7 @@ boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1, return false; } // Compute the solution "t" closest to the origin - float t = -b - etk::sqrt(discriminant); + float t = -b - sqrt(discriminant); assert(t >= 0.0f); // If the hit point is withing the segment ray fraction if (t < maxFraction * raySquareLength) { @@ -225,17 +225,17 @@ float CapsuleShape::getHeight() { return this.halfHeight + this.halfHeight; } -void CapsuleShape::setLocalScaling( vec3 scaling) { +void CapsuleShape::setLocalScaling( Vector3f scaling) { this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y(); this.margin = (this.margin / this.scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } -sizet CapsuleShape::getSizeInBytes() { +long CapsuleShape::getSizeInBytes() { return sizeof(CapsuleShape); } -void CapsuleShape::getLocalBounds(vec3 min, vec3 max) { +void CapsuleShape::getLocalBounds(Vector3f min, Vector3f max) { // Maximum bounds max.setX(this.margin); max.setY(this.halfHeight + this.margin); @@ -246,7 +246,7 @@ void CapsuleShape::getLocalBounds(vec3 min, vec3 max) { min.setZ(min.x()); } -vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction, +Vector3f CapsuleShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { // Support point top sphere float dotProductTop = this.halfHeight * direction.y(); @@ -254,7 +254,7 @@ vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction, float dotProductBottom = -this.halfHeight * direction.y(); // Return the point with the maximum dot product if (dotProductTop > dotProductBottom) { - return vec3(0, this.halfHeight, 0); + return Vector3f(0, this.halfHeight, 0); } - return vec3(0, -this.halfHeight, 0); + return Vector3f(0, -this.halfHeight, 0); } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.java similarity index 53% rename from src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.java index 6eb9f12..4053a69 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.java @@ -1,18 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include - -namespace ephysics { /** * @brief It 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. @@ -22,7 +9,7 @@ namespace ephysics { * and height of the shape. Therefore, no need to specify an object margin for a * capsule shape. */ - class CapsuleShape : public ConvexShape { + class CapsuleShape extends ConvexShape { public : /** * @brief Constructor @@ -44,23 +31,23 @@ namespace ephysics { * @return The height of the capsule shape (in meters) */ float getHeight() ; - void setLocalScaling( vec3 scaling) override; - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; + void setLocalScaling( Vector3f scaling) ; + void getLocalBounds(Vector3f min, Vector3f max) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; protected: - float this.halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres) - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; + float halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres) + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ; + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; /** * @brief Raycasting method between a ray one of the two spheres end cap of the capsule */ - boolean raycastWithSphereEndCap( vec3 point1, - vec3 point2, - vec3 sphereCenter, + boolean raycastWithSphereEndCap( Vector3f point1, + Vector3f point2, + Vector3f sphereCenter, float maxFraction, - vec3 hitLocalPoint, + Vector3f hitLocalPoint, float hitFraction) ; - sizet getSizeInBytes() override; + long getSizeInBytes() ; }; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.cpp index e3865e1..6a04192 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.cpp @@ -19,23 +19,23 @@ CollisionShape::CollisionShape(CollisionShapeType type) : } -void CollisionShape::computeAABB(AABB aabb, etk::Transform3D transform) { +void CollisionShape::computeAABB(AABB aabb, Transform3D transform) { PROFILE("CollisionShape::computeAABB()"); // Get the local bounds in x,y and z direction - vec3 minBounds(0,0,0); - vec3 maxBounds(0,0,0); + Vector3f minBounds(0,0,0); + Vector3f maxBounds(0,0,0); getLocalBounds(minBounds, maxBounds); // Rotate the local bounds according to the orientation of the body - etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute(); - vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), + Matrix3f worldAxis = transform.getOrientation().getMatrix().getAbsolute(); + Vector3f worldMinBounds(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds)); - vec3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds), + Vector3f worldMaxBounds(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 - vec3 minCoordinates = transform.getPosition() + worldMinBounds; - vec3 maxCoordinates = transform.getPosition() + worldMaxBounds; + Vector3f minCoordinates = transform.getPosition() + worldMinBounds; + Vector3f maxCoordinates = transform.getPosition() + worldMaxBounds; // Update the AABB with the new minimum and maximum coordinates aabb.setMin(minCoordinates); aabb.setMax(maxCoordinates); @@ -44,7 +44,7 @@ void CollisionShape::computeAABB(AABB aabb, etk::Transform3D transform) { int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, CollisionShapeType shapeType2) { // If both shapes are convex - if (isConvex(shapeType1) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isConvex(shapeType2)) { + if (isConvex(shapeType1) && isConvex(shapeType2)) { return NBMAXCONTACTMANIFOLDSCONVEXSHAPE; } // If there is at least one concave shape diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.java similarity index 58% rename from src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.java index 6ecd0d1..386d806 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.java @@ -1,21 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include -#include -#include -#include - -namespace ephysics { enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD}; int NBCOLLISIONSHAPETYPES = 9; @@ -31,12 +15,6 @@ class CollisionShape { public : /// Constructor CollisionShape(CollisionShapeType type); - /// DELETE copy-ructor - CollisionShape( CollisionShape shape) = delete; - /// DELETE assignment operator - CollisionShape operator=( CollisionShape shape) = delete; - /// Virtualize destructor - virtual ~CollisionShape() {}; /** * @brief Get the type of the collision shapes * @return The type of the collision shape (box, sphere, cylinder, ...) @@ -49,22 +27,22 @@ class CollisionShape { * @return true If the collision shape is convex * @return false If it is concave */ - virtual boolean isConvex() = 0; + boolean isConvex() = 0; /** * @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 */ - virtual void getLocalBounds(vec3 min, vec3 max) =0; + void getLocalBounds(Vector3f min, Vector3f max) =0; /// Return the scaling vector of the collision shape - vec3 getScaling() { + Vector3f getScaling() { return this.scaling; } /** * @brief Set the scaling vector of the collision shape */ - virtual void setLocalScaling( vec3 scaling) { + void setLocalScaling( Vector3f scaling) { this.scaling = scaling; } /** @@ -72,13 +50,13 @@ class CollisionShape { * @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 */ - virtual void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) =0; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) =0; /** * @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 etk::Transform3D used to compute the AABB of the collision shape + * @param[in] transform Transform3D used to compute the AABB of the collision shape */ - virtual void computeAABB(AABB aabb, etk::Transform3D transform) ; + void computeAABB(AABB aabb, Transform3D transform) ; /** * @brief Check if the shape is convex * @param[in] shapeType shape type @@ -87,7 +65,7 @@ class CollisionShape { */ static boolean isConvex(CollisionShapeType shapeType) { return shapeType != CONCAVEMESH - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapeType != HEIGHTFIELD; + && shapeType != HEIGHTFIELD; } /** * @brief Get the maximum number of contact @@ -95,17 +73,15 @@ class CollisionShape { */ static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, CollisionShapeType shapeType2); - friend class ProxyShape; - friend class CollisionWorld; protected : - CollisionShapeType this.type; //!< Type of the collision shape - vec3 this.scaling; //!< Scaling vector of the collision shape + CollisionShapeType type; //!< Type of the collision shape + Vector3f scaling; //!< Scaling vector of the collision shape /// Return true if a point is inside the collision shape - virtual boolean testPointInside( vec3 worldPoint, ProxyShape* proxyShape) = 0; + boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape) = 0; /// Raycast method with feedback information - virtual boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0; /// Return the number of bytes used by the collision shape - virtual sizet getSizeInBytes() = 0; + long getSizeInBytes() = 0; }; diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp index f44d7e7..0dbaeed 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp @@ -21,13 +21,13 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh): void ConcaveMeshShape::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; subPartgetNbSubparts(); subPart++) { + for (int subPart=0; subPartgetSubpart(subPart); + TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart); // For each triangle of the concave mesh - for (sizet iii=0; iiigetNbTriangles(); ++iii) { - ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(iii); - vec3 trianglePoints2[3]; + for (long iii=0; iiigetSubpart(subPart); + TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart); if (triangleVertexArray == null) { Log.error("get null ..."); } - ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(triangleIndex); + ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex); outTriangleVertices[0] = trianglePoints[0] * this.scaling; outTriangleVertices[1] = trianglePoints[1] * this.scaling; outTriangleVertices[2] = trianglePoints[2] * this.scaling; @@ -60,7 +60,7 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback callback, AABB localAA // Get the node data (triangle index and mesh subpart index) int* data = this.dynamicAABBTree.getNodeDataInt(nodeId); // Get the triangle vertices for this node from the concave mesh shape - vec3 trianglePoints[3]; + Vector3f trianglePoints[3]; getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); // Call the callback to test narrow-phase collision with this triangle callback.testTriangle(trianglePoints); @@ -86,13 +86,13 @@ float ConcaveMeshRaycastCallback::operator()(int nodeId, Ray ray) { } void ConcaveMeshRaycastCallback::raycastTriangles() { - etk::Vector::Iterator it; + Vector::Iterator it; float smallestHitFraction = this.ray.maxFraction; for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) { // Get the node data (triangle index and mesh subpart index) int* data = this.dynamicAABBTree.getNodeDataInt(*it); // Get the triangle vertices for this node from the concave mesh shape - vec3 trianglePoints[3]; + Vector3f trianglePoints[3]; this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); // Create a triangle collision shape float margin = this.concaveMeshShape.getTriangleMargin(); @@ -102,7 +102,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { RaycastInfo raycastInfo; boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape); // If the ray hit the collision shape - if (isTriangleHit hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= smallestHitFraction) { + if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) { assert(raycastInfo.hitFraction >= 0.0f); this.raycastInfo.body = raycastInfo.body; this.raycastInfo.proxyShape = raycastInfo.proxyShape; @@ -117,24 +117,24 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { } } -sizet ConcaveMeshShape::getSizeInBytes() { +long ConcaveMeshShape::getSizeInBytes() { return sizeof(ConcaveMeshShape); } -void ConcaveMeshShape::getLocalBounds(vec3 min, vec3 max) { +void ConcaveMeshShape::getLocalBounds(Vector3f min, Vector3f max) { // Get the AABB of the whole tree AABB treeAABB = this.dynamicAABBTree.getRootAABB(); min = treeAABB.getMin(); max = treeAABB.getMax(); } -void ConcaveMeshShape::setLocalScaling( vec3 scaling) { +void ConcaveMeshShape::setLocalScaling( Vector3f scaling) { CollisionShape::setLocalScaling(scaling); this.dynamicAABBTree.reset(); initBVHTree(); } -void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3f tensor, 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, diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.hpp deleted file mode 100644 index 984e911..0000000 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.hpp +++ /dev/null @@ -1,85 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include -#include -#include - -namespace ephysics { - class ConcaveMeshShape; - class ConcaveMeshRaycastCallback { - private: - etk::Vector this.hitAABBNodes; - DynamicAABBTree this.dynamicAABBTree; - ConcaveMeshShape this.concaveMeshShape; - ProxyShape* this.proxyShape; - RaycastInfo this.raycastInfo; - Ray this.ray; - boolean this.isHit; - public: - // Constructor - ConcaveMeshRaycastCallback( DynamicAABBTree dynamicAABBTree, - ConcaveMeshShape concaveMeshShape, - ProxyShape* proxyShape, - RaycastInfo raycastInfo, - Ray ray): - this.dynamicAABBTree(dynamicAABBTree), - this.concaveMeshShape(concaveMeshShape), - this.proxyShape(proxyShape), - this.raycastInfo(raycastInfo), - this.ray(ray), - this.isHit(false) { - - } - /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree - float operator()(int nodeId, ephysics::Ray ray); - /// Raycast all collision shapes that have been collected - void raycastTriangles(); - /// Return true if a raycast hit has been found - boolean getIsHit() { - return this.isHit; - } - }; - /** - * @brief 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. - */ - class ConcaveMeshShape : public ConcaveShape { - public: - /// Constructor - ConcaveMeshShape(TriangleMesh* triangleMesh); - /// DELETE copy-ructor - ConcaveMeshShape( ConcaveMeshShape shape) = delete; - /// DELETE assignment operator - ConcaveMeshShape operator=( ConcaveMeshShape shape) = delete; - virtual void getLocalBounds(vec3 min, vec3 max) override; - virtual void setLocalScaling( vec3 scaling) override; - virtual void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; - virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) override; - friend class ConvexTriangleAABBOverlapCallback; - friend class ConcaveMeshRaycastCallback; - protected: - TriangleMesh* this.triangleMesh; //!< Triangle mesh - DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles - virtual boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - virtual sizet getSizeInBytes() override; - /// Insert all the triangles into the dynamic AABB tree - void initBVHTree(); - /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle - /// given the start vertex index pointer of the triangle. - void getTriangleVerticesWithIndexPointer(int subPart, - int triangleIndex, - vec3* outTriangleVertices) ; - }; - -} 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..12a36dd --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.java @@ -0,0 +1,63 @@ +package org.atriaSoft.ephysics.collision.shapes; + + class ConcaveMeshRaycastCallback { + private: + Vector hitAABBNodes; + DynamicAABBTree dynamicAABBTree; + ConcaveMeshShape concaveMeshShape; + ProxyShape* proxyShape; + RaycastInfo raycastInfo; + Ray ray; + boolean isHit; + public: + // Constructor + ConcaveMeshRaycastCallback( DynamicAABBTree dynamicAABBTree, + ConcaveMeshShape concaveMeshShape, + ProxyShape* proxyShape, + RaycastInfo raycastInfo, + Ray ray): + this.dynamicAABBTree(dynamicAABBTree), + this.concaveMeshShape(concaveMeshShape), + this.proxyShape(proxyShape), + this.raycastInfo(raycastInfo), + this.ray(ray), + this.isHit(false) { + + } + /// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree + float operator()(int nodeId, ephysics::Ray ray); + /// Raycast all collision shapes that have been collected + void raycastTriangles(); + /// Return true if a raycast hit has been found + boolean getIsHit() { + return this.isHit; + } + }; + /** + * @brief 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. + */ + class ConcaveMeshShape extends ConcaveShape { + public: + /// Constructor + ConcaveMeshShape(TriangleMesh* triangleMesh); + void getLocalBounds(Vector3f min, Vector3f max); + void setLocalScaling( Vector3f scaling); + void computeLocalInertiaTensor(Matrix3f tensor, float mass); + void testAllTriangles(TriangleCallback callback, AABB localAABB); + protected: + TriangleMesh* triangleMesh; //!< Triangle mesh + DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape); + long getSizeInBytes(); + /// Insert all the triangles into the dynamic AABB tree + void initBVHTree(); + /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle + /// given the start vertex index pointer of the triangle. + void getTriangleVerticesWithIndexPointer(int subPart, + int triangleIndex, + Vector3f* outTriangleVertices) ; + }; + +} diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp index 6c19feb..1f7de1e 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp @@ -29,7 +29,7 @@ boolean ConcaveShape::isConvex() { return false; } -boolean ConcaveShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean ConcaveShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { return false; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.java similarity index 57% rename from src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.java index 73ce9b9..75ddf38 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.java @@ -1,33 +1,19 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include - -namespace ephysics { /** * @brief It is used to encapsulate a callback method for * a single triangle of a ConcaveMesh. */ class TriangleCallback { - public: - virtual ~TriangleCallback() = default; - /// Report a triangle - virtual void testTriangle( vec3* trianglePoints)=0; + /// Report a triangle + public void testTriangle( Vector3f* trianglePoints)=0; }; /** * @brief This abstract class represents a concave collision shape associated with a * body that is used during the narrow-phase collision detection. */ - class ConcaveShape : public CollisionShape { + class ConcaveShape extends CollisionShape { public : /// Constructor ConcaveShape(CollisionShapeType type); @@ -36,10 +22,10 @@ namespace ephysics { /// DELETE assignment operator ConcaveShape operator=( ConcaveShape shape) = delete; protected : - boolean this.isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled - float this.triangleMargin; //!< Margin use for collision detection for each triangle - TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; + boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled + float triangleMargin; //!< Margin use for collision detection for each triangle + TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; public: /// Return the triangle margin float getTriangleMargin() ; @@ -51,9 +37,9 @@ namespace ephysics { */ void setRaycastTestType(TriangleRaycastSide testType); /// Return true if the collision shape is convex, false if it is concave - virtual boolean isConvex() override; + boolean isConvex() ; /// Use a callback method on all triangles of the concave shape inside a given AABB - virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) =0; + void testAllTriangles(TriangleCallback callback, AABB localAABB) =0; /// Return true if the smooth mesh collision is enabled boolean getIsSmoothMeshCollisionEnabled() ; /** diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.cpp index 1503131..9eff827 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.cpp @@ -22,49 +22,49 @@ ConeShape::ConeShape(float radius, float height, float margin): this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height)); } -vec3 ConeShape::getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) { - vec3 v = direction; +Vector3f ConeShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { + Vector3f v = direction; float sinThetaTimesLengthV = this.sinTheta * v.length(); - vec3 supportPoint; + Vector3f supportPoint; if (v.y() > sinThetaTimesLengthV) { - supportPoint = vec3(0.0, this.halfHeight, 0.0); + supportPoint = Vector3f(0.0, this.halfHeight, 0.0); } else { float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); if (projectedLength > FLTEPSILON) { float d = this.radius / projectedLength; - supportPoint = vec3(v.x() * d, -this.halfHeight, v.z() * d); + supportPoint = Vector3f(v.x() * d, -this.halfHeight, v.z() * d); } else { - supportPoint = vec3(0.0, -this.halfHeight, 0.0); + supportPoint = Vector3f(0.0, -this.halfHeight, 0.0); } } return supportPoint; } boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - vec3 r = ray.point2 - ray.point1; + Vector3f r = ray.point2 - ray.point1; float epsilon = float(0.00001); - vec3 V(0, this.halfHeight, 0); - vec3 centerBase(0, -this.halfHeight, 0); - vec3 axis(0, float(-1.0), 0); + Vector3f V(0, this.halfHeight, 0); + Vector3f centerBase(0, -this.halfHeight, 0); + Vector3f axis(0, float(-1.0), 0); float heightSquare = float(4.0) * this.halfHeight * this.halfHeight; float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius); float factor = 1.0f - cosThetaSquare; - vec3 delta = ray.point1 - V; + Vector3f delta = ray.point1 - V; float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z(); float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z(); float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z(); float tHit[] = {float(-1.0), float(-1.0), float(-1.0)}; - vec3 localHitPoint[3]; - vec3 localNormal[3]; + Vector3f localHitPoint[3]; + Vector3f localNormal[3]; // If c2 is different from zero - if (etk::abs(c2) > FLTEPSILON) { + if (abs(c2) > FLTEPSILON) { 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 intersections - float sqrRoot = etk::sqrt(gamma); + float sqrRoot = sqrt(gamma); tHit[0] = (-c1 - sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2; } else { // If the equation has a single real root @@ -73,7 +73,7 @@ boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyS } } else { // If c2 == 0 - if (etk::abs(c1) > FLTEPSILON) { + if (abs(c1) > FLTEPSILON) { // If c2 = 0 and c1 != 0 tHit[0] = -c0 / (float(2.0) * c1); } else { @@ -140,14 +140,14 @@ boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyS float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z()); float rOverH = this.radius / h; float value2 = 1.0f + rOverH * rOverH; - float factor = 1.0f / etk::sqrt(value1 * value2); + float factor = 1.0f / sqrt(value1 * value2); float x = localHitPoint[hitIndex].x() * factor; float z = localHitPoint[hitIndex].z() * factor; localNormal[hitIndex].setX(x); - localNormal[hitIndex].setY(etk::sqrt(x * x + z * z) * rOverH); + localNormal[hitIndex].setY(sqrt(x * x + z * z) * rOverH); localNormal[hitIndex].setZ(z); } - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint[hitIndex]; @@ -163,17 +163,17 @@ float ConeShape::getHeight() { return float(2.0) * this.halfHeight; } -void ConeShape::setLocalScaling( vec3 scaling) { +void ConeShape::setLocalScaling( Vector3f scaling) { this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y(); this.radius = (this.radius / this.scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } -sizet ConeShape::getSizeInBytes() { +long ConeShape::getSizeInBytes() { return sizeof(ConeShape); } -void ConeShape::getLocalBounds(vec3 min, vec3 max) { +void ConeShape::getLocalBounds(Vector3f min, Vector3f max) { // Maximum bounds max.setX(this.radius + this.margin); max.setY(this.halfHeight + this.margin); @@ -184,7 +184,7 @@ void ConeShape::getLocalBounds(vec3 min, vec3 max) { min.setZ(min.x()); } -void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void ConeShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { float rSquare = this.radius * this.radius; float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight); tensor.setValue(diagXZ, 0.0, 0.0, @@ -192,11 +192,11 @@ void ConeShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { 0.0, 0.0, 0.0, diagXZ); } -boolean ConeShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean ConeShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { float radiusHeight = this.radius * (-localPoint.y() + this.halfHeight) / (this.halfHeight * float(2.0)); return ( localPoint.y() < this.halfHeight - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); + && localPoint.y() > -this.halfHeight) + && (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.java similarity index 51% rename from src/org/atriaSoft/ephysics/collision/shapes/ConeShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/ConeShape.java index 2003894..5a1e8be 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConeShape.java @@ -1,18 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include - -namespace ephysics { /** * @brief This class represents a cone collision shape centered at the * origin and alligned with the Y axis. The cone is defined @@ -27,7 +14,7 @@ namespace ephysics { * ructor of the cone shape. Otherwise, it is recommended to use the * default margin distance by not using the "margin" parameter in the ructor. */ - class ConeShape : public ConvexShape { + class ConeShape extends ConvexShape { public : /** * @brief Constructor @@ -36,32 +23,28 @@ namespace ephysics { * @param margin Collision margin (in meters) around the collision shape */ ConeShape(float radius, float height, float margin = OBJECTMARGIN); - /// DELETE copy-ructor - ConeShape( ConeShape shape) = delete; - /// DELETE assignment operator - ConeShape operator=( ConeShape shape) = delete; protected : - float this.radius; //!< Radius of the base - float this.halfHeight; //!< Half height of the cone - float this.sinTheta; //!< sine of the semi angle at the apex point - virtual vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; + float radius; //!< Radius of the base + float halfHeight; //!< Half height of the cone + float sinTheta; //!< sine of the semi angle at the apex point + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ; + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() ; public: /** * @brief Return the radius * @return Radius of the cone (in meters) */ - float getRadius() ; + float getRadius(); /** * @brief Return the height * @return Height of the cone (in meters) */ - float getHeight() ; + float getHeight(); - void setLocalScaling( vec3 scaling) override; - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; + void setLocalScaling( Vector3f scaling) ; + void getLocalBounds(Vector3f min, Vector3f max) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; }; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp index 3166510..32278d0 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp @@ -26,7 +26,7 @@ ConvexMeshShape::ConvexMeshShape( float* arrayVertices, // Copy all the vertices into the internal array for (int iii=0; iiigetVertices()) { + for (auto it: triangleVertexArray.getVertices()) { this.vertices.pushBack(it*this.scaling); } // If we need to use the edges information of the mesh if (this.isEdgesInformationUsed) { // For each triangle of the mesh - for (sizet iii=0; iiigetNbTriangles(); iii++) { + for (long iii=0; iiigetIndices()[iii*3]; - vertexIndex[1] = triangleVertexArray->getIndices()[iii*3+1]; - vertexIndex[2] = triangleVertexArray->getIndices()[iii*3+2]; + 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]); @@ -71,7 +71,7 @@ ConvexMeshShape::ConvexMeshShape(float margin): } -vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin( vec3 direction, +Vector3f ConvexMeshShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { assert(this.numberVertices == this.vertices.size()); assert(cachedCollisionData != null); @@ -91,9 +91,9 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin( vec3 direction, isOptimal = true; assert(this.edgesAdjacencyList[maxVertex].size() > 0); // For all neighbors of the current vertex - etk::Set::Iterator it; - etk::Set::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin(); - etk::Set::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end(); + Set::Iterator it; + Set::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin(); + Set::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end(); for (it = itBegin; it != itEnd; ++it) { // Compute the dot product float dotProduct = direction.dot(this.vertices[*it]); @@ -160,32 +160,32 @@ void ConvexMeshShape::recalculateBounds() { this.maxBounds = this.maxBounds * this.scaling; this.minBounds = this.minBounds * this.scaling; // Add the object margin to the bounds - this.maxBounds += vec3(this.margin, this.margin, this.margin); - this.minBounds -= vec3(this.margin, this.margin, this.margin); + this.maxBounds += Vector3f(this.margin, this.margin, this.margin); + this.minBounds -= Vector3f(this.margin, this.margin, this.margin); } boolean ConvexMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - return proxyShape->this.body->this.world.this.collisionDetection.this.narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); + return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.raycast(ray, proxyShape, raycastInfo); } -void ConvexMeshShape::setLocalScaling( vec3 scaling) { +void ConvexMeshShape::setLocalScaling( Vector3f scaling) { ConvexShape::setLocalScaling(scaling); recalculateBounds(); } -sizet ConvexMeshShape::getSizeInBytes() { +long ConvexMeshShape::getSizeInBytes() { return sizeof(ConvexMeshShape); } -void ConvexMeshShape::getLocalBounds(vec3 min, vec3 max) { +void ConvexMeshShape::getLocalBounds(Vector3f min, Vector3f max) { min = this.minBounds; max = this.maxBounds; } -void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void ConvexMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { float factor = (1.0f / float(3.0)) * mass; - vec3 realExtent = 0.5f * (this.maxBounds - this.minBounds); - assert(realExtent.x() > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0); + Vector3f realExtent = 0.5f * (this.maxBounds - this.minBounds); + assert(realExtent.x() > 0 && realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0); float xSquare = realExtent.x() * realExtent.x(); float ySquare = realExtent.y() * realExtent.y(); float zSquare = realExtent.z() * realExtent.z(); @@ -194,7 +194,7 @@ void ConvexMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mas 0.0, 0.0, factor * (xSquare + ySquare)); } -void ConvexMeshShape::addVertex( vec3 vertex) { +void ConvexMeshShape::addVertex( Vector3f vertex) { // Add the vertex in to vertices array this.vertices.pushBack(vertex); this.numberVertices++; @@ -222,11 +222,11 @@ void ConvexMeshShape::addVertex( vec3 vertex) { void ConvexMeshShape::addEdge(int v1, int v2) { // If the entry for vertex v1 does not exist in the adjacency list if (this.edgesAdjacencyList.count(v1) == 0) { - this.edgesAdjacencyList.add(v1, etk::Set()); + this.edgesAdjacencyList.add(v1, Set()); } // If the entry for vertex v2 does not exist in the adjacency list if (this.edgesAdjacencyList.count(v2) == 0) { - this.edgesAdjacencyList.add(v2, etk::Set()); + this.edgesAdjacencyList.add(v2, Set()); } // Add the edge in the adjacency list this.edgesAdjacencyList[v1].add(v2); @@ -241,8 +241,8 @@ void ConvexMeshShape::setIsEdgesInformationUsed(boolean isEdgesUsed) { this.isEdgesInformationUsed = isEdgesUsed; } -boolean ConvexMeshShape::testPointInside( vec3 localPoint, +boolean ConvexMeshShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { // Use the GJK algorithm to test if the point is inside the convex mesh - return proxyShape->this.body->this.world.this.collisionDetection.this.narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); + return proxyShape.this.body.this.world.collisionDetection.narrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape); } \ No newline at end of file diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.java similarity index 66% rename from src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.java index b9cbf92..4d6b888 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.java @@ -1,23 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include -#include -#include -#include -#include - -namespace ephysics { - class CollisionWorld; /** * @brief 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 @@ -33,25 +15,21 @@ namespace ephysics { * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method * in order to use the edges information for collision detection. */ - class ConvexMeshShape : public ConvexShape { + class ConvexMeshShape extends ConvexShape { protected : - etk::Vector this.vertices; //!< Array with the vertices of the mesh - int this.numberVertices; //!< Number of vertices in the mesh - vec3 this.minBounds; //!< Mesh minimum bounds in the three local x, y and z directions - vec3 this.maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions - boolean this.isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster - etk::Map > this.edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh - /// Private copy-ructor - ConvexMeshShape( ConvexMeshShape shape); - /// Private assignment operator - ConvexMeshShape operator=( ConvexMeshShape shape); + Vector vertices; //!< Array with the vertices of the mesh + int numberVertices; //!< Number of vertices in the mesh + Vector3f minBounds; //!< Mesh minimum bounds in the three local x, y and z directions + Vector3f maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions + boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster + Map > edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh /// Recompute the bounds of the mesh void recalculateBounds(); - void setLocalScaling( vec3 scaling) override; - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; + void setLocalScaling( Vector3f scaling) ; + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ; + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() ; public : /** * @brief Constructor to initialize with an array of 3D vertices. @@ -81,13 +59,13 @@ namespace ephysics { */ ConvexMeshShape(float margin = OBJECTMARGIN); public: - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; + void getLocalBounds(Vector3f min, Vector3f max) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; /** * @brief Add a vertex into the convex mesh * @param vertex Vertex to be added */ - void addVertex( vec3 vertex); + void addVertex( Vector3f vertex); /** * @brief Add an edge into 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 diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp index 873a0d4..f2bc6e7 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp @@ -18,12 +18,12 @@ ephysics::ConvexShape::~ConvexShape() { } -vec3 ephysics::ConvexShape::getLocalSupportPointWithMargin( vec3 direction, void** cachedCollisionData) { +Vector3f ephysics::ConvexShape::getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData) { // Get the support point without margin - vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); + Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); if (this.margin != 0.0f) { // Add the margin to the support point - vec3 unitVec(0.0, -1.0, 0.0); + Vector3f unitVec(0.0, -1.0, 0.0); if (direction.length2() > FLTEPSILON * FLTEPSILON) { unitVec = direction.safeNormalized(); } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.hpp deleted file mode 100644 index 408134c..0000000 --- a/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.hpp +++ /dev/null @@ -1,51 +0,0 @@ -/** @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) - */ -#pragma once - -#include -namespace ephysics { -/** - * @brief It represents a convex collision shape associated with a - * body that is used during the narrow-phase collision detection. - */ -class ConvexShape: public CollisionShape { - protected: - float this.margin; //!< Margin used for the GJK collision detection algorithm - /// Private copy-ructor - ConvexShape( ConvexShape shape) = delete; - /// Private assignment operator - ConvexShape operator=( ConvexShape shape) = delete; - // Return a local support point in a given direction with the object margin - virtual vec3 getLocalSupportPointWithMargin( vec3 direction, void** cachedCollisionData) ; - /// Return a local support point in a given direction without the object margin - virtual vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) =0; - boolean testPointInside( vec3 worldPoint, ProxyShape* proxyShape) override = 0; - public: - /// Constructor - ConvexShape(CollisionShapeType type, float margin); - /// Destructor - virtual ~ConvexShape(); - public: - /** - * @brief Get the current object margin - * @return The margin (in meters) around the collision shape - */ - float getMargin() { - return this.margin; - } - virtual boolean isConvex() override { - return true; - } - friend class GJKAlgorithm; - friend class EPAAlgorithm; -}; - -} - - 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..d25597a --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.java @@ -0,0 +1,33 @@ +package org.atriaSoft.ephysics.collision.shapes; + +/** + * @brief It represents a convex collision shape associated with a + * body that is used during the narrow-phase collision detection. + */ +class ConvexShape: public CollisionShape { + protected: + float margin; //!< Margin used for the GJK collision detection algorithm + // Return a local support point in a given direction with the object margin + Vector3f getLocalSupportPointWithMargin( Vector3f direction, void** cachedCollisionData) ; + /// Return a local support point in a given direction without the object margin + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) =0; + boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape) = 0; + public: + /// Constructor + ConvexShape(CollisionShapeType type, float margin); + public: + /** + * @brief Get the current object margin + * @return The margin (in meters) around the collision shape + */ + float getMargin() { + return this.margin; + } + boolean isConvex() { + return true; + } +}; + +} + + diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp index e91af83..914ab2f 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp @@ -20,11 +20,11 @@ CylinderShape::CylinderShape(float radius, assert(height > 0.0f); } -vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction, +Vector3f CylinderShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { - vec3 supportPoint(0.0, 0.0, 0.0); + Vector3f supportPoint(0.0, 0.0, 0.0); float uDotv = direction.y(); - vec3 w(direction.x(), 0.0, direction.z()); + Vector3f w(direction.x(), 0.0, direction.z()); float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z()); if (lengthW > FLTEPSILON) { if (uDotv < 0.0) { @@ -44,21 +44,21 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction, } boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - vec3 n = ray.point2 - ray.point1; + Vector3f n = ray.point2 - ray.point1; float epsilon = float(0.01); - vec3 p(float(0), -this.halfHeight, float(0)); - vec3 q(float(0), this.halfHeight, float(0)); - vec3 d = q - p; - vec3 m = ray.point1 - p; + Vector3f p(float(0), -this.halfHeight, float(0)); + Vector3f q(float(0), this.halfHeight, float(0)); + Vector3f d = q - p; + Vector3f m = ray.point1 - p; float t; float mDotD = m.dot(d); float nDotD = n.dot(d); float dDotD = d.dot(d); // Test if the segment is outside the cylinder - if (mDotD < 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD < float(0.0)) { + if (mDotD < 0.0f && mDotD + nDotD < float(0.0)) { return false; } - if (mDotD > dDotD hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD > dDotD) { + if (mDotD > dDotD && mDotD + nDotD > dDotD) { return false; } float nDotN = n.dot(n); @@ -67,7 +67,7 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr float k = m.dot(m) - this.radius * this.radius; float c = dDotD * k - mDotD * mDotD; // If the ray is parallel to the cylinder axis - if (etk::abs(a) < epsilon) { + if (abs(a) < epsilon) { // If the origin is outside the surface of the cylinder, we return no hit if (c > 0.0f) { return false; @@ -82,12 +82,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, float(-1), 0); + Vector3f normalDirection(0, float(-1), 0); raycastInfo.worldNormal = normalDirection; return true; } @@ -100,12 +100,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, 1.0f, 0); + Vector3f normalDirection(0, 1.0f, 0); raycastInfo.worldNormal = normalDirection; return true; } @@ -119,7 +119,7 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the smallest root (first intersection along the ray) - float t0 = t = (-b - etk::sqrt(discriminant)) / a; + float t0 = t = (-b - sqrt(discriminant)) / a; // If the intersection is outside the cylinder on "p" endcap side float value = mDotD + t * nDotD; if (value < 0.0f) { @@ -139,12 +139,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, float(-1.0), 0); + Vector3f normalDirection(0, float(-1.0), 0); raycastInfo.worldNormal = normalDirection; return true; } @@ -166,12 +166,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 normalDirection(0, 1.0f, 0); + Vector3f normalDirection(0, 1.0f, 0); raycastInfo.worldNormal = normalDirection; return true; } @@ -182,14 +182,14 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return false; } // Compute the hit information - vec3 localHitPoint = ray.point1 + t * n; - raycastInfo.body = proxyShape->getBody(); + Vector3f localHitPoint = ray.point1 + t * n; + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = localHitPoint; - vec3 v = localHitPoint - p; - vec3 w = (v.dot(d) / d.length2()) * d; - vec3 normalDirection = (localHitPoint - (p + w)); + Vector3f v = localHitPoint - p; + Vector3f w = (v.dot(d) / d.length2()) * d; + Vector3f normalDirection = (localHitPoint - (p + w)); raycastInfo.worldNormal = normalDirection; return true; } @@ -202,17 +202,17 @@ float CylinderShape::getHeight() { return this.halfHeight + this.halfHeight; } -void CylinderShape::setLocalScaling( vec3 scaling) { +void CylinderShape::setLocalScaling( Vector3f scaling) { this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y(); this.radius = (this.radius / this.scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } -sizet CylinderShape::getSizeInBytes() { +long CylinderShape::getSizeInBytes() { return sizeof(CylinderShape); } -void CylinderShape::getLocalBounds(vec3 min, vec3 max) { +void CylinderShape::getLocalBounds(Vector3f min, Vector3f max) { // Maximum bounds max.setX(this.radius + this.margin); max.setY(this.halfHeight + this.margin); @@ -223,7 +223,7 @@ void CylinderShape::getLocalBounds(vec3 min, vec3 max) { min.setZ(min.x()); } -void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void CylinderShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { float height = float(2.0) * this.halfHeight; float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height); tensor.setValue(diag, 0.0, 0.0, 0.0, @@ -231,8 +231,8 @@ void CylinderShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) 0.0, 0.0, diag); } -boolean CylinderShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean CylinderShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.halfHeight - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight); + && localPoint.y() < this.halfHeight + && localPoint.y() > -this.halfHeight); } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.java similarity index 55% rename from src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.java index 24a6f56..00ce5d4 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.java @@ -1,18 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include - -namespace ephysics { /** * @brief It represents a cylinder collision shape around the Y axis * and centered at the origin. The cylinder is defined by its height @@ -28,16 +15,12 @@ namespace ephysics { */ class CylinderShape: public ConvexShape { protected: - float this.radius; //!< Radius of the base - float this.halfHeight; //!< Half height of the cylinder - /// DELETED copy-ructor - CylinderShape( CylinderShape) = delete; - /// DELETED assignment operator - CylinderShape operator=( CylinderShape) = delete; - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; + float radius; //!< Radius of the base + float halfHeight; //!< Half height of the cylinder + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ; + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() ; public: /** * @brief Contructor @@ -56,9 +39,9 @@ namespace ephysics { * @return Height of the cylinder (in meters) */ float getHeight() ; - void setLocalScaling( vec3 scaling) override; - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; + void setLocalScaling( Vector3f scaling) ; + void getLocalBounds(Vector3f min, Vector3f max) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; }; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp index fe24bb5..40d484e 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp @@ -40,25 +40,25 @@ HeightFieldShape::HeightFieldShape(int nbGridColumns, assert(halfHeight >= 0); // Compute the local AABB of the height field if (this.upAxis == 0) { - this.AABB.setMin(vec3(-halfHeight, -this.width * 0.5f, -this.length * float(0.5))); - this.AABB.setMax(vec3(halfHeight, this.width * 0.5f, this.length* float(0.5))); + this.AABB.setMin(Vector3f(-halfHeight, -this.width * 0.5f, -this.length * float(0.5))); + this.AABB.setMax(Vector3f(halfHeight, this.width * 0.5f, this.length* float(0.5))); } else if (this.upAxis == 1) { - this.AABB.setMin(vec3(-this.width * 0.5f, -halfHeight, -this.length * float(0.5))); - this.AABB.setMax(vec3(this.width * 0.5f, halfHeight, this.length * float(0.5))); + this.AABB.setMin(Vector3f(-this.width * 0.5f, -halfHeight, -this.length * float(0.5))); + this.AABB.setMax(Vector3f(this.width * 0.5f, halfHeight, this.length * float(0.5))); } else if (this.upAxis == 2) { - this.AABB.setMin(vec3(-this.width * 0.5f, -this.length * float(0.5), -halfHeight)); - this.AABB.setMax(vec3(this.width * 0.5f, this.length * float(0.5), halfHeight)); + this.AABB.setMin(Vector3f(-this.width * 0.5f, -this.length * float(0.5), -halfHeight)); + this.AABB.setMax(Vector3f(this.width * 0.5f, this.length * float(0.5), halfHeight)); } } -void HeightFieldShape::getLocalBounds(vec3 min, vec3 max) { +void HeightFieldShape::getLocalBounds(Vector3f min, Vector3f max) { min = this.AABB.getMin() * this.scaling; max = this.AABB.getMax() * this.scaling; } void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAABB) { // Compute the non-scaled AABB - vec3 inverseScaling(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), float(1.0) / this.scaling.z()); + Vector3f inverseScaling(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), float(1.0) / this.scaling.z()); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling); // Compute the integer grid coordinates inside the area we need to test for collision int minGridCoords[3]; @@ -89,20 +89,20 @@ void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAA jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1); break; } - assert(iMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMin < this.numberColumns); - assert(iMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMax < this.numberColumns); - assert(jMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMin < this.numberRows); - assert(jMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMax < this.numberRows); + 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 - vec3 p1 = getVertexAt(i, j); - vec3 p2 = getVertexAt(i, j + 1); - vec3 p3 = getVertexAt(i + 1, j); - vec3 p4 = getVertexAt(i + 1, j + 1); + Vector3f p1 = getVertexAt(i, j); + Vector3f p2 = getVertexAt(i, j + 1); + Vector3f p3 = getVertexAt(i + 1, j); + Vector3f p4 = getVertexAt(i + 1, j + 1); // Generate the first triangle for the current grid rectangle - vec3 trianglePoints[3] = {p1, p2, p3}; + Vector3f trianglePoints[3] = {p1, p2, p3}; // Test collision against the first triangle callback.testTriangle(trianglePoints); // Generate the second triangle for the current grid rectangle @@ -117,14 +117,14 @@ void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAA void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, AABB aabbToCollide) { // Clamp the min/max coords of the AABB to collide inside the height field AABB - vec3 minPoint = etk::max(aabbToCollide.getMin(), this.AABB.getMin()); - minPoint = etk::min(minPoint, this.AABB.getMax()); - vec3 maxPoint = etk::min(aabbToCollide.getMax(), this.AABB.getMax()); - maxPoint = etk::max(maxPoint, this.AABB.getMin()); + Vector3f minPoint = max(aabbToCollide.getMin(), this.AABB.getMin()); + minPoint = min(minPoint, this.AABB.getMax()); + Vector3f maxPoint = min(aabbToCollide.getMax(), this.AABB.getMax()); + maxPoint = 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] - vec3 translateVec = this.AABB.getExtent() * 0.5f; + Vector3f translateVec = this.AABB.getExtent() * 0.5f; minPoint += translateVec; maxPoint += translateVec; // Convert the floating min/max coords of the AABB into closest integer @@ -144,27 +144,27 @@ boolean HeightFieldShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* PROFILE("HeightFieldShape::raycast()"); TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); // Compute the AABB for the ray - vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); - AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd)); + Vector3f rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); + AABB rayAABB(min(ray.point1, rayEnd), max(ray.point1, rayEnd)); testAllTriangles(triangleCallback, rayAABB); return triangleCallback.getIsHit(); } -vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) { +Vector3f HeightFieldShape::getVertexAt(int xxx, int yyy) { // Get the height value float height = getHeightAt(xxx, yyy); // Height values origin float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight; - vec3 vertex; + Vector3f vertex; switch (this.upAxis) { case 0: - vertex = vec3(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * float(0.5) + yyy); + vertex = Vector3f(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * float(0.5) + yyy); break; case 1: - vertex = vec3(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * float(0.5) + yyy); + vertex = Vector3f(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * float(0.5) + yyy); break; case 2: - vertex = vec3(-this.width * 0.5f + xxx, -this.length * float(0.5) + yyy, heightOrigin + height); + vertex = Vector3f(-this.width * 0.5f + xxx, -this.length * float(0.5) + yyy, heightOrigin + height); break; default: assert(false); @@ -173,7 +173,7 @@ vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) { return vertex * this.scaling; } -void TriangleOverlapCallback::testTriangle( vec3* trianglePoints) { +void TriangleOverlapCallback::testTriangle( Vector3f* trianglePoints) { // Create a triangle collision shape float margin = this.heightFieldShape.getTriangleMargin(); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); @@ -183,7 +183,7 @@ void TriangleOverlapCallback::testTriangle( vec3* trianglePoints) { boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape); // If the ray hit the collision shape if ( isTriangleHit - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= this.smallestHitFraction) { + && raycastInfo.hitFraction <= this.smallestHitFraction) { assert(raycastInfo.hitFraction >= 0.0f); this.raycastInfo.body = raycastInfo.body; this.raycastInfo.proxyShape = raycastInfo.proxyShape; @@ -209,11 +209,11 @@ HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() { return this.heightDataType; } -sizet HeightFieldShape::getSizeInBytes() { +long HeightFieldShape::getSizeInBytes() { return sizeof(HeightFieldShape); } -void HeightFieldShape::setLocalScaling( vec3 scaling) { +void HeightFieldShape::setLocalScaling( Vector3f scaling) { CollisionShape::setLocalScaling(scaling); } @@ -235,7 +235,7 @@ int HeightFieldShape::computeIntegerGridValue(float value) { return (value < 0.0f) ? value - 0.5f : value + 0.5f; } -void HeightFieldShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void HeightFieldShape::computeLocalInertiaTensor(Matrix3f tensor, 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, diff --git a/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.java similarity index 63% rename from src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.java index 2ce2e77..3c8a7d0 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.java @@ -1,30 +1,17 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include -#include - -namespace ephysics { class HeightFieldShape; /** * @brief This class is used for testing AABB and triangle overlap for raycasting */ - class TriangleOverlapCallback : public TriangleCallback { + class TriangleOverlapCallback extends TriangleCallback { protected: - Ray this.ray; - ProxyShape* this.proxyShape; - RaycastInfo this.raycastInfo; - boolean this.isHit; - float this.smallestHitFraction; - HeightFieldShape this.heightFieldShape; + Ray ray; + ProxyShape* proxyShape; + RaycastInfo raycastInfo; + boolean isHit; + float smallestHitFraction; + HeightFieldShape heightFieldShape; public: TriangleOverlapCallback( Ray ray, ProxyShape* proxyShape, @@ -41,7 +28,7 @@ namespace ephysics { return this.isHit; } /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle( vec3* trianglePoints); + void testTriangle( Vector3f* trianglePoints); }; @@ -55,7 +42,7 @@ namespace ephysics { * 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. */ - class HeightFieldShape : public ConcaveShape { + class HeightFieldShape extends ConcaveShape { public: /** * @brief Data type for the height data of the height field @@ -66,32 +53,28 @@ namespace ephysics { HEIGHTINTTYPE }; protected: - int this.numberColumns; //!< Number of columns in the grid of the height field - int this.numberRows; //!< Number of rows in the grid of the height field - float this.width; //!< Height field width - float this.length; //!< Height field length - float this.minHeight; //!< Minimum height of the height field - float this.maxHeight; //!< Maximum height of the height field - int this.upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z) - float this.integerHeightScale; //!< Height values scale for height field with integer height values - HeightDataType this.heightDataType; //!< Data type of the height values - void* this.heightFieldData; //!< Array of data with all the height values of the height field - AABB this.AABB; //!< Local AABB of the height field (without scaling) - /// DELETED copy-ructor - HeightFieldShape( HeightFieldShape) = delete; - /// DELETED assignment operator - HeightFieldShape operator=( HeightFieldShape) = delete; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; + int numberColumns; //!< Number of columns in the grid of the height field + int numberRows; //!< Number of rows in the grid of the height field + float width; //!< Height field width + float length; //!< Height field length + float minHeight; //!< Minimum height of the height field + float maxHeight; //!< Maximum height of the height field + int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z) + float integerHeightScale; //!< Height values scale for height field with integer height values + HeightDataType heightDataType; //!< Data type of the height values + void* heightFieldData; //!< Array of data with all the height values of the height field + AABB AABB; //!< Local AABB of the height field (without scaling) + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() ; /// Insert all the triangles into the dynamic AABB tree void initBVHTree(); /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// given the start vertex index pointer of the triangle. void getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, - vec3* outTriangleVertices) ; + Vector3f* outTriangleVertices) ; /// Return the vertex (local-coordinates) of the height field at a given (x,y) position - vec3 getVertexAt(int x, int y) ; + Vector3f getVertexAt(int x, int y) ; /// Return the height of a given (x,y) point in the height field float getHeightAt(int x, int y) ; /// Return the closest inside integer grid value of a given floating grid value @@ -123,10 +106,10 @@ namespace ephysics { int getNbColumns() ; /// Return the type of height value in the height field HeightDataType getHeightDataType() ; - void getLocalBounds(vec3 min, vec3 max) override; - void setLocalScaling( vec3 scaling) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; - virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) override; + void getLocalBounds(Vector3f min, Vector3f max) ; + void setLocalScaling( Vector3f scaling) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; + void testAllTriangles(TriangleCallback callback, AABB localAABB) ; friend class ConvexTriangleAABBOverlapCallback; friend class ConcaveMeshRaycastCallback; }; diff --git a/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp index cf5b975..107ce3b 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp @@ -18,27 +18,27 @@ SphereShape::SphereShape(float radius): assert(radius > 0.0f); } -void SphereShape::setLocalScaling( vec3 scaling) { +void SphereShape::setLocalScaling( Vector3f scaling) { this.margin = (this.margin / this.scaling.x()) * scaling.x(); CollisionShape::setLocalScaling(scaling); } -void SphereShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void SphereShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { float diag = 0.4f * mass * this.margin * this.margin; tensor.setValue(diag, 0.0f, 0.0f, 0.0f, diag, 0.0f, 0.0f, 0.0f, diag); } -void SphereShape::computeAABB(AABB aabb, etk::Transform3D transform) { +void SphereShape::computeAABB(AABB aabb, Transform3D transform) { // Get the local extents in x,y and z direction - vec3 extents(this.margin, this.margin, this.margin); + Vector3f extents(this.margin, this.margin, this.margin); // Update the AABB with the new minimum and maximum coordinates aabb.setMin(transform.getPosition() - extents); aabb.setMax(transform.getPosition() + extents); } -void SphereShape::getLocalBounds(vec3 min, vec3 max) { +void SphereShape::getLocalBounds(Vector3f min, Vector3f max) { // Maximum bounds max.setX(this.margin); max.setY(this.margin); @@ -50,13 +50,13 @@ void SphereShape::getLocalBounds(vec3 min, vec3 max) { } boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { - vec3 m = ray.point1; + Vector3f m = ray.point1; float c = m.dot(m) - this.margin * this.margin; // If the origin of the ray is inside the sphere, we return no intersection if (c < 0.0f) { return false; } - vec3 rayDirection = ray.point2 - ray.point1; + Vector3f rayDirection = ray.point2 - ray.point1; 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 intersection @@ -72,13 +72,13 @@ boolean SphereShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* prox return false; } // Compute the solution "t" closest to the origin - float t = -b - etk::sqrt(discriminant); + float t = -b - sqrt(discriminant); assert(t >= 0.0f); // If the hit point is withing the segment ray fraction if (t < ray.maxFraction * raySquareLength) { // Compute the intersection information t /= raySquareLength; - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.hitFraction = t; raycastInfo.worldPoint = ray.point1 + t * rayDirection; diff --git a/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.hpp deleted file mode 100644 index d6d3d7b..0000000 --- a/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.hpp +++ /dev/null @@ -1,57 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include -#include - -namespace ephysics { - - /** - * @brief 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. - */ - class SphereShape : public ConvexShape { - protected : - SphereShape( SphereShape shape); - SphereShape operator=( SphereShape shape) = delete; - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override { - return vec3(0.0, 0.0, 0.0); - } - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override { - return (localPoint.length2() < this.margin * this.margin); - } - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override { - return sizeof(SphereShape); - } - public : - /** - * @brief Constructor - * @param[in] radius Radius of the sphere (in meters) - */ - SphereShape(float radius); - /** - * @brief Get the radius of the sphere - * @return Radius of the sphere (in meters) - */ - float getRadius() { - return this.margin; - } - void setLocalScaling( vec3 scaling) override; - void getLocalBounds(vec3 min, vec3 max) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; - void computeAABB(AABB aabb, etk::Transform3D transform) override; - }; - -} 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..aed7c0d --- /dev/null +++ b/src/org/atriaSoft/ephysics/collision/shapes/SphereShape.java @@ -0,0 +1,42 @@ +package org.atriaSoft.ephysics.collision.shapes; + + /** + * @brief 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. + */ + class SphereShape extends ConvexShape { + protected : + SphereShape( SphereShape shape); + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { + return Vector3f(0.0, 0.0, 0.0); + } + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { + return (localPoint.length2() < this.margin * this.margin); + } + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() { + return sizeof(SphereShape); + } + public : + /** + * @brief Constructor + * @param[in] radius Radius of the sphere (in meters) + */ + SphereShape(float radius); + /** + * @brief Get the radius of the sphere + * @return Radius of the sphere (in meters) + */ + float getRadius() { + return this.margin; + } + void setLocalScaling( Vector3f scaling) ; + void getLocalBounds(Vector3f min, Vector3f max) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; + void computeAABB(AABB aabb, Transform3D transform) ; + }; + +} diff --git a/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp b/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp index ae734f3..153f4d2 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp @@ -14,7 +14,7 @@ // TODO: REMOVE this... using namespace ephysics; -TriangleShape::TriangleShape( vec3 point1, vec3 point2, vec3 point3, float margin) +TriangleShape::TriangleShape( Vector3f point1, Vector3f point2, Vector3f point3, float margin) : ConvexShape(TRIANGLE, margin) { this.points[0] = point1; this.points[1] = point2; @@ -24,13 +24,13 @@ TriangleShape::TriangleShape( vec3 point1, vec3 point2, vec3 point3, float mar boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { PROFILE("TriangleShape::raycast()"); - vec3 pq = ray.point2 - ray.point1; - vec3 pa = this.points[0] - ray.point1; - vec3 pb = this.points[1] - ray.point1; - vec3 pc = this.points[2] - ray.point1; + Vector3f pq = ray.point2 - ray.point1; + Vector3f pa = this.points[0] - ray.point1; + Vector3f pb = this.points[1] - ray.point1; + Vector3f pc = this.points[2] - ray.point1; // Test if the line PQ is inside the eges BC, CA and AB. We use the triple // product for this test. - vec3 m = pq.cross(pc); + Vector3f m = pq.cross(pc); float u = pb.dot(m); if (this.raycastTestType == FRONT) { if (u < 0.0f) { @@ -71,8 +71,8 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr } // If the line PQ is in the triangle plane (case where u=v=w=0) if ( approxEqual(u, 0) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(v, 0) - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(w, 0)) { + && approxEqual(v, 0) + && approxEqual(w, 0)) { return false; } // Compute the barycentric coordinates (u, v, w) to determine the @@ -82,17 +82,17 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr v *= denom; w *= denom; // Compute the local hit point using the barycentric coordinates - vec3 localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2]; + Vector3f localHitPoint = u * this.points[0] + v * this.points[1] + w * this.points[2]; float hitFraction = (localHitPoint - ray.point1).length() / pq.length(); if ( hitFraction < 0.0f || hitFraction > ray.maxFraction) { return false; } - vec3 localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]); + Vector3f localHitNormal = (this.points[1] - this.points[0]).cross(this.points[2] - this.points[0]); if (localHitNormal.dot(pq) > 0.0f) { localHitNormal = -localHitNormal; } - raycastInfo.body = proxyShape->getBody(); + raycastInfo.body = proxyShape.getBody(); raycastInfo.proxyShape = proxyShape; raycastInfo.worldPoint = localHitPoint; raycastInfo.hitFraction = hitFraction; @@ -100,49 +100,49 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr return true; } -sizet TriangleShape::getSizeInBytes() { +long TriangleShape::getSizeInBytes() { return sizeof(TriangleShape); } -vec3 TriangleShape::getLocalSupportPointWithoutMargin( vec3 direction, +Vector3f TriangleShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) { - vec3 dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2])); + Vector3f dotProducts(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2])); return this.points[dotProducts.getMaxAxis()]; } -void TriangleShape::getLocalBounds(vec3 min, vec3 max) { - vec3 xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x()); - vec3 yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y()); - vec3 zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z()); +void TriangleShape::getLocalBounds(Vector3f min, Vector3f max) { + Vector3f xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x()); + Vector3f yAxis(this.points[0].y(), this.points[1].y(), this.points[2].y()); + Vector3f zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z()); min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); - min -= vec3(this.margin, this.margin, this.margin); - max += vec3(this.margin, this.margin, this.margin); + min -= Vector3f(this.margin, this.margin, this.margin); + max += Vector3f(this.margin, this.margin, this.margin); } -void TriangleShape::setLocalScaling( vec3 scaling) { +void TriangleShape::setLocalScaling( Vector3f scaling) { this.points[0] = (this.points[0] / this.scaling) * scaling; this.points[1] = (this.points[1] / this.scaling) * scaling; this.points[2] = (this.points[2] / this.scaling) * scaling; CollisionShape::setLocalScaling(scaling); } -void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { +void TriangleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) { tensor.setZero(); } -void TriangleShape::computeAABB(AABB aabb, etk::Transform3D transform) { - vec3 worldPoint1 = transform * this.points[0]; - vec3 worldPoint2 = transform * this.points[1]; - vec3 worldPoint3 = transform * this.points[2]; - vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); - vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); - vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); - aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); - aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); +void TriangleShape::computeAABB(AABB aabb, Transform3D transform) { + Vector3f worldPoint1 = transform * this.points[0]; + Vector3f worldPoint2 = transform * this.points[1]; + Vector3f worldPoint3 = transform * this.points[2]; + Vector3f xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); + Vector3f yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); + Vector3f zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); + aabb.setMin(Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); + aabb.setMax(Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); } -boolean TriangleShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { +boolean TriangleShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) { return false; } @@ -155,9 +155,9 @@ void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) { this.raycastTestType = testType; } -vec3 TriangleShape::getVertex(int index) { +Vector3f TriangleShape::getVertex(int index) { assert( index >= 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); + && index < 3); return this.points[index]; } diff --git a/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.hpp b/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.java similarity index 54% rename from src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.hpp rename to src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.java index 867cdde..d74985a 100644 --- a/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.hpp +++ b/src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.java @@ -1,17 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.collision.shapes; -#include -#include - -namespace ephysics { /** * @brief Raycast test side for the triangle */ @@ -27,16 +15,16 @@ namespace ephysics { */ class TriangleShape: public ConvexShape { protected: - vec3 this.points[3]; //!< Three points of the triangle - TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) + Vector3f this.points[3]; //!< Three points of the triangle + TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) /// Private copy-ructor TriangleShape( TriangleShape shape); /// Private assignment operator TriangleShape operator=( TriangleShape shape); - vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; - boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; - boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; - sizet getSizeInBytes() override; + Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ; + boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ; + boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ; + long getSizeInBytes() ; public: /** * @brief Constructor @@ -45,14 +33,14 @@ namespace ephysics { * @param point3 Third point of the triangle * @param margin The collision margin (in meters) around the collision shape */ - TriangleShape( vec3 point1, - vec3 point2, - vec3 point3, + TriangleShape( Vector3f point1, + Vector3f point2, + Vector3f point3, float margin = OBJECTMARGIN); - void getLocalBounds(vec3 min, vec3 max) override; - void setLocalScaling( vec3 scaling) override; - void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; - void computeAABB(AABB aabb, etk::Transform3D transform) override; + void getLocalBounds(Vector3f min, Vector3f max) ; + void setLocalScaling( Vector3f scaling) ; + void computeLocalInertiaTensor(Matrix3f tensor, float mass) ; + void computeAABB(AABB aabb, Transform3D transform) ; /// Return the raycast test type (front, back, front-back) TriangleRaycastSide getRaycastTestType() ; /** @@ -64,7 +52,7 @@ namespace ephysics { * @brief Return the coordinates of a given vertex of the triangle * @param[in] index Index (0 to 2) of a vertex of the triangle */ - vec3 getVertex(int index) ; + Vector3f getVertex(int index) ; friend class ConcaveMeshRaycastCallback; friend class TriangleOverlapCallback; }; diff --git a/src/org/atriaSoft/ephysics/configuration.hpp b/src/org/atriaSoft/ephysics/configuration.hpp deleted file mode 100644 index e1c1a15..0000000 --- a/src/org/atriaSoft/ephysics/configuration.hpp +++ /dev/null @@ -1,99 +0,0 @@ -/** @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) - */ -#pragma once - -// Libraries -#include -#include - -/// Namespace ephysics -namespace ephysics { - // ------------------- Type definitions ------------------- // - typedef long long; - typedef etk::Pair longpair; - - // ------------------- Enumerations ------------------- // - - /// Position correction technique used in the raint solver (for joints). - /// BAUMGARTEJOINTS : Faster but can be innacurate in some situations. - /// NONLINEARGAUSSSEIDEL : Slower but more precise. This is the option used by default. - enum JointsPositionCorrectionTechnique {BAUMGARTEJOINTS, NONLINEARGAUSSSEIDEL}; - - /// Position correction technique used in the contact solver (for contacts) - /// BAUMGARTECONTACTS : 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). - /// SPLITIMPULSES : A bit slower but the error correction factor is not added to the - /// bodies momentum. This is the option used by default. - enum ContactsPositionCorrectionTechnique {BAUMGARTECONTACTS, SPLITIMPULSES}; - - // ------------------- Constants ------------------- // - /// Pi ant - float PI = float(3.14159265); - - /// 2*Pi ant - float PITIMES2 = float(6.28318530); - - /// Default friction coefficient for a rigid body - float DEFAULTFRICTIONCOEFFICIENT = float(0.3); - - /// Default bounciness factor for a rigid body - float DEFAULTBOUNCINESS = 0.5f; - - /// Default rolling resistance - float DEFAULTROLLINGRESISTANCE = 0.0f; - - /// True if the spleeping technique is enabled - boolean SPLEEPINGENABLED = true; - - /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) - float OBJECTMARGIN = float(0.04); - - /// Distance threshold for two contact points for a valid persistent contact (in meters) - float PERSISTENTCONTACTDISTTHRESHOLD = float(0.03); - - /// Velocity threshold for contact velocity restitution - float RESTITUTIONVELOCITYTHRESHOLD = 1.0f; - - /// Number of iterations when solving the velocity raints of the Sequential Impulse technique - int DEFAULTVELOCITYSOLVERNBITERATIONS = 10; - - /// Number of iterations when solving the position raints of the Sequential Impulse technique - int DEFAULTPOSITIONSOLVERNBITERATIONS = 5; - - /// Time (in seconds) that a body must stay still to be considered sleeping - float DEFAULTTIMEBEFORESLEEP = 1.0f; - - /// A body with a linear velocity smaller than the sleep linear velocity (in m/s) - /// might enter sleeping mode. - float DEFAULTSLEEPLINEARVELOCITY = float(0.02); - - /// A body with angular velocity smaller than the sleep angular velocity (in rad/s) - /// might enter sleeping mode - float DEFAULTSLEEPANGULARVELOCITY = float(3.0 * (PI / 180.0)); - - /// 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 - float DYNAMICTREEAABBGAP = float(0.1); - - /// 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. - float DYNAMICTREEAABBLINGAPMULTIPLIER = float(1.7); - - /// Maximum number of contact manifolds in an overlapping pair that involves two - /// convex collision shapes. - int NBMAXCONTACTMANIFOLDSCONVEXSHAPE = 1; - - /// Maximum number of contact manifolds in an overlapping pair that involves at - /// least one concave collision shape. - int NBMAXCONTACTMANIFOLDSCONCAVESHAPE = 3; - -} diff --git a/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp b/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp index 807a974..df81591 100644 --- a/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp @@ -17,41 +17,41 @@ using namespace ephysics; // Constructor BallAndSocketJoint::BallAndSocketJoint( BallAndSocketJointInfo jointInfo) - : Joint(jointInfo), this.impulse(vec3(0, 0, 0)) { + : Joint(jointInfo), this.impulse(Vector3f(0, 0, 0)) { // Compute the local-space anchor point for each body - this.localAnchorPointBody1 = this.body1->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace; - this.localAnchorPointBody2 = this.body2->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace; + this.localAnchorPointBody1 = this.body1.getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + this.localAnchorPointBody2 = this.body2.getTransform().getInverse() * jointInfo.anchorPointWorldSpace; } // Initialize before solving the raint void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Initialize the bodies index in the velocity array - this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; - this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second; + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second; // Get the bodies center of mass and orientations - vec3 x1 = this.body1->this.centerOfMassWorld; - vec3 x2 = this.body2->this.centerOfMassWorld; - etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); - etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); + Vector3f x1 = this.body1.this.centerOfMassWorld; + Vector3f x2 = this.body2.this.centerOfMassWorld; + Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); // Get the inertia tensor of bodies - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; this.r2World = orientationBody2 * this.localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse; + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + @@ -59,7 +59,7 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) // Compute the inverse mass matrix K^-1 this.inverseMassMatrix.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrix = massMatrix.getInverse(); } @@ -82,24 +82,24 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Compute the impulse P=J^T * lambda for the body 1 - vec3 linearImpulseBody1 = -this.impulse; - vec3 angularImpulseBody1 = this.impulse.cross(this.r1World); + Vector3f linearImpulseBody1 = -this.impulse; + Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World); // Apply the impulse to the body 1 - v1 += this.body1->this.massInverse * linearImpulseBody1; + v1 += this.body1.this.massInverse * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - vec3 angularImpulseBody2 = -this.impulse.cross(this.r2World); + Vector3f angularImpulseBody2 = -this.impulse.cross(this.r2World); // Apply the impulse to the body to the body 2 - v2 += this.body2->this.massInverse * this.impulse; + v2 += this.body2.this.massInverse * this.impulse; w2 += this.i2 * angularImpulseBody2; } @@ -107,31 +107,31 @@ void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) { void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Compute J*v - vec3 Jv = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); + Vector3f Jv = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); // Compute the Lagrange multiplier lambda - vec3 deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector); + Vector3f deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector); this.impulse += deltaLambda; // Compute the impulse P=J^T * lambda for the body 1 - vec3 linearImpulseBody1 = -deltaLambda; - vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World); + Vector3f linearImpulseBody1 = -deltaLambda; + Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World); // Apply the impulse to the body 1 - v1 += this.body1->this.massInverse * linearImpulseBody1; + v1 += this.body1.this.massInverse * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 - vec3 angularImpulseBody2 = -deltaLambda.cross(this.r2World); + Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World); // Apply the impulse to the body 2 - v2 += this.body2->this.massInverse * deltaLambda; + v2 += this.body2.this.massInverse * deltaLambda; w2 += this.i2 * angularImpulseBody2; } @@ -143,70 +143,70 @@ void BallAndSocketJoint::solvePositionConstraint( ConstraintSolverData raintSolv if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; // Get the bodies center of mass and orientations - vec3 x1 = raintSolverData.positions[this.indexBody1]; - vec3 x2 = raintSolverData.positions[this.indexBody2]; - etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; - etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + Vector3f x1 = raintSolverData.positions[this.indexBody1]; + Vector3f x2 = raintSolverData.positions[this.indexBody2]; + Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + Quaternion q2 = raintSolverData.orientations[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Recompute the inverse inertia tensors - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; this.r2World = q2 * this.localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints float inverseMassBodies = inverseMassBody1 + inverseMassBody2; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); this.inverseMassMatrix.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrix = massMatrix.getInverse(); } // Compute the raint error (value of the C(x) function) - vec3 raintError = (x2 + this.r2World - x1 - this.r1World); + Vector3f raintError = (x2 + this.r2World - x1 - 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. - vec3 lambda = this.inverseMassMatrix * (-raintError); + Vector3f lambda = this.inverseMassMatrix * (-raintError); // Compute the impulse of body 1 - vec3 linearImpulseBody1 = -lambda; - vec3 angularImpulseBody1 = lambda.cross(this.r1World); + Vector3f linearImpulseBody1 = -lambda; + Vector3f angularImpulseBody1 = lambda.cross(this.r1World); // Compute the pseudo velocity of body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body center of mass and orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - vec3 angularImpulseBody2 = -lambda.cross(this.r2World); + Vector3f angularImpulseBody2 = -lambda.cross(this.r2World); // Compute the pseudo velocity of body 2 - vec3 v2 = inverseMassBody2 * lambda; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * lambda; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } diff --git a/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.hpp b/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.hpp deleted file mode 100644 index 5f5bd38..0000000 --- a/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.hpp +++ /dev/null @@ -1,68 +0,0 @@ -/** @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) - */ -#pragma once - -#include -#include - -namespace ephysics { - /** - * @brief 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. - */ - struct BallAndSocketJointInfo : public JointInfo { - public : - vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) - /** - * @brief 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 - */ - BallAndSocketJointInfo(RigidBody* rigidBody1, - RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace): - JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), - this.anchorPointWorldSpace(initAnchorPointWorldSpace) { - - } - }; - /** - * @brief Represents a ball-and-socket joint that allows arbitrary rotation - * between two bodies. This joint has three degrees of freedom. It can be used to - * create a chain of bodies for instance. - */ - class BallAndSocketJoint : public Joint { - private: - static float BETA; //!< Beta value for the bias factor of position correction - vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) - vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) - vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space - vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space - etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) - vec3 this.biasVector; //!< Bias vector for the raint - etk::Matrix3x3 this.inverseMassMatrix; //!< Inverse mass matrix K=JM^-1J^-t of the raint - vec3 this.impulse; //!< Accumulated impulse - /// Private copy-ructor - BallAndSocketJoint( BallAndSocketJoint raint); - /// Private assignment operator - BallAndSocketJoint operator=( BallAndSocketJoint raint); - sizet getSizeInBytes() override { - return sizeof(BallAndSocketJoint); - } - void initBeforeSolve( ConstraintSolverData raintSolverData) override; - void warmstart( ConstraintSolverData raintSolverData) override; - void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; - void solvePositionConstraint( ConstraintSolverData raintSolverData) override; - public: - /// Constructor - BallAndSocketJoint( BallAndSocketJointInfo jointInfo); - }; -} diff --git a/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.java b/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.java new file mode 100644 index 0000000..823d522 --- /dev/null +++ b/src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.java @@ -0,0 +1,52 @@ +package org.atriaSoft.ephysics.constraint; + + /** + * @brief 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. + */ + struct BallAndSocketJointInfo extends JointInfo { + public : + Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + /** + * @brief 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 + */ + BallAndSocketJointInfo(RigidBody* rigidBody1, + RigidBody* rigidBody2, + Vector3f initAnchorPointWorldSpace): + JointInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), + this.anchorPointWorldSpace(initAnchorPointWorldSpace) { + + } + }; + /** + * @brief Represents a ball-and-socket joint that allows arbitrary rotation + * between two bodies. This joint has three degrees of freedom. It can be used to + * create a chain of bodies for instance. + */ + class BallAndSocketJoint extends Joint { + private: + static float BETA; //!< Beta value for the bias factor of position correction + Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space + Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space + Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates) + Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates) + Vector3f biasVector; //!< Bias vector for the raint + Matrix3f inverseMassMatrix; //!< Inverse mass matrix K=JM^-1J^-t of the raint + Vector3f impulse; //!< Accumulated impulse + long getSizeInBytes() { + return sizeof(BallAndSocketJoint); + } + void initBeforeSolve( ConstraintSolverData raintSolverData) ; + void warmstart( ConstraintSolverData raintSolverData) ; + void solveVelocityConstraint( ConstraintSolverData raintSolverData) ; + void solvePositionConstraint( ConstraintSolverData raintSolverData) ; + public: + /// Constructor + BallAndSocketJoint( BallAndSocketJointInfo jointInfo); + }; +} diff --git a/src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp b/src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp index 6cf81fc..a0c6297 100644 --- a/src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp @@ -14,22 +14,22 @@ using namespace std; // Constructor ContactPoint::ContactPoint( ContactPointInfo contactInfo): - this.body1(contactInfo.shape1->getBody()), - this.body2(contactInfo.shape2->getBody()), + this.body1(contactInfo.shape1.getBody()), + this.body2(contactInfo.shape2.getBody()), this.normal(contactInfo.normal), this.penetrationDepth(contactInfo.penetrationDepth), this.localPointOnBody1(contactInfo.localPoint1), this.localPointOnBody2(contactInfo.localPoint2), - this.worldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * - contactInfo.shape1->getLocalToBodyTransform() * + this.worldPointOnBody1(contactInfo.shape1.getBody().getTransform() * + contactInfo.shape1.getLocalToBodyTransform() * contactInfo.localPoint1), - this.worldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * - contactInfo.shape2->getLocalToBodyTransform() * + this.worldPointOnBody2(contactInfo.shape2.getBody().getTransform() * + contactInfo.shape2.getLocalToBodyTransform() * contactInfo.localPoint2), this.isRestingContact(false) { - this.frictionVectors[0] = vec3(0, 0, 0); - this.frictionVectors[1] = vec3(0, 0, 0); + this.frictionVectors[0] = Vector3f(0, 0, 0); + this.frictionVectors[1] = Vector3f(0, 0, 0); assert(this.penetrationDepth >= 0.0); @@ -51,32 +51,32 @@ CollisionBody* ContactPoint::getBody2() { } // Return the normal vector of the contact -vec3 ContactPoint::getNormal() { +Vector3f ContactPoint::getNormal() { return this.normal; } // Set the penetration depth of the contact void ContactPoint::setPenetrationDepth(float penetrationDepth) { - this->this.penetrationDepth = penetrationDepth; + this.this.penetrationDepth = penetrationDepth; } // Return the contact point on body 1 -vec3 ContactPoint::getLocalPointOnBody1() { +Vector3f ContactPoint::getLocalPointOnBody1() { return this.localPointOnBody1; } // Return the contact point on body 2 -vec3 ContactPoint::getLocalPointOnBody2() { +Vector3f ContactPoint::getLocalPointOnBody2() { return this.localPointOnBody2; } // Return the contact world point on body 1 -vec3 ContactPoint::getWorldPointOnBody1() { +Vector3f ContactPoint::getWorldPointOnBody1() { return this.worldPointOnBody1; } // Return the contact world point on body 2 -vec3 ContactPoint::getWorldPointOnBody2() { +Vector3f ContactPoint::getWorldPointOnBody2() { return this.worldPointOnBody2; } @@ -96,7 +96,7 @@ float ContactPoint::getFrictionImpulse2() { } // Return the cached rolling resistance impulse -vec3 ContactPoint::getRollingResistanceImpulse() { +Vector3f ContactPoint::getRollingResistanceImpulse() { return this.rollingResistanceImpulse; } @@ -116,17 +116,17 @@ void ContactPoint::setFrictionImpulse2(float impulse) { } // Set the cached rolling resistance impulse -void ContactPoint::setRollingResistanceImpulse( vec3 impulse) { +void ContactPoint::setRollingResistanceImpulse( Vector3f impulse) { this.rollingResistanceImpulse = impulse; } // Set the contact world point on body 1 -void ContactPoint::setWorldPointOnBody1( vec3 worldPoint) { +void ContactPoint::setWorldPointOnBody1( Vector3f worldPoint) { this.worldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 -void ContactPoint::setWorldPointOnBody2( vec3 worldPoint) { +void ContactPoint::setWorldPointOnBody2( Vector3f worldPoint) { this.worldPointOnBody2 = worldPoint; } @@ -141,22 +141,22 @@ void ContactPoint::setIsRestingContact(boolean isRestingContact) { } // Get the first friction vector -vec3 ContactPoint::getFrictionVector1() { +Vector3f ContactPoint::getFrictionVector1() { return this.frictionVectors[0]; } // Set the first friction vector -void ContactPoint::setFrictionVector1( vec3 frictionVector1) { +void ContactPoint::setFrictionVector1( Vector3f frictionVector1) { this.frictionVectors[0] = frictionVector1; } // Get the second friction vector -vec3 ContactPoint::getFrictionvec2() { +Vector3f ContactPoint::getFrictionvec2() { return this.frictionVectors[1]; } // Set the second friction vector -void ContactPoint::setFrictionvec2( vec3 frictionvec2) { +void ContactPoint::setFrictionvec2( Vector3f frictionvec2) { this.frictionVectors[1] = frictionvec2; } @@ -166,6 +166,6 @@ float ContactPoint::getPenetrationDepth() { } // Return the number of bytes used by the contact point -sizet ContactPoint::getSizeInBytes() { +long ContactPoint::getSizeInBytes() { return sizeof(ContactPoint); } diff --git a/src/org/atriaSoft/ephysics/constraint/ContactPoint.hpp b/src/org/atriaSoft/ephysics/constraint/ContactPoint.java similarity index 52% rename from src/org/atriaSoft/ephysics/constraint/ContactPoint.hpp rename to src/org/atriaSoft/ephysics/constraint/ContactPoint.java index 2e9d150..6bfdce1 100644 --- a/src/org/atriaSoft/ephysics/constraint/ContactPoint.hpp +++ b/src/org/atriaSoft/ephysics/constraint/ContactPoint.java @@ -1,20 +1,4 @@ -/** @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) - */ -#pragma once - -#include -#include -#include -#include -#include - -namespace ephysics { +package org.atriaSoft.ephysics.constraint; /** * @brief This structure contains informations about a collision contact @@ -28,18 +12,18 @@ namespace ephysics { ProxyShape* shape2; //!< Second proxy shape of the contact CollisionShape* collisionShape1; //!< First collision shape CollisionShape* collisionShape2; //!< Second collision shape - vec3 normal; //!< Normalized normal vector of the collision contact in world space + Vector3f normal; //!< Normalized normal vector of the collision contact in world space float penetrationDepth; //!< Penetration depth of the contact - vec3 localPoint1; //!< Contact point of body 1 in local space of body 1 - vec3 localPoint2; //!< Contact point of body 2 in local space of body 2 + Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1 + Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2 ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, CollisionShape* collShape1, CollisionShape* collShape2, - vec3 normal, + Vector3f normal, float penetrationDepth, - vec3 localPoint1, - vec3 localPoint2): + Vector3f localPoint1, + Vector3f localPoint2): shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), @@ -55,7 +39,7 @@ namespace ephysics { shape2(null), collisionShape1(null), collisionShape2(null) { - // TODO: add it for etk::Vector + // TODO: add it for Vector } }; @@ -65,45 +49,39 @@ namespace ephysics { */ class ContactPoint { private : - CollisionBody* this.body1; //!< First rigid body of the contact - CollisionBody* this.body2; //!< Second rigid body of the contact - vec3 this.normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space - float this.penetrationDepth; //!< Penetration depth - vec3 this.localPointOnBody1; //!< Contact point on body 1 in local space of body 1 - vec3 this.localPointOnBody2; //!< Contact point on body 2 in local space of body 2 - vec3 this.worldPointOnBody1; //!< Contact point on body 1 in world space - vec3 this.worldPointOnBody2; //!< Contact point on body 2 in world space - boolean this.isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step) - vec3 this.frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane - float this.penetrationImpulse; //!< Cached penetration impulse - float this.frictionImpulse1; //!< Cached first friction impulse - float this.frictionImpulse2; //!< Cached second friction impulse - vec3 this.rollingResistanceImpulse; //!< Cached rolling resistance impulse - /// Private copy-ructor - ContactPoint( ContactPoint contact) = delete; - /// Private assignment operator - ContactPoint operator=( ContactPoint contact) = delete; + CollisionBody* body1; //!< First rigid body of the contact + CollisionBody* body2; //!< Second rigid body of the contact + Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space + float penetrationDepth; //!< Penetration depth + Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1 + Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2 + Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space + Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space + boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step) + Vector3f frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane + float penetrationImpulse; //!< Cached penetration impulse + float frictionImpulse1; //!< Cached first friction impulse + float frictionImpulse2; //!< Cached second friction impulse + Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse public : /// Constructor ContactPoint( ContactPointInfo contactInfo); - /// Destructor - ~ContactPoint(); /// Return the reference to the body 1 CollisionBody* getBody1() ; /// Return the reference to the body 2 CollisionBody* getBody2() ; /// Return the normal vector of the contact - vec3 getNormal() ; + Vector3f getNormal() ; /// Set the penetration depth of the contact void setPenetrationDepth(float penetrationDepth); /// Return the contact local point on body 1 - vec3 getLocalPointOnBody1() ; + Vector3f getLocalPointOnBody1() ; /// Return the contact local point on body 2 - vec3 getLocalPointOnBody2() ; + Vector3f getLocalPointOnBody2() ; /// Return the contact world point on body 1 - vec3 getWorldPointOnBody1() ; + Vector3f getWorldPointOnBody1() ; /// Return the contact world point on body 2 - vec3 getWorldPointOnBody2() ; + Vector3f getWorldPointOnBody2() ; /// Return the cached penetration impulse float getPenetrationImpulse() ; /// Return the cached first friction impulse @@ -111,7 +89,7 @@ namespace ephysics { /// Return the cached second friction impulse float getFrictionImpulse2() ; /// Return the cached rolling resistance impulse - vec3 getRollingResistanceImpulse() ; + Vector3f getRollingResistanceImpulse() ; /// Set the cached penetration impulse void setPenetrationImpulse(float impulse); /// Set the first cached friction impulse @@ -119,27 +97,27 @@ namespace ephysics { /// Set the second cached friction impulse void setFrictionImpulse2(float impulse); /// Set the cached rolling resistance impulse - void setRollingResistanceImpulse( vec3 impulse); + void setRollingResistanceImpulse( Vector3f impulse); /// Set the contact world point on body 1 - void setWorldPointOnBody1( vec3 worldPoint); + void setWorldPointOnBody1( Vector3f worldPoint); /// Set the contact world point on body 2 - void setWorldPointOnBody2( vec3 worldPoint); + void setWorldPointOnBody2( Vector3f worldPoint); /// Return true if the contact is a resting contact boolean getIsRestingContact() ; /// Set the this.isRestingContact variable void setIsRestingContact(boolean isRestingContact); /// Get the first friction vector - vec3 getFrictionVector1() ; + Vector3f getFrictionVector1() ; /// Set the first friction vector - void setFrictionVector1( vec3 frictionVector1); + void setFrictionVector1( Vector3f frictionVector1); /// Get the second friction vector - vec3 getFrictionvec2() ; + Vector3f getFrictionvec2() ; /// Set the second friction vector - void setFrictionvec2( vec3 frictionvec2); + void setFrictionvec2( Vector3f frictionvec2); /// Return the penetration depth float getPenetrationDepth() ; /// Return the number of bytes used by the contact point - sizet getSizeInBytes() ; + long getSizeInBytes() ; }; } diff --git a/src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp b/src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp index 27b33c5..295f3bb 100644 --- a/src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp @@ -19,10 +19,10 @@ FixedJoint::FixedJoint( FixedJointInfo jointInfo) : Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body - etk::Transform3D transform1 = this.body1->getTransform(); - etk::Transform3D transform2 = this.body2->getTransform(); - this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; - this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; + Transform3D transform1 = this.body1.getTransform(); + Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; // Compute the inverse of the initial orientation difference between the two bodies this.initOrientationDifferenceInv = transform2.getOrientation() * @@ -40,30 +40,30 @@ FixedJoint::~FixedJoint() { void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Initialize the bodies index in the velocity array - this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; - this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second; + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second; // Get the bodies positions and orientations - vec3 x1 = this.body1->this.centerOfMassWorld; - vec3 x2 = this.body2->this.centerOfMassWorld; - etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); - etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); + Vector3f x1 = this.body1.this.centerOfMassWorld; + Vector3f x2 = this.body2.this.centerOfMassWorld; + Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); // Get the inertia tensor of bodies - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; this.r2World = orientationBody2 * this.localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints - float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse; + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() @@ -71,7 +71,7 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Compute the inverse mass matrix K^-1 for the 3 translation raints this.inverseMassMatrixTranslation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -85,16 +85,16 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) this.inverseMassMatrixRotation = this.i1 + this.i2; - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse(); } // Compute the bias "b" for the 3 rotation raints this.biasRotation.setZero(); if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) { - etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); - etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; + Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; this.biasRotation = biasFactor * float(2.0) * qError.getVectorV(); } @@ -111,18 +111,18 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void FixedJoint::warmstart( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Compute the impulse P=J^T * lambda for the 3 translation raints for body 1 - vec3 linearImpulseBody1 = -this.impulseTranslation; - vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); + Vector3f linearImpulseBody1 = -this.impulseTranslation; + Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 angularImpulseBody1 += -this.impulseRotation; @@ -132,7 +132,7 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) { w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation raints for body 2 - vec3 angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World); + Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World); // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2 angularImpulseBody2 += this.impulseRotation; @@ -146,35 +146,35 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) { void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // --------------- Translation Constraints --------------- // // Compute J*v for the 3 translation raints - vec3 JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); + Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); // Compute the Lagrange multiplier lambda - vec3 deltaLambda = this.inverseMassMatrixTranslation * + Vector3f deltaLambda = this.inverseMassMatrixTranslation * (-JvTranslation - this.biasTranslation); this.impulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for body 1 - vec3 linearImpulseBody1 = -deltaLambda; - vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World); + Vector3f linearImpulseBody1 = -deltaLambda; + Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for body 2 - vec3 angularImpulseBody2 = -deltaLambda.cross(this.r2World); + Vector3f angularImpulseBody2 = -deltaLambda.cross(this.r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambda; @@ -183,10 +183,10 @@ void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // --------------- Rotation Constraints --------------- // // Compute J*v for the 3 rotation raints - vec3 JvRotation = w2 - w1; + Vector3f JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation raints - vec3 deltaLambda2 = this.inverseMassMatrixRotation * (-JvRotation - this.biasRotation); + Vector3f deltaLambda2 = this.inverseMassMatrixRotation * (-JvRotation - this.biasRotation); this.impulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 @@ -207,70 +207,70 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; // Get the bodies positions and orientations - vec3 x1 = raintSolverData.positions[this.indexBody1]; - vec3 x2 = raintSolverData.positions[this.indexBody2]; - etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; - etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + Vector3f x1 = raintSolverData.positions[this.indexBody1]; + Vector3f x2 = raintSolverData.positions[this.indexBody2]; + Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + Quaternion q2 = raintSolverData.orientations[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Recompute the inverse inertia tensors - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; this.r2World = q2 * this.localAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints - float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse; + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); this.inverseMassMatrixTranslation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute position error for the 3 translation raints - vec3 errorTranslation = x2 + this.r2World - x1 - this.r1World; + Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World; // Compute the Lagrange multiplier lambda - vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); + Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); // Compute the impulse of body 1 - vec3 linearImpulseBody1 = -lambdaTranslation; - vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World); + Vector3f linearImpulseBody1 = -lambdaTranslation; + Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World); // Compute the pseudo velocity of body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - vec3 angularImpulseBody2 = -lambdaTranslation.cross(this.r2World); + Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World); // Compute the pseudo velocity of body 2 - vec3 v2 = inverseMassBody2 * lambdaTranslation; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * lambdaTranslation; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // @@ -278,18 +278,18 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) this.inverseMassMatrixRotation = this.i1 + this.i2; - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse(); } // Compute the position error for the 3 rotation raints - etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); + Quaternion currentOrientationDifference = q2 * q1.getInverse(); currentOrientationDifference.normalize(); - etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; - vec3 errorRotation = float(2.0) * qError.getVectorV(); + Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; + Vector3f errorRotation = float(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation raints - vec3 lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation); + Vector3f lambdaRotation = this.inverseMassMatrixRotation * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 angularImpulseBody1 = -lambdaRotation; @@ -298,14 +298,14 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the pseudo velocity of body 2 w2 = this.i2 * lambdaRotation; // Update the body position/orientation of body 2 - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } diff --git a/src/org/atriaSoft/ephysics/constraint/FixedJoint.hpp b/src/org/atriaSoft/ephysics/constraint/FixedJoint.hpp deleted file mode 100644 index 5e4b399..0000000 --- a/src/org/atriaSoft/ephysics/constraint/FixedJoint.hpp +++ /dev/null @@ -1,78 +0,0 @@ -/** @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) - */ -#pragma once - -// Libraries -#include -#include - -namespace ephysics { - - /** - * 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. - */ - struct FixedJointInfo : public JointInfo { - public : - vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) - /** - * @breif Contructor - * @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(RigidBody* rigidBody1, - RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace): - JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), - this.anchorPointWorldSpace(initAnchorPointWorldSpace){ - - } - }; - - /** - * @breif It represents a fixed joint that is used to forbid any translation or rotation - * between two bodies. - */ - class FixedJoint : public Joint { - private: - static float BETA; //!< Beta value for the bias factor of position correction - vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) - vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) - vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space - vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space - etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) - vec3 this.impulseTranslation; //!< Accumulated impulse for the 3 translation raints - vec3 this.impulseRotation; //!< Accumulate impulse for the 3 rotation raints - etk::Matrix3x3 this.inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix) - etk::Matrix3x3 this.inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix) - vec3 this.biasTranslation; //!< Bias vector for the 3 translation raints - vec3 this.biasRotation; //!< Bias vector for the 3 rotation raints - etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies - /// Private copy-ructor - FixedJoint( FixedJoint raint) = delete; - /// Private assignment operator - FixedJoint operator=( FixedJoint raint) = delete; - - sizet getSizeInBytes() override { - return sizeof(FixedJoint); - } - void initBeforeSolve( ConstraintSolverData raintSolverData) override; - void warmstart( ConstraintSolverData raintSolverData) override; - void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; - void solvePositionConstraint( ConstraintSolverData raintSolverData) override; - public: - /// Constructor - FixedJoint( FixedJointInfo jointInfo); - /// Destructor - virtual ~FixedJoint(); - }; -} - diff --git a/src/org/atriaSoft/ephysics/constraint/FixedJoint.java b/src/org/atriaSoft/ephysics/constraint/FixedJoint.java new file mode 100644 index 0000000..42e3f7c --- /dev/null +++ b/src/org/atriaSoft/ephysics/constraint/FixedJoint.java @@ -0,0 +1,58 @@ +package org.atriaSoft.ephysics.constraint; + + /** + * 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. + */ + struct FixedJointInfo extends JointInfo { + public : + Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + /** + * @breif Contructor + * @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(RigidBody* rigidBody1, + RigidBody* rigidBody2, + Vector3f initAnchorPointWorldSpace): + JointInfo(rigidBody1, rigidBody2, FIXEDJOINT), + this.anchorPointWorldSpace(initAnchorPointWorldSpace){ + + } + }; + + /** + * @breif It represents a fixed joint that is used to forbid any translation or rotation + * between two bodies. + */ + class FixedJoint extends Joint { + private: + static float BETA; //!< Beta value for the bias factor of position correction + Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space + Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space + Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates) + Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates) + Vector3f impulseTranslation; //!< Accumulated impulse for the 3 translation raints + Vector3f impulseRotation; //!< Accumulate impulse for the 3 rotation raints + Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix) + Matrix3f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix) + Vector3f biasTranslation; //!< Bias vector for the 3 translation raints + Vector3f biasRotation; //!< Bias vector for the 3 rotation raints + Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies + + long getSizeInBytes() { + return sizeof(FixedJoint); + } + void initBeforeSolve( ConstraintSolverData raintSolverData) ; + void warmstart( ConstraintSolverData raintSolverData) ; + void solveVelocityConstraint( ConstraintSolverData raintSolverData) ; + void solvePositionConstraint( ConstraintSolverData raintSolverData) ; + public: + /// Constructor + FixedJoint( FixedJointInfo jointInfo); + }; +} + diff --git a/src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp b/src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp index 6bd19d8..750bf50 100644 --- a/src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp @@ -23,14 +23,14 @@ HingeJoint::HingeJoint( HingeJointInfo jointInfo) this.isLowerLimitViolated(false), this.isUpperLimitViolated(false), this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) { - assert(this.lowerLimit <= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lowerLimit >= -2.0 * PI); - assert(this.upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.upperLimit <= 2.0 * PI); + assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI); + assert(this.upperLimit >= 0 && this.upperLimit <= 2.0 * PI); // Compute the local-space anchor point for each body - etk::Transform3D transform1 = this.body1->getTransform(); - etk::Transform3D transform2 = this.body2->getTransform(); - this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; - this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; + Transform3D transform1 = this.body1.getTransform(); + Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; // Compute the local-space hinge axis this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; @@ -54,18 +54,18 @@ HingeJoint::~HingeJoint() { void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Initialize the bodies index in the velocity array - this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; - this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second; + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second; // Get the bodies positions and orientations - vec3 x1 = this.body1->this.centerOfMassWorld; - vec3 x2 = this.body2->this.centerOfMassWorld; - etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); - etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); + Vector3f x1 = this.body1.this.centerOfMassWorld; + Vector3f x2 = this.body2.this.centerOfMassWorld; + Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); // Get the inertia tensor of bodies - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; @@ -90,27 +90,27 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Compute vectors needed in the Jacobian mA1 = orientationBody1 * this.hingeLocalAxisBody1; - vec3 a2 = orientationBody2 * this.hingeLocalAxisBody2; + Vector3f a2 = orientationBody2 * this.hingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - vec3 b2 = a2.getOrthoVector(); - vec3 c2 = a2.cross(b2); + Vector3f b2 = a2.getOrthoVector(); + Vector3f c2 = a2.cross(b2); this.b2CrossA1 = b2.cross(mA1); this.c2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix) - float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse; + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); this.inverseMassMatrixTranslation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslation = massMatrix.getInverse(); } @@ -122,10 +122,10 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { } // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix) - vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1; - vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1; - vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1; - vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1; + Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1; + Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1; + Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1; + Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1; float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1); float el12 = this.b2CrossA1.dot(I1C2CrossA1) + @@ -134,9 +134,9 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { this.c2CrossA1.dot(I2B2CrossA1); float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1); - etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); + Matrix2x2 matrixKRotation(el11, el12, el21, el22); this.inverseMassMatrixRotation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -158,7 +158,7 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { } // If the motor or limits are enabled - if (this.isMotorEnabled || (this.isLimitEnabled hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (this.isLowerLimitViolated || this.isUpperLimitViolated))) { + 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 = mA1.dot(this.i1 * mA1) + mA1.dot(this.i2 * mA1); @@ -186,27 +186,27 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void HingeJoint::warmstart( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Compute the impulse P=J^T * lambda for the 2 rotation raints - vec3 rotationImpulse = -this.b2CrossA1 * this.impulseRotation.x() - this.c2CrossA1 * this.impulseRotation.y(); + Vector3f rotationImpulse = -this.b2CrossA1 * this.impulseRotation.x() - this.c2CrossA1 * this.impulseRotation.y(); // Compute the impulse P=J^T * lambda for the lower and upper limits raints - vec3 limitsImpulse = (this.impulseUpperLimit - this.impulseLowerLimit) * mA1; + Vector3f limitsImpulse = (this.impulseUpperLimit - this.impulseLowerLimit) * mA1; // Compute the impulse P=J^T * lambda for the motor raint - vec3 motorImpulse = -this.impulseMotor * mA1; + Vector3f motorImpulse = -this.impulseMotor * mA1; // Compute the impulse P=J^T * lambda for the 3 translation raints of body 1 - vec3 linearImpulseBody1 = -this.impulseTranslation; - vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); + Vector3f linearImpulseBody1 = -this.impulseTranslation; + Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1 angularImpulseBody1 += rotationImpulse; @@ -222,7 +222,7 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) { w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation raints of body 2 - vec3 angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World); + Vector3f angularImpulseBody2 = -this.impulseTranslation.cross(this.r2World); // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2 angularImpulseBody2 += -rotationImpulse; @@ -242,35 +242,35 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) { void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // --------------- Translation Constraints --------------- // // Compute J*v - vec3 JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); + Vector3f JvTranslation = v2 + w2.cross(this.r2World) - v1 - w1.cross(this.r1World); // Compute the Lagrange multiplier lambda - vec3 deltaLambdaTranslation = this.inverseMassMatrixTranslation * + Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation * (-JvTranslation - this.bTranslation); this.impulseTranslation += deltaLambdaTranslation; // Compute the impulse P=J^T * lambda of body 1 - vec3 linearImpulseBody1 = -deltaLambdaTranslation; - vec3 angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World); + Vector3f linearImpulseBody1 = -deltaLambdaTranslation; + Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World); // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda of body 2 - vec3 angularImpulseBody2 = -deltaLambdaTranslation.cross(this.r2World); + Vector3f angularImpulseBody2 = -deltaLambdaTranslation.cross(this.r2World); // Apply the impulse to the body 2 v2 += inverseMassBody2 * deltaLambdaTranslation; @@ -313,17 +313,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // Compute the Lagrange multiplier lambda for the lower limit raint float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit); float lambdaTemp = this.impulseLowerLimit; - this.impulseLowerLimit = etk::max(this.impulseLowerLimit + deltaLambdaLower, 0.0f); + this.impulseLowerLimit = 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 - vec3 angularImpulseBody1 = -deltaLambdaLower * mA1; + Vector3f angularImpulseBody1 = -deltaLambdaLower * mA1; // Apply the impulse to the body 1 w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 - vec3 angularImpulseBody2 = deltaLambdaLower * mA1; + Vector3f angularImpulseBody2 = deltaLambdaLower * mA1; // Apply the impulse to the body 2 w2 += this.i2 * angularImpulseBody2; @@ -338,17 +338,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // Compute the Lagrange multiplier lambda for the upper limit raint float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit); float lambdaTemp = this.impulseUpperLimit; - this.impulseUpperLimit = etk::max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f); + this.impulseUpperLimit = 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 - vec3 angularImpulseBody1 = deltaLambdaUpper * mA1; + Vector3f angularImpulseBody1 = deltaLambdaUpper * mA1; // Apply the impulse to the body 1 w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 - vec3 angularImpulseBody2 = -deltaLambdaUpper * mA1; + Vector3f angularImpulseBody2 = -deltaLambdaUpper * mA1; // Apply the impulse to the body 2 w2 += this.i2 * angularImpulseBody2; @@ -371,13 +371,13 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) deltaLambdaMotor = this.impulseMotor - lambdaTemp; // Compute the impulse P=J^T * lambda for the motor of body 1 - vec3 angularImpulseBody1 = -deltaLambdaMotor * mA1; + Vector3f angularImpulseBody1 = -deltaLambdaMotor * mA1; // Apply the impulse to the body 1 w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - vec3 angularImpulseBody2 = deltaLambdaMotor * mA1; + Vector3f angularImpulseBody2 = deltaLambdaMotor * mA1; // Apply the impulse to the body 2 w2 += this.i2 * angularImpulseBody2; @@ -392,18 +392,18 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; // Get the bodies positions and orientations - vec3 x1 = raintSolverData.positions[this.indexBody1]; - vec3 x2 = raintSolverData.positions[this.indexBody2]; - etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; - etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + Vector3f x1 = raintSolverData.positions[this.indexBody1]; + Vector3f x2 = raintSolverData.positions[this.indexBody2]; + Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + Quaternion q2 = raintSolverData.orientations[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Recompute the inverse inertia tensors - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + 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 * this.localAnchorPointBody1; @@ -420,70 +420,70 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) // Compute vectors needed in the Jacobian mA1 = q1 * this.hingeLocalAxisBody1; - vec3 a2 = q2 * this.hingeLocalAxisBody2; + Vector3f a2 = q2 * this.hingeLocalAxisBody2; mA1.normalize(); a2.normalize(); - vec3 b2 = a2.getOrthoVector(); - vec3 c2 = a2.cross(b2); + Vector3f b2 = a2.getOrthoVector(); + Vector3f c2 = a2.cross(b2); this.b2CrossA1 = b2.cross(mA1); this.c2CrossA1 = c2.cross(mA1); // Compute the corresponding skew-symmetric matrices - etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); - etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); + Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World); + Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World); // --------------- Translation Constraints --------------- // // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints - float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; - etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, + float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse; + Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies) + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); this.inverseMassMatrixTranslation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslation = massMatrix.getInverse(); } // Compute position error for the 3 translation raints - vec3 errorTranslation = x2 + this.r2World - x1 - this.r1World; + Vector3f errorTranslation = x2 + this.r2World - x1 - this.r1World; // Compute the Lagrange multiplier lambda - vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); + Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); // Compute the impulse of body 1 - vec3 linearImpulseBody1 = -lambdaTranslation; - vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World); + Vector3f linearImpulseBody1 = -lambdaTranslation; + Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World); // Compute the pseudo velocity of body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 - vec3 angularImpulseBody2 = -lambdaTranslation.cross(this.r2World); + Vector3f angularImpulseBody2 = -lambdaTranslation.cross(this.r2World); // Compute the pseudo velocity of body 2 - vec3 v2 = inverseMassBody2 * lambdaTranslation; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * lambdaTranslation; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix) - vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1; - vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1; - vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1; - vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1; + Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1; + Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1; + Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1; + Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1; float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1); float el12 = this.b2CrossA1.dot(I1C2CrossA1) + @@ -492,9 +492,9 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) this.c2CrossA1.dot(I2B2CrossA1); float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1); - etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); + Matrix2x2 matrixKRotation(el11, el12, el21, el22); this.inverseMassMatrixRotation.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotation = matrixKRotation.getInverse(); } @@ -511,7 +511,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse of body 2 @@ -521,7 +521,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Limits Constraints --------------- // @@ -543,23 +543,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError ); // Compute the impulse P=J^T * lambda of body 1 - vec3 angularImpulseBody1 = -lambdaLowerLimit * mA1; + Vector3f angularImpulseBody1 = -lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 1 - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - vec3 angularImpulseBody2 = lambdaLowerLimit * mA1; + Vector3f angularImpulseBody2 = lambdaLowerLimit * mA1; // Compute the pseudo velocity of body 2 - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } @@ -570,23 +570,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError); // Compute the impulse P=J^T * lambda of body 1 - vec3 angularImpulseBody1 = lambdaUpperLimit * mA1; + Vector3f angularImpulseBody1 = lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 1 - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda of body 2 - vec3 angularImpulseBody2 = -lambdaUpperLimit * mA1; + Vector3f angularImpulseBody2 = -lambdaUpperLimit * mA1; // Compute the pseudo velocity of body 2 - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } } @@ -620,8 +620,8 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) { this.impulseMotor = 0.0; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } // Set the minimum angle limit @@ -630,7 +630,7 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) { */ void HingeJoint::setMinAngleLimit(float lowerLimit) { - assert(this.lowerLimit <= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lowerLimit >= -2.0 * PI); + assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI); if (lowerLimit != this.lowerLimit) { @@ -647,7 +647,7 @@ void HingeJoint::setMinAngleLimit(float lowerLimit) { */ void HingeJoint::setMaxAngleLimit(float upperLimit) { - assert(upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj upperLimit <= 2.0 * PI); + assert(upperLimit >= 0 && upperLimit <= 2.0 * PI); if (upperLimit != this.upperLimit) { @@ -666,8 +666,8 @@ void HingeJoint::resetLimits() { this.impulseUpperLimit = 0.0; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } // Set the motor speed @@ -678,8 +678,8 @@ void HingeJoint::setMotorSpeed(float motorSpeed) { this.motorSpeed = motorSpeed; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } } @@ -695,8 +695,8 @@ void HingeJoint::setMaxMotorTorque(float maxMotorTorque) { this.maxMotorTorque = maxMotorTorque; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } } @@ -742,17 +742,17 @@ float HingeJoint::computeCorrespondingAngleNearLimits(float inputAngle, float lo } // Compute the current angle around the hinge axis -float HingeJoint::computeCurrentHingeAngle( etk::Quaternion orientationBody1, - etk::Quaternion orientationBody2) { +float HingeJoint::computeCurrentHingeAngle( Quaternion orientationBody1, + Quaternion orientationBody2) { float hingeAngle; // Compute the current orientation difference between the two bodies - etk::Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); + Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); currentOrientationDiff.normalize(); // Compute the relative rotation considering the initial orientation difference - etk::Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv; + Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv; relativeRotation.normalize(); // A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit @@ -770,10 +770,10 @@ float HingeJoint::computeCurrentHingeAngle( etk::Quaternion orientationBody1, // If the relative rotation axis and the hinge axis are pointing the same direction if (dotProduct >= 0.0f) { - hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, cosHalfAngle); + hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, cosHalfAngle); } else { - hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, -cosHalfAngle); + hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, -cosHalfAngle); } // Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi] @@ -842,7 +842,7 @@ float HingeJoint::getMotorTorque(float timeStep) { } // Return the number of bytes used by the joint -sizet HingeJoint::getSizeInBytes() { +long HingeJoint::getSizeInBytes() { return sizeof(HingeJoint); } diff --git a/src/org/atriaSoft/ephysics/constraint/HingeJoint.hpp b/src/org/atriaSoft/ephysics/constraint/HingeJoint.java similarity index 57% rename from src/org/atriaSoft/ephysics/constraint/HingeJoint.hpp rename to src/org/atriaSoft/ephysics/constraint/HingeJoint.java index e80e2af..723c5d4 100644 --- a/src/org/atriaSoft/ephysics/constraint/HingeJoint.hpp +++ b/src/org/atriaSoft/ephysics/constraint/HingeJoint.java @@ -1,25 +1,13 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.constraint; -#include -#include - -namespace ephysics { /** * @brief It is used to gather the information needed to create a hinge joint. * This structure will be used to create the actual hinge joint. */ - struct HingeJointInfo : public JointInfo { + struct HingeJointInfo extends JointInfo { public : - vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) - vec3 rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates) + Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates) boolean isLimitEnabled; //!< True if the hinge joint limits are enabled boolean isMotorEnabled; //!< True if the hinge joint motor is enabled float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0] @@ -35,8 +23,8 @@ namespace ephysics { */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initRotationAxisWorld): + Vector3f initAnchorPointWorldSpace, + Vector3f initRotationAxisWorld): JointInfo(rigidBody1, rigidBody2, HINGEJOINT), this.anchorPointWorldSpace(initAnchorPointWorldSpace), rotationAxisWorld(initRotationAxisWorld), @@ -59,8 +47,8 @@ namespace ephysics { */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initRotationAxisWorld, + Vector3f initAnchorPointWorldSpace, + Vector3f initRotationAxisWorld, float initMinAngleLimit, float initMaxAngleLimit): JointInfo(rigidBody1, rigidBody2, HINGEJOINT), @@ -87,8 +75,8 @@ namespace ephysics { */ HingeJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initRotationAxisWorld, + Vector3f initAnchorPointWorldSpace, + Vector3f initRotationAxisWorld, float initMinAngleLimit, float initMaxAngleLimit, float initMotorSpeed, @@ -111,46 +99,42 @@ namespace ephysics { * between two bodies around a single axis. This joint has one degree of freedom. It * can be useful to simulate doors or pendulumns. */ - class HingeJoint : public Joint { + class HingeJoint extends Joint { private : - static float BETA; //!< Beta value for the bias factor of position correction - vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) - vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) - vec3 this.hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1) - vec3 this.hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2) - etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) - vec3 mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1 - vec3 this.r1World; //!< Vector from center of body 2 to anchor point in world-space - vec3 this.r2World; //!< Vector from center of body 2 to anchor point in world-space - vec3 this.b2CrossA1; //!< Cross product of vector b2 and a1 - vec3 this.c2CrossA1; //!< Cross product of vector c2 and a1; - vec3 this.impulseTranslation; //!< Impulse for the 3 translation raints - vec2 this.impulseRotation; //!< Impulse for the 2 rotation raints - float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint - float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint - float this.impulseMotor; //!< Accumulated impulse for the motor raint; - etk::Matrix3x3 this.inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints - etk::Matrix2x2 this.inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints - float this.inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix) - float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor - vec3 this.bTranslation; //!< Bias vector for the error correction for the translation raints - vec2 this.bRotation; //!< Bias vector for the error correction for the rotation raints - float this.bLowerLimit; //!< Bias of the lower limit raint - float this.bUpperLimit; //!< Bias of the upper limit raint - etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies - boolean this.isLimitEnabled; //!< True if the joint limits are enabled - boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled - float this.lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian) - float this.upperLimit; //!< Upper limit (maximum translation distance) - boolean this.isLowerLimitViolated; //!< True if the lower limit is violated - boolean this.isUpperLimitViolated; //!< True if the upper limit is violated - float this.motorSpeed; //!< Motor speed (in rad/s) - float this.maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed - /// Private copy-ructor - HingeJoint( HingeJoint raint); - /// Private assignment operator - HingeJoint operator=( HingeJoint raint); + static float BETA; //!< Beta value for the bias factor of position correction + Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1) + Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2) + Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates) + Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates) + Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1 + Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space + Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space + Vector3f b2CrossA1; //!< Cross product of vector b2 and a1 + Vector3f c2CrossA1; //!< Cross product of vector c2 and a1; + Vector3f impulseTranslation; //!< Impulse for the 3 translation raints + vec2 impulseRotation; //!< Impulse for the 2 rotation raints + float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint + float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint + float impulseMotor; //!< Accumulated impulse for the motor raint; + Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints + Matrix2x2 inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints + float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix) + float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor + Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints + vec2 bRotation; //!< Bias vector for the error correction for the rotation raints + float bLowerLimit; //!< Bias of the lower limit raint + float bUpperLimit; //!< Bias of the upper limit raint + Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies + boolean isLimitEnabled; //!< True if the joint limits are enabled + boolean isMotorEnabled; //!< True if the motor of the joint in enabled + float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian) + float upperLimit; //!< Upper limit (maximum translation distance) + boolean isLowerLimitViolated; //!< True if the lower limit is violated + boolean isUpperLimitViolated; //!< True if the upper limit is violated + float motorSpeed; //!< Motor speed (in rad/s) + float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed /// Reset the limits void resetLimits(); /// Given an angle in radian, this method returns the corresponding @@ -163,17 +147,15 @@ namespace ephysics { float lowerLimitAngle, float upperLimitAngle) ; /// Compute the current angle around the hinge axis - float computeCurrentHingeAngle( etk::Quaternion orientationBody1, etk::Quaternion orientationBody2); - sizet getSizeInBytes() override; - void initBeforeSolve( ConstraintSolverData raintSolverData) override; - void warmstart( ConstraintSolverData raintSolverData) override; - void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; - void solvePositionConstraint( ConstraintSolverData raintSolverData) override; + float computeCurrentHingeAngle( Quaternion orientationBody1, Quaternion orientationBody2); + long getSizeInBytes() ; + void initBeforeSolve( ConstraintSolverData raintSolverData) ; + void warmstart( ConstraintSolverData raintSolverData) ; + void solveVelocityConstraint( ConstraintSolverData raintSolverData) ; + void solvePositionConstraint( ConstraintSolverData raintSolverData) ; public : /// Constructor HingeJoint( HingeJointInfo jointInfo); - /// Destructor - virtual ~HingeJoint(); /// Return true if the limits or the joint are enabled boolean isLimitEnabled() ; /// Return true if the motor of the joint is enabled diff --git a/src/org/atriaSoft/ephysics/constraint/Joint.cpp b/src/org/atriaSoft/ephysics/constraint/Joint.cpp index 188bcb7..607a8e3 100644 --- a/src/org/atriaSoft/ephysics/constraint/Joint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/Joint.cpp @@ -44,7 +44,7 @@ RigidBody* Joint::getBody2() { * @return True if the joint is active */ boolean Joint::isActive() { - return (this.body1->isActive() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.body2->isActive()); + return (this.body1.isActive() && this.body2.isActive()); } // Return the type of the joint diff --git a/src/org/atriaSoft/ephysics/constraint/Joint.hpp b/src/org/atriaSoft/ephysics/constraint/Joint.java similarity index 63% rename from src/org/atriaSoft/ephysics/constraint/Joint.hpp rename to src/org/atriaSoft/ephysics/constraint/Joint.java index 4e78c1c..2869513 100644 --- a/src/org/atriaSoft/ephysics/constraint/Joint.hpp +++ b/src/org/atriaSoft/ephysics/constraint/Joint.java @@ -1,18 +1,4 @@ -/** @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) - */ -#pragma once - -#include -#include -#include - -namespace ephysics { +package org.atriaSoft.ephysics.constraint; /// Enumeration for the type of a raint enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; @@ -69,7 +55,7 @@ namespace ephysics { } /// Destructor - virtual ~JointInfo() = default; + ~JointInfo() = default; }; /** @@ -77,14 +63,14 @@ namespace ephysics { */ class Joint { protected : - RigidBody* this.body1; //!< Pointer to the first body of the joint - RigidBody* this.body2; //!< Pointer to the second body of the joint - JointType this.type; //!< Type of the joint - int this.indexBody1; //!< Body 1 index in the velocity array to solve the raint - int this.indexBody2; //!< Body 2 index in the velocity array to solve the raint - JointsPositionCorrectionTechnique this.positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints) - boolean this.isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other - boolean this.isAlreadyInIsland; //!< True if the joint has already been added into an island + RigidBody* body1; //!< Pointer to the first body of the joint + RigidBody* body2; //!< Pointer to the second body of the joint + JointType type; //!< Type of the joint + int indexBody1; //!< Body 1 index in the velocity array to solve the raint + int indexBody2; //!< Body 2 index in the velocity array to solve the raint + JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints) + boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other + boolean isAlreadyInIsland; //!< True if the joint has already been added into an island /// Private copy-ructor Joint( Joint raint); /// Private assignment operator @@ -92,20 +78,20 @@ namespace ephysics { /// Return true if the joint has already been added into an island boolean isAlreadyInIsland() ; /// Return the number of bytes used by the joint - virtual sizet getSizeInBytes() = 0; + long getSizeInBytes() = 0; /// Initialize before solving the joint - virtual void initBeforeSolve( ConstraintSolverData raintSolverData) = 0; + void initBeforeSolve( ConstraintSolverData raintSolverData) = 0; /// Warm start the joint (apply the previous impulse at the beginning of the step) - virtual void warmstart( ConstraintSolverData raintSolverData) = 0; + void warmstart( ConstraintSolverData raintSolverData) = 0; /// Solve the velocity raint - virtual void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0; + void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0; /// Solve the position raint - virtual void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0; + void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0; public : /// Constructor Joint( JointInfo jointInfo); /// Destructor - virtual ~Joint(); + ~Joint(); /// Return the reference to the body 1 RigidBody* getBody1() ; /// Return the reference to the body 2 diff --git a/src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp b/src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp index 3dcb6db..89d8e4c 100644 --- a/src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp +++ b/src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp @@ -28,10 +28,10 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo) assert(this.maxMotorForce >= 0.0); // Compute the local-space anchor point for each body - etk::Transform3D transform1 = this.body1->getTransform(); - etk::Transform3D transform2 = this.body2->getTransform(); - this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; - this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; + Transform3D transform1 = this.body1.getTransform(); + Transform3D transform2 = this.body2.getTransform(); + this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; // Compute the inverse of the initial orientation difference between the two bodies this.initOrientationDifferenceInv = transform2.getOrientation() * @@ -40,7 +40,7 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo) this.initOrientationDifferenceInv.inverse(); // Compute the slider axis in local-space of body 1 - this.sliderAxisBody1 = this.body1->getTransform().getOrientation().getInverse() * + this.sliderAxisBody1 = this.body1.getTransform().getOrientation().getInverse() * jointInfo.sliderAxisWorldSpace; this.sliderAxisBody1.normalize(); } @@ -54,25 +54,25 @@ SliderJoint::~SliderJoint() { void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Initialize the bodies index in the veloc ity array - this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; - this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; + this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second; + this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second; // Get the bodies positions and orientations - vec3 x1 = this.body1->this.centerOfMassWorld; - vec3 x2 = this.body2->this.centerOfMassWorld; - etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); - etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); + Vector3f x1 = this.body1.this.centerOfMassWorld; + Vector3f x2 = this.body2.this.centerOfMassWorld; + Quaternion orientationBody1 = this.body1.getTransform().getOrientation(); + Quaternion orientationBody2 = this.body2.getTransform().getOrientation(); // Get the inertia tensor of bodies - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); // Vector from body center to the anchor point this.R1 = orientationBody1 * this.localAnchorPointBody1; this.R2 = orientationBody2 * this.localAnchorPointBody2; // Compute the vector u (difference between anchor points) - vec3 u = x2 + this.R2 - x1 - this.R1; + Vector3f u = x2 + this.R2 - x1 - this.R1; // Compute the two orthogonal vectors to the slider axis in world-space this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1; @@ -99,18 +99,18 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { this.R2CrossN1 = this.R2.cross(this.N1); this.R2CrossN2 = this.R2.cross(this.N2); this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); - vec3 r1PlusU = this.R1 + u; + Vector3f r1PlusU = this.R1 + 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) - float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse; - vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; - vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; - vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1; - vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2; + float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse; + Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; + Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; + Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1; + Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2; float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1); float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -119,9 +119,9 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { this.R2CrossN2.dot(I2R2CrossN1); float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2); - etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); this.inverseMassMatrixTranslationConstraint.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -137,24 +137,24 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) this.inverseMassMatrixRotationConstraint = this.i1 + this.i2; - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse(); } // Compute the bias "b" of the rotation raint this.bRotation.setZero(); if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) { - etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); currentOrientationDifference.normalize(); - etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; + Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; this.bRotation = biasFactor * float(2.0) * qError.getVectorV(); } // If the limits are enabled - if (this.isLimitEnabled hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (this.isLowerLimitViolated || this.isUpperLimitViolated)) { + 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->this.massInverse + this.body2->this.massInverse + + this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) + this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis); this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ? @@ -177,7 +177,7 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { if (this.isMotorEnabled) { // Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) - this.inverseMassMatrixMotor = this.body1->this.massInverse + this.body2->this.massInverse; + this.inverseMassMatrixMotor = this.body1.this.massInverse + this.body2.this.massInverse; this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0) ? 1.0f / this.inverseMassMatrixMotor : 0.0f; } @@ -198,25 +198,25 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void SliderJoint::warmstart( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1 float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit; - vec3 linearImpulseLimits = impulseLimits * this.sliderAxisWorld; + Vector3f linearImpulseLimits = impulseLimits * this.sliderAxisWorld; // Compute the impulse P=J^T * lambda for the motor raint of body 1 - vec3 impulseMotor = this.impulseMotor * this.sliderAxisWorld; + Vector3f impulseMotor = this.impulseMotor * this.sliderAxisWorld; // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 - vec3 linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y(); - vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() - + Vector3f linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y(); + Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() - this.R1PlusUCrossN2 * this.impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 @@ -234,8 +234,8 @@ void SliderJoint::warmstart( ConstraintSolverData raintSolverData) { w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 - vec3 linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y(); - vec3 angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() + + Vector3f linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y(); + Vector3f angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() + this.R2CrossN2 * this.impulseTranslation.y(); // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 @@ -257,14 +257,14 @@ void SliderJoint::warmstart( ConstraintSolverData raintSolverData) { void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { // Get the velocities - vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; - vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; - vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; - vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; + Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1]; + Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2]; + Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1]; + Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // --------------- Translation Constraints --------------- // @@ -280,8 +280,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) this.impulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 - vec3 linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y(); - vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() - + Vector3f linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y(); + Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() - this.R1PlusUCrossN2 * deltaLambda.y(); // Apply the impulse to the body 1 @@ -289,8 +289,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 - vec3 linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y(); - vec3 angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y(); + Vector3f linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y(); + Vector3f angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y(); // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -299,10 +299,10 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // --------------- Rotation Constraints --------------- // // Compute J*v for the 3 rotation raints - vec3 JvRotation = w2 - w1; + Vector3f JvRotation = w2 - w1; // Compute the Lagrange multiplier lambda for the 3 rotation raints - vec3 deltaLambda2 = this.inverseMassMatrixRotationConstraint * (-JvRotation - this.bRotation); + Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint * (-JvRotation - this.bRotation); this.impulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 @@ -331,20 +331,20 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // Compute the Lagrange multiplier lambda for the lower limit raint float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit); float lambdaTemp = this.impulseLowerLimit; - this.impulseLowerLimit = etk::max(this.impulseLowerLimit + deltaLambdaLower, 0.0f); + this.impulseLowerLimit = 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 - vec3 linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld; - vec3 angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis; + Vector3f linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld; + Vector3f angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 - vec3 linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld; - vec3 angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis; + Vector3f linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld; + Vector3f angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -361,20 +361,20 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) // Compute the Lagrange multiplier lambda for the upper limit raint float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit); float lambdaTemp = this.impulseUpperLimit; - this.impulseUpperLimit = etk::max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f); + this.impulseUpperLimit = 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 - vec3 linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld; - vec3 angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis; + Vector3f linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld; + Vector3f angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; w1 += this.i1 * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 - vec3 linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld; - vec3 angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis; + Vector3f linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld; + Vector3f angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -397,13 +397,13 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) deltaLambdaMotor = this.impulseMotor - lambdaTemp; // Compute the impulse P=J^T * lambda for the motor of body 1 - vec3 linearImpulseBody1 = deltaLambdaMotor * this.sliderAxisWorld; + Vector3f linearImpulseBody1 = deltaLambdaMotor * this.sliderAxisWorld; // Apply the impulse to the body 1 v1 += inverseMassBody1 * linearImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 - vec3 linearImpulseBody2 = -deltaLambdaMotor * this.sliderAxisWorld; + Vector3f linearImpulseBody2 = -deltaLambdaMotor * this.sliderAxisWorld; // Apply the impulse to the body 2 v2 += inverseMassBody2 * linearImpulseBody2; @@ -418,25 +418,25 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; // Get the bodies positions and orientations - vec3 x1 = raintSolverData.positions[this.indexBody1]; - vec3 x2 = raintSolverData.positions[this.indexBody2]; - etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; - etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; + Vector3f x1 = raintSolverData.positions[this.indexBody1]; + Vector3f x2 = raintSolverData.positions[this.indexBody2]; + Quaternion q1 = raintSolverData.orientations[this.indexBody1]; + Quaternion q2 = raintSolverData.orientations[this.indexBody2]; // Get the inverse mass and inverse inertia tensors of the bodies - float inverseMassBody1 = this.body1->this.massInverse; - float inverseMassBody2 = this.body2->this.massInverse; + float inverseMassBody1 = this.body1.this.massInverse; + float inverseMassBody2 = this.body2.this.massInverse; // Recompute the inertia tensor of bodies - this.i1 = this.body1->getInertiaTensorInverseWorld(); - this.i2 = this.body2->getInertiaTensorInverseWorld(); + this.i1 = this.body1.getInertiaTensorInverseWorld(); + this.i2 = this.body2.getInertiaTensorInverseWorld(); // Vector from body center to the anchor point this.R1 = q1 * this.localAnchorPointBody1; this.R2 = q2 * this.localAnchorPointBody2; // Compute the vector u (difference between anchor points) - vec3 u = x2 + this.R2 - x1 - this.R1; + Vector3f u = x2 + this.R2 - x1 - this.R1; // Compute the two orthogonal vectors to the slider axis in world-space this.sliderAxisWorld = q1 * this.sliderAxisBody1; @@ -455,7 +455,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) this.R2CrossN1 = this.R2.cross(this.N1); this.R2CrossN2 = this.R2.cross(this.N2); this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); - vec3 r1PlusU = this.R1 + u; + Vector3f r1PlusU = this.R1 + u; this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1); this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2); this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld); @@ -464,11 +464,11 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // raints (2x2 matrix) - float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse; - vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; - vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; - vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1; - vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2; + float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse; + Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; + Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; + Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1; + Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2; float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1); float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + @@ -477,9 +477,9 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) this.R2CrossN2.dot(I2R2CrossN1); float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2); - etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); this.inverseMassMatrixTranslationConstraint.setZero(); - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } @@ -490,31 +490,31 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError); // Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 - vec3 linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y(); - vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() - + Vector3f linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y(); + Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() - this.R1PlusUCrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 - vec3 linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y(); - vec3 angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() + + Vector3f linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y(); + Vector3f angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() + this.R2CrossN2 * lambdaTranslation.y(); // Apply the impulse to the body 2 - vec3 v2 = inverseMassBody2 * linearImpulseBody2; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * linearImpulseBody2; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Rotation Constraints --------------- // @@ -522,18 +522,18 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) this.inverseMassMatrixRotationConstraint = this.i1 + this.i2; - if (this.body1->getType() == DYNAMIC || this.body2->getType() == DYNAMIC) { + if (this.body1.getType() == DYNAMIC || this.body2.getType() == DYNAMIC) { this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse(); } // Compute the position error for the 3 rotation raints - etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); + Quaternion currentOrientationDifference = q2 * q1.getInverse(); currentOrientationDifference.normalize(); - etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; - vec3 errorRotation = float(2.0) * qError.getVectorV(); + Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; + Vector3f errorRotation = float(2.0) * qError.getVectorV(); // Compute the Lagrange multiplier lambda for the 3 rotation raints - vec3 lambdaRotation = this.inverseMassMatrixRotationConstraint * (-errorRotation); + Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint * (-errorRotation); // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 angularImpulseBody1 = -lambdaRotation; @@ -542,7 +542,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 @@ -552,7 +552,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); // --------------- Limits Constraints --------------- // @@ -562,7 +562,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) 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->this.massInverse + this.body2->this.massInverse + + this.inverseMassMatrixLimit = this.body1.this.massInverse + this.body2.this.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) + this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis); this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ? @@ -576,29 +576,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError); // Compute the impulse P=J^T * lambda for the lower limit raint of body 1 - vec3 linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld; - vec3 angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis; + Vector3f linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld; + Vector3f angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the lower limit raint of body 2 - vec3 linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld; - vec3 angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis; + Vector3f linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld; + Vector3f angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis; // Apply the impulse to the body 2 - vec3 v2 = inverseMassBody2 * linearImpulseBody2; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * linearImpulseBody2; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } @@ -609,29 +609,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData) float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError); // Compute the impulse P=J^T * lambda for the upper limit raint of body 1 - vec3 linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld; - vec3 angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis; + Vector3f linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld; + Vector3f angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - vec3 v1 = inverseMassBody1 * linearImpulseBody1; - vec3 w1 = this.i1 * angularImpulseBody1; + Vector3f v1 = inverseMassBody1 * linearImpulseBody1; + Vector3f w1 = this.i1 * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; - q1 += etk::Quaternion(0, w1) * q1 * 0.5f; + q1 += Quaternion(0, w1) * q1 * 0.5f; q1.normalize(); // Compute the impulse P=J^T * lambda for the upper limit raint of body 2 - vec3 linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld; - vec3 angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis; + Vector3f linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld; + Vector3f angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis; // Apply the impulse to the body 2 - vec3 v2 = inverseMassBody2 * linearImpulseBody2; - vec3 w2 = this.i2 * angularImpulseBody2; + Vector3f v2 = inverseMassBody2 * linearImpulseBody2; + Vector3f w2 = this.i2 * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; - q2 += etk::Quaternion(0, w2) * q2 * 0.5f; + q2 += Quaternion(0, w2) * q2 * 0.5f; q2.normalize(); } } @@ -664,8 +664,8 @@ void SliderJoint::enableMotor(boolean isMotorEnabled) { this.impulseMotor = 0.0; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } // Return the current translation value of the joint @@ -677,20 +677,20 @@ float SliderJoint::getTranslation() { // TODO : Check if we need to compare rigid body position or center of mass here // Get the bodies positions and orientations - vec3 x1 = this.body1->getTransform().getPosition(); - vec3 x2 = this.body2->getTransform().getPosition(); - etk::Quaternion q1 = this.body1->getTransform().getOrientation(); - etk::Quaternion q2 = this.body2->getTransform().getOrientation(); + Vector3f x1 = this.body1.getTransform().getPosition(); + Vector3f x2 = this.body2.getTransform().getPosition(); + Quaternion q1 = this.body1.getTransform().getOrientation(); + Quaternion q2 = this.body2.getTransform().getOrientation(); // Compute the two anchor points in world-space coordinates - vec3 anchorBody1 = x1 + q1 * this.localAnchorPointBody1; - vec3 anchorBody2 = x2 + q2 * this.localAnchorPointBody2; + Vector3f anchorBody1 = x1 + q1 * this.localAnchorPointBody1; + Vector3f anchorBody2 = x2 + q2 * this.localAnchorPointBody2; // Compute the vector u (difference between anchor points) - vec3 u = anchorBody2 - anchorBody1; + Vector3f u = anchorBody2 - anchorBody1; // Compute the slider axis in world-space - vec3 sliderAxisWorld = q1 * this.sliderAxisBody1; + Vector3f sliderAxisWorld = q1 * this.sliderAxisBody1; sliderAxisWorld.normalize(); // Compute and return the translation value @@ -739,8 +739,8 @@ void SliderJoint::resetLimits() { this.impulseUpperLimit = 0.0; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } // Set the motor speed @@ -754,8 +754,8 @@ void SliderJoint::setMotorSpeed(float motorSpeed) { this.motorSpeed = motorSpeed; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } } @@ -771,8 +771,8 @@ void SliderJoint::setMaxMotorForce(float maxMotorForce) { this.maxMotorForce = maxMotorForce; // Wake up the two bodies of the joint - this.body1->setIsSleeping(false); - this.body2->setIsSleeping(false); + this.body1.setIsSleeping(false); + this.body2.setIsSleeping(false); } } @@ -834,7 +834,7 @@ float SliderJoint::getMotorForce(float timeStep) { } // Return the number of bytes used by the joint -sizet SliderJoint::getSizeInBytes() { +long SliderJoint::getSizeInBytes() { return sizeof(SliderJoint); } diff --git a/src/org/atriaSoft/ephysics/constraint/SliderJoint.hpp b/src/org/atriaSoft/ephysics/constraint/SliderJoint.java similarity index 70% rename from src/org/atriaSoft/ephysics/constraint/SliderJoint.hpp rename to src/org/atriaSoft/ephysics/constraint/SliderJoint.java index 4bc902d..eb1b3a5 100644 --- a/src/org/atriaSoft/ephysics/constraint/SliderJoint.hpp +++ b/src/org/atriaSoft/ephysics/constraint/SliderJoint.java @@ -1,26 +1,14 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.constraint; -#include -#include - -namespace ephysics { /** * 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. */ - struct SliderJointInfo : public JointInfo { + struct SliderJointInfo extends JointInfo { public : - vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) - vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates) + Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) + Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates) boolean isLimitEnabled; //!< True if the slider limits are enabled boolean isMotorEnabled; //!< True if the slider motor is enabled float minTranslationLimit; //!< Mininum allowed translation if limits are enabled @@ -36,8 +24,8 @@ namespace ephysics { */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initSliderAxisWorldSpace): + Vector3f initAnchorPointWorldSpace, + Vector3f initSliderAxisWorldSpace): JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), this.anchorPointWorldSpace(initAnchorPointWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace), @@ -60,8 +48,8 @@ namespace ephysics { */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initSliderAxisWorldSpace, + Vector3f initAnchorPointWorldSpace, + Vector3f initSliderAxisWorldSpace, float initMinTranslationLimit, float initMaxTranslationLimit): JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), @@ -88,8 +76,8 @@ namespace ephysics { */ SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, - vec3 initAnchorPointWorldSpace, - vec3 initSliderAxisWorldSpace, + Vector3f initAnchorPointWorldSpace, + Vector3f initSliderAxisWorldSpace, float initMinTranslationLimit, float initMaxTranslationLimit, float initMotorSpeed, @@ -115,38 +103,38 @@ namespace ephysics { class SliderJoint: public Joint { private: static float BETA; //!< Beta value for the position correction bias factor - vec3 this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) - vec3 this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) - vec3 this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1) - etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) - etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) - etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies - vec3 this.N1; //!< First vector orthogonal to the slider axis local-space of body 1 - vec3 this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1 - vec3 this.R1; //!< Vector r1 in world-space coordinates - vec3 this.R2; //!< Vector r2 in world-space coordinates - vec3 this.R2CrossN1; //!< Cross product of r2 and n1 - vec3 this.R2CrossN2; //!< Cross product of r2 and n2 - vec3 this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis - vec3 this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1 - vec3 this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2 - vec3 this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis + Vector3f this.localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1) + Vector3f this.localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2) + Vector3f this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1) + Matrix3f this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) + Matrix3f this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) + Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies + Vector3f this.N1; //!< First vector orthogonal to the slider axis local-space of body 1 + Vector3f this.N2; //!< Second vector orthogonal to the slider axis and this.N1 in local-space of body 1 + Vector3f this.R1; //!< Vector r1 in world-space coordinates + Vector3f this.R2; //!< Vector r2 in world-space coordinates + Vector3f this.R2CrossN1; //!< Cross product of r2 and n1 + Vector3f this.R2CrossN2; //!< Cross product of r2 and n2 + Vector3f this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis + Vector3f this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1 + Vector3f this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2 + Vector3f this.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis vec2 this.bTranslation; //!< Bias of the 2 translation raints - vec3 this.bRotation; //!< Bias of the 3 rotation raints + Vector3f this.bRotation; //!< Bias of the 3 rotation raints float this.bLowerLimit; //!< Bias of the lower limit raint float this.bUpperLimit; //!< Bias of the upper limit raint - etk::Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix) - etk::Matrix3x3 this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix) + Matrix2x2 this.inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix) + Matrix3f this.inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix) float this.inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix) float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints - vec3 this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints + Vector3f this.impulseRotation; //!< Accumulated impulse for the 3 rotation raints float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint float this.impulseMotor; //!< Accumulated impulse for the motor boolean this.isLimitEnabled; //!< True if the slider limits are enabled boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled - vec3 this.sliderAxisWorld; //!< Slider axis in world-space coordinates + Vector3f this.sliderAxisWorld; //!< Slider axis in world-space coordinates float this.lowerLimit; //!< Lower limit (minimum translation distance) float this.upperLimit; //!< Upper limit (maximum translation distance) boolean this.isLowerLimitViolated; //!< True if the lower limit is violated @@ -159,16 +147,16 @@ namespace ephysics { SliderJoint operator=( SliderJoint raint); /// Reset the limits void resetLimits(); - sizet getSizeInBytes() override; - void initBeforeSolve( ConstraintSolverData raintSolverData) override; - void warmstart( ConstraintSolverData raintSolverData) override; - void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; - void solvePositionConstraint( ConstraintSolverData raintSolverData) override; + long getSizeInBytes() ; + void initBeforeSolve( ConstraintSolverData raintSolverData) ; + void warmstart( ConstraintSolverData raintSolverData) ; + void solveVelocityConstraint( ConstraintSolverData raintSolverData) ; + void solvePositionConstraint( ConstraintSolverData raintSolverData) ; public : /// Constructor SliderJoint( SliderJointInfo jointInfo); /// Destructor - virtual ~SliderJoint(); + ~SliderJoint(); /// Return true if the limits or the joint are enabled boolean isLimitEnabled() ; /// Return true if the motor of the joint is enabled diff --git a/src/org/atriaSoft/ephysics/debug.cpp b/src/org/atriaSoft/ephysics/debug.cpp deleted file mode 100644 index d77ade8..0000000 --- a/src/org/atriaSoft/ephysics/debug.cpp +++ /dev/null @@ -1,12 +0,0 @@ -/** @file - * @author Edouard DUPIN - * @copyright 2011, Edouard DUPIN, all right reserved - * @license MPL v2.0 (see license file) - */ - -#include - -int ephysic::getLogId() { - static int gval = elog::registerInstance("ephysic"); - return gval; -} diff --git a/src/org/atriaSoft/ephysics/debug.hpp b/src/org/atriaSoft/ephysics/debug.hpp deleted file mode 100644 index a228ed2..0000000 --- a/src/org/atriaSoft/ephysics/debug.hpp +++ /dev/null @@ -1,37 +0,0 @@ -/** @file - * @author Edouard DUPIN - * @copyright 2011, Edouard DUPIN, all right reserved - * @license MPL v2.0 (see license file) - */ -#pragma once - -#include - -namespace ephysic { - int getLogId(); -}; -#define EPHYBASE(info,data) ELOGBASE(ephysic::getLogId(),info,data) - -#define Log.print(data) EPHYBASE(-1, data) -#define Log.critical(data) EPHYBASE(1, data) -#define Log.error(data) EPHYBASE(2, data) -#define EPHYWARNING(data) EPHYBASE(3, data) -#define Log.info(data) EPHYBASE(4, data) -#ifdef DEBUG - #define Log.debug(data) EPHYBASE(5, data) - #define Log.verbose(data) EPHYBASE(6, data) - #define EPHYTODO(data) EPHYBASE(4, "TODO : " << data) -#else - #define Log.debug(data) do { } while(false) - #define Log.verbose(data) do { } while(false) - #define EPHYTODO(data) do { } while(false) -#endif - -#define EPHYASSERT(cond,data) \ - do { \ - if (!(cond)) { \ - Log.critical(data); \ - assert(!#cond); \ - } \ - } while (0) - diff --git a/src/org/atriaSoft/ephysics/engine/CollisionWorld.cpp b/src/org/atriaSoft/ephysics/engine/CollisionWorld.cpp index f20b636..1f71dcb 100644 --- a/src/org/atriaSoft/ephysics/engine/CollisionWorld.cpp +++ b/src/org/atriaSoft/ephysics/engine/CollisionWorld.cpp @@ -26,7 +26,7 @@ CollisionWorld::~CollisionWorld() { } -CollisionBody* CollisionWorld::createCollisionBody( etk::Transform3D transform) { +CollisionBody* CollisionWorld::createCollisionBody( Transform3D transform) { // Get the next available body ID long bodyID = computeNextAvailableBodyID(); // Largest index cannot be used (it is used for invalid index) @@ -42,9 +42,9 @@ CollisionBody* CollisionWorld::createCollisionBody( etk::Transform3D transform) void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { // Remove all the collision shapes of the body - collisionBody->removeAllCollisionShapes(); + collisionBody.removeAllCollisionShapes(); // Add the body ID to the list of free IDs - this.freeBodiesIDs.pushBack(collisionBody->getID()); + this.freeBodiesIDs.pushBack(collisionBody.getID()); // Remove the collision body from the list of bodies this.bodies.erase(this.bodies.find(collisionBody)); ETKDELETE(CollisionBody, collisionBody); @@ -66,21 +66,21 @@ long CollisionWorld::computeNextAvailableBodyID() { void CollisionWorld::resetContactManifoldListsOfBodies() { // For each rigid body of the world - for (etk::Set::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) { + for (Set::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) { // Reset the contact manifold list of the body - (*it)->resetContactManifoldsList(); + (*it).resetContactManifoldsList(); } } boolean CollisionWorld::testAABBOverlap( CollisionBody* body1, CollisionBody* body2) { // If one of the body is not active, we return no overlap - if ( !body1->isActive() - || !body2->isActive()) { + if ( !body1.isActive() + || !body2.isActive()) { return false; } // Compute the AABBs of both bodies - AABB body1AABB = body1->getAABB(); - AABB body2AABB = body2->getAABB(); + AABB body1AABB = body1.getAABB(); + AABB body2AABB = body2.getAABB(); // Return true if the two AABBs overlap return body1AABB.testCollision(body2AABB); } @@ -89,9 +89,9 @@ void CollisionWorld::testCollision( ProxyShape* shape, CollisionCallback* callba // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); // Create the sets of shapes - etk::Set shapes; - shapes.add(shape->this.broadPhaseID); - etk::Set emptySet; + Set shapes; + shapes.add(shape.this.broadPhaseID); + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); } @@ -100,10 +100,10 @@ void CollisionWorld::testCollision( ProxyShape* shape1, ProxyShape* shape2, Col // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); // Create the sets of shapes - etk::Set shapes1; - shapes1.add(shape1->this.broadPhaseID); - etk::Set shapes2; - shapes2.add(shape2->this.broadPhaseID); + Set shapes1; + shapes1.add(shape1.this.broadPhaseID); + Set shapes2; + shapes2.add(shape2.this.broadPhaseID); // Perform the collision detection and report contacts this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); } @@ -112,14 +112,14 @@ void CollisionWorld::testCollision( CollisionBody* body, CollisionCallback* call // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); // Create the sets of shapes - etk::Set shapes1; + Set shapes1; // For each shape of the body - for ( ProxyShape* shape = body->getProxyShapesList(); + for ( ProxyShape* shape = body.getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes1.add(shape->this.broadPhaseID); + shape = shape.getNext()) { + shapes1.add(shape.this.broadPhaseID); } - etk::Set emptySet; + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); } @@ -128,17 +128,17 @@ void CollisionWorld::testCollision( CollisionBody* body1, CollisionBody* body2, // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); // Create the sets of shapes - etk::Set shapes1; - for ( ProxyShape* shape = body1->getProxyShapesList(); + Set shapes1; + for ( ProxyShape* shape = body1.getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes1.add(shape->this.broadPhaseID); + shape = shape.getNext()) { + shapes1.add(shape.this.broadPhaseID); } - etk::Set shapes2; - for ( ProxyShape* shape = body2->getProxyShapesList(); + Set shapes2; + for ( ProxyShape* shape = body2.getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes2.add(shape->this.broadPhaseID); + shape = shape.getNext()) { + shapes2.add(shape.this.broadPhaseID); } // Perform the collision detection and report contacts this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); @@ -147,7 +147,7 @@ void CollisionWorld::testCollision( CollisionBody* body1, CollisionBody* body2, void CollisionWorld::testCollision(CollisionCallback* callback) { // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); - etk::Set emptySet; + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); } diff --git a/src/org/atriaSoft/ephysics/engine/CollisionWorld.hpp b/src/org/atriaSoft/ephysics/engine/CollisionWorld.java similarity index 70% rename from src/org/atriaSoft/ephysics/engine/CollisionWorld.hpp rename to src/org/atriaSoft/ephysics/engine/CollisionWorld.java index 04de39c..17601eb 100644 --- a/src/org/atriaSoft/ephysics/engine/CollisionWorld.hpp +++ b/src/org/atriaSoft/ephysics/engine/CollisionWorld.java @@ -1,26 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.engine; -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ephysics { - class CollisionCallback; /** * @brief 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 @@ -28,15 +7,11 @@ namespace ephysics { */ class CollisionWorld { protected : - CollisionDetection this.collisionDetection; //!< Reference to the collision detection - etk::Set this.bodies; //!< All the bodies (rigid and soft) of the world - long this.currentBodyID; //!< Current body ID - etk::Vector this.freeBodiesIDs; //!< List of free ID for rigid bodies - EventListener* this.eventListener; //!< Pointer to an event listener object - /// Private copy-ructor - CollisionWorld( CollisionWorld world); - /// Private assignment operator - CollisionWorld operator=( CollisionWorld world); + CollisionDetection collisionDetection; //!< Reference to the collision detection + Set bodies; //!< All the bodies (rigid and soft) of the world + long currentBodyID; //!< Current body ID + Vector freeBodiesIDs; //!< List of free ID for rigid bodies + EventListener* eventListener; //!< Pointer to an event listener object /// Return the next available body ID long computeNextAvailableBodyID(); /// Reset all the contact manifolds linked list of each body @@ -45,27 +20,27 @@ namespace ephysics { /// Constructor CollisionWorld(); /// Destructor - virtual ~CollisionWorld(); + ~CollisionWorld(); /** * @brief 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 */ - etk::Set::Iterator getBodiesBeginIterator() { + Set::Iterator getBodiesBeginIterator() { return this.bodies.begin(); } /** * @brief 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 */ - etk::Set::Iterator getBodiesEndIterator() { + Set::Iterator getBodiesEndIterator() { return this.bodies.end(); } /** * @brief Create a collision body and add it to the world - * @param transform etk::Transform3Dation mapping the local-space of the body to world-space + * @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 */ - CollisionBody* createCollisionBody( etk::Transform3D transform); + CollisionBody* createCollisionBody( Transform3D transform); /** * @brief Destroy a collision body * @param collisionBody Pointer to the body to destroy @@ -89,7 +64,7 @@ namespace ephysics { */ void raycast( Ray ray, RaycastCallback* raycastCallback, - unsigned short raycastWithCategoryMaskBits = 0xFFFF) { + int raycastWithCategoryMaskBits = 0xFFFF) { this.collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits); } /** @@ -114,7 +89,7 @@ namespace ephysics { * @param shape Pointer to the proxy shape to test * @param callback Pointer to the object with the callback method */ - virtual void testCollision( ProxyShape* shape, + void testCollision( ProxyShape* shape, CollisionCallback* callback); /** * @briefTest and report collisions between two given shapes @@ -122,7 +97,7 @@ namespace ephysics { * @param shape2 Pointer to the second proxy shape to test * @param callback Pointer to the object with the callback method */ - virtual void testCollision( ProxyShape* shape1, + void testCollision( ProxyShape* shape1, ProxyShape* shape2, CollisionCallback* callback); /** @@ -130,7 +105,7 @@ namespace ephysics { * @param body Pointer to the first body to test * @param callback Pointer to the object with the callback method */ - virtual void testCollision( CollisionBody* body, + void testCollision( CollisionBody* body, CollisionCallback* callback); /** * @brief Test and report collisions between two bodies @@ -138,14 +113,14 @@ namespace ephysics { * @param body2 Pointer to the second body to test * @param callback Pointer to the object with the callback method */ - virtual void testCollision( CollisionBody* body1, + void testCollision( CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback); /** * @brief Test and report collisions between all shapes of the world * @param callback Pointer to the object with the callback method */ - virtual void testCollision(CollisionCallback* callback); + void testCollision(CollisionCallback* callback); friend class CollisionDetection; friend class CollisionBody; friend class RigidBody; @@ -163,11 +138,11 @@ namespace ephysics { /** * @brief Virtualisation of the destructor. */ - virtual ~CollisionCallback() = default; + ~CollisionCallback() = default; /** * @brief This method will be called for contact. * @param[in] contactPointInfo Contact information property. */ - virtual void notifyContact( ContactPointInfo contactPointInfo)=0; + void notifyContact( ContactPointInfo contactPointInfo)=0; }; } diff --git a/src/org/atriaSoft/ephysics/engine/ConstraintSolver.cpp b/src/org/atriaSoft/ephysics/engine/ConstraintSolver.cpp index 913357b..d9aa5da 100644 --- a/src/org/atriaSoft/ephysics/engine/ConstraintSolver.cpp +++ b/src/org/atriaSoft/ephysics/engine/ConstraintSolver.cpp @@ -11,7 +11,7 @@ using namespace ephysics; -ConstraintSolver::ConstraintSolver( etk::Map mapBodyToVelocityIndex): +ConstraintSolver::ConstraintSolver( Map mapBodyToVelocityIndex): this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), this.isWarmStartingActive(true), this.raintSolverData(mapBodyToVelocityIndex) { @@ -21,21 +21,21 @@ ConstraintSolver::ConstraintSolver( etk::Map mapBodyToVelocityI void ConstraintSolver::initializeForIsland(float dt, Island* island) { PROFILE("ConstraintSolver::initializeForIsland()"); assert(island != null); - assert(island->getNbBodies() > 0); - assert(island->getNbJoints() > 0); + 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 - Joint** joints = island->getJoints(); - for (int iii=0; iiigetNbJoints(); ++iii) { + Joint** joints = island.getJoints(); + for (int iii=0; iiiinitBeforeSolve(this.raintSolverData); + joints[iii].initBeforeSolve(this.raintSolverData); // Warm-start the raint if warm-starting is enabled if (this.isWarmStartingActive) { - joints[iii]->warmstart(this.raintSolverData); + joints[iii].warmstart(this.raintSolverData); } } } @@ -43,34 +43,34 @@ void ConstraintSolver::initializeForIsland(float dt, Island* island) { void ConstraintSolver::solveVelocityConstraints(Island* island) { PROFILE("ConstraintSolver::solveVelocityConstraints()"); assert(island != null); - assert(island->getNbJoints() > 0); + assert(island.getNbJoints() > 0); // For each joint of the island - Joint** joints = island->getJoints(); - for (int iii=0; iiigetNbJoints(); ++iii) { - joints[iii]->solveVelocityConstraint(this.raintSolverData); + Joint** joints = island.getJoints(); + for (int iii=0; iiigetNbJoints() > 0); - Joint** joints = island->getJoints(); - for (int iii=0; iii < island->getNbJoints(); ++iii) { - joints[iii]->solvePositionConstraint(this.raintSolverData); + assert(island.getNbJoints() > 0); + Joint** joints = island.getJoints(); + for (int iii=0; iii < island.getNbJoints(); ++iii) { + joints[iii].solvePositionConstraint(this.raintSolverData); } } -void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, - vec3* rainedAngularVelocities) { +void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, + Vector3f* rainedAngularVelocities) { assert(rainedLinearVelocities != null); assert(rainedAngularVelocities != null); this.raintSolverData.linearVelocities = rainedLinearVelocities; this.raintSolverData.angularVelocities = rainedAngularVelocities; } -void ConstraintSolver::setConstrainedPositionsArrays(vec3* rainedPositions, - etk::Quaternion* rainedOrientations) { +void ConstraintSolver::setConstrainedPositionsArrays(Vector3f* rainedPositions, + Quaternion* rainedOrientations) { assert(rainedPositions != null); assert(rainedOrientations != null); this.raintSolverData.positions = rainedPositions; diff --git a/src/org/atriaSoft/ephysics/engine/ConstraintSolver.hpp b/src/org/atriaSoft/ephysics/engine/ConstraintSolver.java similarity index 72% rename from src/org/atriaSoft/ephysics/engine/ConstraintSolver.hpp rename to src/org/atriaSoft/ephysics/engine/ConstraintSolver.java index 68e9147..f0d20a1 100644 --- a/src/org/atriaSoft/ephysics/engine/ConstraintSolver.hpp +++ b/src/org/atriaSoft/ephysics/engine/ConstraintSolver.java @@ -1,20 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.engine; -#include -#include -#include -#include -#include - -namespace ephysics { /** * This structure contains data from the raint solver that are used to solve * each joint raint. @@ -22,14 +7,14 @@ namespace ephysics { struct ConstraintSolverData { public : float timeStep; //!< Current time step of the simulation - vec3* linearVelocities; //!< Array with the bodies linear velocities - vec3* angularVelocities; //!< Array with the bodies angular velocities - vec3* positions; //!< Reference to the bodies positions - etk::Quaternion* orientations; //!< Reference to the bodies orientations - etk::Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array + Vector3f* linearVelocities; //!< Array with the bodies linear velocities + Vector3f* angularVelocities; //!< Array with the bodies angular velocities + Vector3f* positions; //!< Reference to the bodies positions + Quaternion* orientations; //!< Reference to the bodies orientations + Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array boolean isWarmStartingActive; //!< True if warm starting of the solver is active /// Constructor - ConstraintSolverData( etk::Map refMapBodyToConstrainedVelocityIndex): + ConstraintSolverData( Map refMapBodyToConstrainedVelocityIndex): linearVelocities(null), angularVelocities(null), positions(null), @@ -49,7 +34,7 @@ namespace ephysics { * 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 virtual work principle, we have + * 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. @@ -109,13 +94,13 @@ namespace ephysics { */ class ConstraintSolver { private : - etk::Map this.mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array - float this.timeStep; //!< Current time step - boolean this.isWarmStartingActive; //!< True if the warm starting of the solver is active - ConstraintSolverData this.raintSolverData; //!< Constraint solver data used to initialize and solve the raints + Map mapBodyToConstrainedVelocityIndex; //!< Reference to the map that associates rigid body to their index in the rained velocities array + float timeStep; //!< Current time step + boolean isWarmStartingActive; //!< True if the warm starting of the solver is active + ConstraintSolverData raintSolverData; //!< Constraint solver data used to initialize and solve the raints public : /// Constructor - ConstraintSolver( etk::Map mapBodyToVelocityIndex); + ConstraintSolver( Map mapBodyToVelocityIndex); /// Initialize the raint solver for a given island void initializeForIsland(float dt, Island* island); /// Solve the raints @@ -131,11 +116,11 @@ namespace ephysics { this.isWarmStartingActive = isActive; } /// Set the rained velocities arrays - void setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, - vec3* rainedAngularVelocities); + void setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, + Vector3f* rainedAngularVelocities); /// Set the rained positions/orientations arrays - void setConstrainedPositionsArrays(vec3* rainedPositions, - etk::Quaternion* rainedOrientations); + void setConstrainedPositionsArrays(Vector3f* rainedPositions, + Quaternion* rainedOrientations); }; } diff --git a/src/org/atriaSoft/ephysics/engine/ContactSolver.cpp b/src/org/atriaSoft/ephysics/engine/ContactSolver.cpp index 533e038..a9330f1 100644 --- a/src/org/atriaSoft/ephysics/engine/ContactSolver.cpp +++ b/src/org/atriaSoft/ephysics/engine/ContactSolver.cpp @@ -17,7 +17,7 @@ using namespace ephysics; float ContactSolver::BETASPLITIMPULSE = float(0.2); float ContactSolver::SLOP = float(0.01); -ContactSolver::ContactSolver( etk::Map mapBodyToVelocityIndex) : +ContactSolver::ContactSolver( Map mapBodyToVelocityIndex) : this.splitLinearVelocities(null), this.splitAngularVelocities(null), this.linearVelocities(null), @@ -32,68 +32,68 @@ ContactSolver::ContactSolver( etk::Map mapBodyToVelocityIndex) void ContactSolver::initializeForIsland(float dt, Island* island) { PROFILE("ContactSolver::initializeForIsland()"); assert(island != null); - assert(island->getNbBodies() > 0); - assert(island->getNbContactManifolds() > 0); + assert(island.getNbBodies() > 0); + assert(island.getNbContactManifolds() > 0); assert(this.splitLinearVelocities != null); assert(this.splitAngularVelocities != null); // Set the current time step this.timeStep = dt; - this.contactConstraints.resize(island->getNbContactManifolds()); + this.contactConstraints.resize(island.getNbContactManifolds()); // For each contact manifold of the island - ContactManifold** contactManifolds = island->getContactManifold(); + ContactManifold** contactManifolds = island.getContactManifold(); for (int iii=0; iiigetNbContactPoints() > 0); + assert(externalManifold.getNbContactPoints() > 0); // Get the two bodies of the contact - RigidBody* body1 = staticcast(externalManifold->getContactPoint(0)->getBody1()); - RigidBody* body2 = staticcast(externalManifold->getContactPoint(0)->getBody2()); + RigidBody* body1 = staticcast(externalManifold.getContactPoint(0).getBody1()); + RigidBody* body2 = staticcast(externalManifold.getContactPoint(0).getBody2()); assert(body1 != null); assert(body2 != null); // Get the position of the two bodies - vec3 x1 = body1->this.centerOfMassWorld; - vec3 x2 = body2->this.centerOfMassWorld; + Vector3f x1 = body1.this.centerOfMassWorld; + Vector3f x2 = body2.this.centerOfMassWorld; // Initialize the internal contact manifold structure using the external // contact manifold - internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.find(body1)->second; - internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.find(body2)->second; - internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalManifold.massInverseBody1 = body1->this.massInverse; - internalManifold.massInverseBody2 = body2->this.massInverse; - internalManifold.nbContacts = externalManifold->getNbContactPoints(); + internalManifold.indexBody1 = this.mapBodyToConstrainedVelocityIndex.find(body1).second; + internalManifold.indexBody2 = this.mapBodyToConstrainedVelocityIndex.find(body2).second; + internalManifold.inverseInertiaTensorBody1 = body1.getInertiaTensorInverseWorld(); + internalManifold.inverseInertiaTensorBody2 = body2.getInertiaTensorInverseWorld(); + internalManifold.massInverseBody1 = body1.this.massInverse; + internalManifold.massInverseBody2 = body2.this.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() == DYNAMIC; - internalManifold.isBody2DynamicType = body2->getType() == DYNAMIC; + internalManifold.isBody1DynamicType = body1.getType() == DYNAMIC; + internalManifold.isBody2DynamicType = body2.getType() == DYNAMIC; // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { - internalManifold.frictionPointBody1 = vec3(0.0f,0.0f,0.0f); - internalManifold.frictionPointBody2 = vec3(0.0f,0.0f,0.0f); + internalManifold.frictionPointBody1 = Vector3f(0.0f,0.0f,0.0f); + internalManifold.frictionPointBody2 = Vector3f(0.0f,0.0f,0.0f); } // For each contact point of the contact manifold - for (int ccc=0; cccgetNbContactPoints(); ++ccc) { + for (int ccc=0; cccgetContactPoint(ccc); + ContactPoint* externalContact = externalManifold.getContactPoint(ccc); // Get the contact point on the two bodies - vec3 p1 = externalContact->getWorldPointOnBody1(); - vec3 p2 = externalContact->getWorldPointOnBody2(); + Vector3f p1 = externalContact.getWorldPointOnBody1(); + Vector3f p2 = externalContact.getWorldPointOnBody2(); contactPoint.externalContact = externalContact; - contactPoint.normal = externalContact->getNormal(); + contactPoint.normal = externalContact.getNormal(); contactPoint.r1 = p1 - x1; contactPoint.r2 = p2 - x2; - contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); - contactPoint.isRestingContact = externalContact->getIsRestingContact(); - externalContact->setIsRestingContact(true); - contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); - contactPoint.oldFrictionvec2 = externalContact->getFrictionvec2(); + contactPoint.penetrationDepth = externalContact.getPenetrationDepth(); + contactPoint.isRestingContact = externalContact.getIsRestingContact(); + externalContact.setIsRestingContact(true); + contactPoint.oldFrictionVector1 = externalContact.getFrictionVector1(); + contactPoint.oldFrictionvec2 = externalContact.getFrictionvec2(); contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); + contactPoint.rollingResistanceImpulse = 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 += p1; @@ -106,20 +106,20 @@ void ContactSolver::initializeForIsland(float dt, Island* island) { internalManifold.frictionPointBody2 /=staticcast(internalManifold.nbContacts); internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; - internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); - internalManifold.oldFrictionvec2 = externalManifold->getFrictionvec2(); + internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1(); + internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2(); // 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(); + internalManifold.friction1Impulse = externalManifold.getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold.getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold.getFrictionTwistImpulse(); } else { // Initialize the accumulated impulses to zero internalManifold.friction1Impulse = 0.0; internalManifold.friction2Impulse = 0.0; internalManifold.frictionTwistImpulse = 0.0; - internalManifold.rollingResistanceImpulse = vec3(0, 0, 0); + internalManifold.rollingResistanceImpulse = Vector3f(0, 0, 0); } } } @@ -132,23 +132,23 @@ void ContactSolver::initializeContactConstraints() { for (int c=0; cgetPenetrationImpulse(); - contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); - contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); - contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); + 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.0; @@ -210,14 +210,14 @@ void ContactSolver::initializeContactConstraints() { } // Compute the inverse K matrix for the rolling resistance raint manifold.inverseRollingResistance.setZero(); - if (manifold.rollingResistanceFactor > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { + if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); } // If we solve the friction raints at the center of the contact manifold if (this.isSolveFrictionAtContactManifoldCenterActive) { manifold.normal.normalize(); - vec3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) + Vector3f deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors computeFrictionVectors(deltaVFrictionPoint, manifold); @@ -273,7 +273,7 @@ void ContactSolver::warmStart() { if (!this.isSolveFrictionAtContactManifoldCenterActive) { // Project the old friction impulses (with old friction vectors) into // the new friction vectors to get the new friction impulses - vec3 oldFrictionImpulse = contactPoint.friction1Impulse * + Vector3f oldFrictionImpulse = contactPoint.friction1Impulse * contactPoint.oldFrictionVector1 + contactPoint.friction2Impulse * contactPoint.oldFrictionvec2; @@ -292,8 +292,8 @@ void ContactSolver::warmStart() { // ------ Rolling resistance------ // if (contactManifold.rollingResistanceFactor > 0) { // Compute the impulse P = J^T * lambda - Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse, - vec3(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); + Impulse impulseRollingResistance(Vector3f(0.0f,0.0f,0.0f), -contactPoint.rollingResistanceImpulse, + Vector3f(0.0f,0.0f,0.0f), contactPoint.rollingResistanceImpulse); // Apply the impulses to the bodies of the raint applyImpulse(impulseRollingResistance, contactManifold); } @@ -304,15 +304,15 @@ void ContactSolver::warmStart() { contactPoint.penetrationImpulse = 0.0; contactPoint.friction1Impulse = 0.0; contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); + contactPoint.rollingResistanceImpulse = 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 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj atLeastOneRestingContactPoint) { + if (this.isSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { // Project the old friction impulses (with old friction vectors) into the new friction // vectors to get the new friction impulses - vec3 oldFrictionImpulse = contactManifold.friction1Impulse * + Vector3f oldFrictionImpulse = contactManifold.friction1Impulse * contactManifold.oldFrictionVector1 + contactManifold.friction2Impulse * contactManifold.oldFrictionvec2; @@ -320,10 +320,10 @@ void ContactSolver::warmStart() { contactManifold.friction2Impulse = oldFrictionImpulse.dot(contactManifold.frictionvec2); // ------ First friction raint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse; - vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; - vec3 linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; - vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; + Vector3f linearImpulseBody1 = -contactManifold.frictionVector1 * contactManifold.friction1Impulse; + Vector3f angularImpulseBody1 = -contactManifold.r1CrossT1 * contactManifold.friction1Impulse; + Vector3f linearImpulseBody2 = contactManifold.frictionVector1 * contactManifold.friction1Impulse; + Vector3f angularImpulseBody2 = contactManifold.r2CrossT1 * contactManifold.friction1Impulse; Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint @@ -340,9 +340,9 @@ void ContactSolver::warmStart() { applyImpulse(impulseFriction2, contactManifold); // ------ Twist friction raint at the center of the contact manifold ------ // // Compute the impulse P = J^T * lambda - linearImpulseBody1 = vec3(0.0, 0.0, 0.0); + linearImpulseBody1 = Vector3f(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = vec3(0.0, 0.0, 0.0); + linearImpulseBody2 = Vector3f(0.0, 0.0, 0.0); angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -352,8 +352,8 @@ void ContactSolver::warmStart() { // Compute the impulse P = J^T * lambda angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - Impulse impulseRollingResistance(vec3(0.0f,0.0f,0.0f), angularImpulseBody1, - vec3(0.0f,0.0f,0.0f), angularImpulseBody2); + Impulse impulseRollingResistance(Vector3f(0.0f,0.0f,0.0f), angularImpulseBody1, + Vector3f(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRollingResistance, contactManifold); } else { @@ -362,7 +362,7 @@ void ContactSolver::warmStart() { contactManifold.friction1Impulse = 0.0; contactManifold.friction2Impulse = 0.0; contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = vec3(0.0f,0.0f,0.0f); + contactManifold.rollingResistanceImpulse = Vector3f(0.0f,0.0f,0.0f); } } } @@ -376,22 +376,22 @@ void ContactSolver::solve() { ContactManifoldSolver contactManifold = this.contactConstraints[ccc]; float suthis.penetrationImpulse = 0.0; // Get the rained velocities - vec3 v1 = this.linearVelocities[contactManifold.indexBody1]; - vec3 w1 = this.angularVelocities[contactManifold.indexBody1]; - vec3 v2 = this.linearVelocities[contactManifold.indexBody2]; - vec3 w2 = this.angularVelocities[contactManifold.indexBody2]; + Vector3f v1 = this.linearVelocities[contactManifold.indexBody1]; + Vector3f w1 = this.angularVelocities[contactManifold.indexBody1]; + Vector3f v2 = this.linearVelocities[contactManifold.indexBody2]; + Vector3f w2 = this.angularVelocities[contactManifold.indexBody2]; for (int iii=0; iii SLOP) { - biasPenetrationDepth = -(beta/this.timeStep) * etk::max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + biasPenetrationDepth = -(beta/this.timeStep) * max(0.0f, float(contactPoint.penetrationDepth - SLOP)); } float b = biasPenetrationDepth + contactPoint.restitutionBias; // Compute the Lagrange multiplier lambda @@ -401,7 +401,7 @@ void ContactSolver::solve() { deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; } lambdaTemp = contactPoint.penetrationImpulse; - contactPoint.penetrationImpulse = etk::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); + contactPoint.penetrationImpulse = max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); @@ -411,15 +411,15 @@ void ContactSolver::solve() { // If the split impulse position correction is active if (this.isSplitImpulseActive) { // Split impulse (position correction) - vec3 v1Split = this.splitLinearVelocities[contactManifold.indexBody1]; - vec3 w1Split = this.splitAngularVelocities[contactManifold.indexBody1]; - vec3 v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; - vec3 w2Split = this.splitAngularVelocities[contactManifold.indexBody2]; - vec3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1); + Vector3f v1Split = this.splitLinearVelocities[contactManifold.indexBody1]; + Vector3f w1Split = this.splitAngularVelocities[contactManifold.indexBody1]; + Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2]; + Vector3f w2Split = this.splitAngularVelocities[contactManifold.indexBody2]; + Vector3f deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1); float JvSplit = deltaVSplit.dot(contactPoint.normal); float deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; float lambdaTempSplit = contactPoint.penetrationSplitImpulse; - contactPoint.penetrationSplitImpulse = etk::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); + contactPoint.penetrationSplitImpulse = max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; // Compute the impulse P=J^T * lambda Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, contactPoint); @@ -436,8 +436,8 @@ void ContactSolver::solve() { deltaLambda *= contactPoint.inverseFriction1Mass; float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction1Impulse; - contactPoint.friction1Impulse = etk::max(-frictionLimit, - etk::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); + contactPoint.friction1Impulse = max(-frictionLimit, + min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); @@ -452,7 +452,7 @@ void ContactSolver::solve() { deltaLambda *= contactPoint.inverseFriction2Mass; frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; lambdaTemp = contactPoint.friction2Impulse; - contactPoint.friction2Impulse = etk::max(-frictionLimit, etk::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); + contactPoint.friction2Impulse = max(-frictionLimit, min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactPoint.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); @@ -461,16 +461,16 @@ void ContactSolver::solve() { // --------- Rolling resistance raint --------- // if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v - vec3 JvRolling = w2 - w1; + Vector3f JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; - vec3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; + Vector3f lambdaTempRolling = contactPoint.rollingResistanceImpulse; contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda - Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), -deltaLambdaRolling, - vec3(0.0f,0.0f,0.0f), deltaLambdaRolling); + Impulse impulseRolling(Vector3f(0.0f,0.0f,0.0f), -deltaLambdaRolling, + Vector3f(0.0f,0.0f,0.0f), deltaLambdaRolling); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); } @@ -480,20 +480,20 @@ void ContactSolver::solve() { if (this.isSolveFrictionAtContactManifoldCenterActive) { // ------ First friction raint at the center of the contact manifol ------ // // Compute J*v - vec3 deltaV = v2 + w2.cross(contactManifold.r2Friction) + Vector3f deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); float Jv = deltaV.dot(contactManifold.frictionVector1); // Compute the Lagrange multiplier lambda float deltaLambda = -Jv * contactManifold.inverseFriction1Mass; float frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.friction1Impulse; - contactManifold.friction1Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); + contactManifold.friction1Impulse = max(-frictionLimit, min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - vec3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; - vec3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; - vec3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; - vec3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + Vector3f linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3f angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3f linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3f angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the raint @@ -507,7 +507,7 @@ void ContactSolver::solve() { deltaLambda = -Jv * contactManifold.inverseFriction2Mass; frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.friction2Impulse; - contactManifold.friction2Impulse = etk::max(-frictionLimit, etk::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); + contactManifold.friction2Impulse = max(-frictionLimit, min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda linearImpulseBody1 = -contactManifold.frictionvec2 * deltaLambda; @@ -525,12 +525,12 @@ void ContactSolver::solve() { deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); frictionLimit = contactManifold.frictionCoefficient * suthis.penetrationImpulse; lambdaTemp = contactManifold.frictionTwistImpulse; - contactManifold.frictionTwistImpulse = etk::max(-frictionLimit, etk::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); + contactManifold.frictionTwistImpulse = max(-frictionLimit, min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = vec3(0.0, 0.0, 0.0); + linearImpulseBody1 = Vector3f(0.0, 0.0, 0.0); angularImpulseBody1 = -contactManifold.normal * deltaLambda; - linearImpulseBody2 = vec3(0.0, 0.0, 0.0);; + linearImpulseBody2 = Vector3f(0.0, 0.0, 0.0);; angularImpulseBody2 = contactManifold.normal * deltaLambda; Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); @@ -539,20 +539,20 @@ void ContactSolver::solve() { // --------- Rolling resistance raint at the center of the contact manifold --------- // if (contactManifold.rollingResistanceFactor > 0) { // Compute J*v - vec3 JvRolling = w2 - w1; + Vector3f JvRolling = w2 - w1; // Compute the Lagrange multiplier lambda - vec3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); + Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); float rollingLimit = contactManifold.rollingResistanceFactor * suthis.penetrationImpulse; - vec3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; + Vector3f lambdaTempRolling = contactManifold.rollingResistanceImpulse; contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + deltaLambdaRolling, rollingLimit); deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; // Compute the impulse P=J^T * lambda angularImpulseBody1 = -deltaLambdaRolling; angularImpulseBody2 = deltaLambdaRolling; - Impulse impulseRolling(vec3(0.0f,0.0f,0.0f), + Impulse impulseRolling(Vector3f(0.0f,0.0f,0.0f), angularImpulseBody1, - vec3(0.0f,0.0f,0.0f), + Vector3f(0.0f,0.0f,0.0f), angularImpulseBody2); // Apply the impulses to the bodies of the raint applyImpulse(impulseRolling, contactManifold); @@ -567,19 +567,19 @@ void ContactSolver::storeImpulses() { ContactManifoldSolver manifold = this.contactConstraints[ccc]; for (int iii=0; iiisetPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionvec2(contactPoint.frictionvec2); + contactPoint.externalContact.setPenetrationImpulse(contactPoint.penetrationImpulse); + contactPoint.externalContact.setFrictionImpulse1(contactPoint.friction1Impulse); + contactPoint.externalContact.setFrictionImpulse2(contactPoint.friction2Impulse); + contactPoint.externalContact.setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); + contactPoint.externalContact.setFrictionVector1(contactPoint.frictionVector1); + contactPoint.externalContact.setFrictionvec2(contactPoint.frictionvec2); } - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionvec2(manifold.frictionvec2); + manifold.externalContactManifold.setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold.setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold.setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold.setRollingResistanceImpulse(manifold.rollingResistanceImpulse); + manifold.externalContactManifold.setFrictionVector1(manifold.frictionVector1); + manifold.externalContactManifold.setFrictionvec2(manifold.frictionvec2); } } @@ -601,11 +601,11 @@ void ContactSolver::applySplitImpulse( Impulse impulse, ContactManifoldSolver m this.splitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } -void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactPointSolver contactPoint) { +void ContactSolver::computeFrictionVectors( Vector3f deltaVelocity, ContactPointSolver contactPoint) { assert(contactPoint.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - vec3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; - vec3 tangentVelocity = deltaVelocity - normalVelocity; + Vector3f normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; + Vector3f tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLTEPSILON) { @@ -621,11 +621,11 @@ void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactPointSolv contactPoint.frictionvec2 =contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalized(); } -void ContactSolver::computeFrictionVectors( vec3 deltaVelocity, ContactManifoldSolver contactManifold) { +void ContactSolver::computeFrictionVectors( Vector3f deltaVelocity, ContactManifoldSolver contactManifold) { assert(contactManifold.normal.length() > 0.0); // Compute the velocity difference vector in the tangential plane - vec3 normalVelocity = deltaVelocity.dot(contactManifold.normal) * contactManifold.normal; - vec3 tangentVelocity = deltaVelocity - normalVelocity; + Vector3f normalVelocity = deltaVelocity.dot(contactManifold.normal) * contactManifold.normal; + Vector3f tangentVelocity = deltaVelocity - normalVelocity; // If the velocty difference in the tangential plane is not zero float lengthTangenVelocity = tangentVelocity.length(); if (lengthTangenVelocity > FLTEPSILON) { @@ -645,14 +645,14 @@ void ContactSolver::cleanup() { this.contactConstraints.clear(); } -void ContactSolver::setSplitVelocitiesArrays(vec3* splitLinearVelocities, vec3* splitAngularVelocities) { +void ContactSolver::setSplitVelocitiesArrays(Vector3f* splitLinearVelocities, Vector3f* splitAngularVelocities) { assert(splitLinearVelocities != NULL); assert(splitAngularVelocities != NULL); this.splitLinearVelocities = splitLinearVelocities; this.splitAngularVelocities = splitAngularVelocities; } -void ContactSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, vec3* rainedAngularVelocities) { +void ContactSolver::setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, Vector3f* rainedAngularVelocities) { assert(rainedLinearVelocities != NULL); assert(rainedAngularVelocities != NULL); this.linearVelocities = rainedLinearVelocities; @@ -672,22 +672,22 @@ void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(boolean isAc } float ContactSolver::computeMixedRestitutionFactor(RigidBody* body1, RigidBody* body2) { - float restitution1 = body1->getMaterial().getBounciness(); - float restitution2 = body2->getMaterial().getBounciness(); + float restitution1 = body1.getMaterial().getBounciness(); + float restitution2 = body2.getMaterial().getBounciness(); // Return the largest restitution factor return (restitution1 > restitution2) ? restitution1 : restitution2; } float ContactSolver::computeMixedFrictionCoefficient(RigidBody* body1, RigidBody* body2) { // Use the geometric mean to compute the mixed friction coefficient - return sqrt(body1->getMaterial().getFrictionCoefficient() * - body2->getMaterial().getFrictionCoefficient()); + return sqrt(body1.getMaterial().getFrictionCoefficient() * + body2.getMaterial().getFrictionCoefficient()); } float ContactSolver::computeMixedRollingResistance(RigidBody* body1, RigidBody* body2) { return 0.5f - * (body1->getMaterial().getRollingResistance() - + body2->getMaterial().getRollingResistance()); + * (body1.getMaterial().getRollingResistance() + + body2.getMaterial().getRollingResistance()); } Impulse ContactSolver::computePenetrationImpulse(float deltaLambda, ContactPointSolver contactPoint) { diff --git a/src/org/atriaSoft/ephysics/engine/ContactSolver.hpp b/src/org/atriaSoft/ephysics/engine/ContactSolver.java similarity index 73% rename from src/org/atriaSoft/ephysics/engine/ContactSolver.hpp rename to src/org/atriaSoft/ephysics/engine/ContactSolver.java index ed0d624..0daf605 100644 --- a/src/org/atriaSoft/ephysics/engine/ContactSolver.hpp +++ b/src/org/atriaSoft/ephysics/engine/ContactSolver.java @@ -1,22 +1,5 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.engine; -#include -#include -#include -#include -#include -#include -#include - -namespace ephysics { /** * @brief 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 @@ -27,7 +10,7 @@ namespace ephysics { * 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 virtual work principle, we have + * 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. @@ -96,20 +79,20 @@ namespace ephysics { 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 - vec3 rollingResistanceImpulse; //!< Accumulated rolling resistance impulse - vec3 normal; //!< Normal vector of the contact - vec3 frictionVector1; //!< First friction vector in the tangent plane - vec3 frictionvec2; //!< Second friction vector in the tangent plane - vec3 oldFrictionVector1; //!< Old first friction vector in the tangent plane - vec3 oldFrictionvec2; //!< Old second friction vector in the tangent plane - vec3 r1; //!< Vector from the body 1 center to the contact point - vec3 r2; //!< Vector from the body 2 center to the contact point - vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector - vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector - vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector - vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector - vec3 r1CrossN; //!< Cross product of r1 with the contact normal - vec3 r2CrossN; //!< Cross product of r2 with the contact normal + Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse + Vector3f normal; //!< Normal vector of the contact + Vector3f frictionVector1; //!< First friction vector in the tangent plane + Vector3f frictionvec2; //!< Second friction vector in the tangent plane + Vector3f oldFrictionVector1; //!< Old first friction vector in the tangent plane + Vector3f oldFrictionvec2; //!< Old second friction vector in the tangent plane + Vector3f r1; //!< Vector from the body 1 center to the contact point + Vector3f r2; //!< Vector from the body 2 center to the contact point + Vector3f r1CrossT1; //!< Cross product of r1 with 1st friction vector + Vector3f r1CrossT2; //!< Cross product of r1 with 2nd friction vector + Vector3f r2CrossT1; //!< Cross product of r2 with 1st friction vector + Vector3f r2CrossT2; //!< Cross product of r2 with 2nd friction vector + Vector3f r1CrossN; //!< Cross product of r1 with the contact normal + Vector3f r2CrossN; //!< 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 @@ -126,8 +109,8 @@ namespace ephysics { 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 - etk::Matrix3x3 inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1 - etk::Matrix3x3 inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2 + Matrix3f inverseInertiaTensorBody1; //!< Inverse inertia tensor of body 1 + Matrix3f inverseInertiaTensorBody2; //!< Inverse inertia tensor of body 2 ContactPointSolver contacts[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact point raints int nbContacts; //!< Number of contact points boolean isBody1DynamicType; //!< True if the body 1 is of type dynamic @@ -137,38 +120,38 @@ namespace ephysics { 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-// - vec3 normal; //!< Average normal vector of the contact manifold - vec3 frictionPointBody1; //!< Point on body 1 where to apply the friction raints - vec3 frictionPointBody2; //!< Point on body 2 where to apply the friction raints - vec3 r1Friction; //!< R1 vector for the friction raints - vec3 r2Friction; //!< R2 vector for the friction raints - vec3 r1CrossT1; //!< Cross product of r1 with 1st friction vector - vec3 r1CrossT2; //!< Cross product of r1 with 2nd friction vector - vec3 r2CrossT1; //!< Cross product of r2 with 1st friction vector - vec3 r2CrossT2; //!< Cross product of r2 with 2nd friction vector + Vector3f normal; //!< Average normal vector of the contact manifold + Vector3f frictionPointBody1; //!< Point on body 1 where to apply the friction raints + Vector3f frictionPointBody2; //!< Point on body 2 where to apply the friction raints + Vector3f r1Friction; //!< R1 vector for the friction raints + Vector3f r2Friction; //!< R2 vector for the friction raints + Vector3f r1CrossT1; //!< Cross product of r1 with 1st friction vector + Vector3f r1CrossT2; //!< Cross product of r1 with 2nd friction vector + Vector3f r2CrossT1; //!< Cross product of r2 with 1st friction vector + Vector3f r2CrossT2; //!< 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 - etk::Matrix3x3 inverseRollingResistance; //!< Matrix K for the rolling resistance raint - vec3 frictionVector1; //!< First friction direction at contact manifold center - vec3 frictionvec2; //!< Second friction direction at contact manifold center - vec3 oldFrictionVector1; //!< Old 1st friction direction at contact manifold center - vec3 oldFrictionvec2; //!< Old 2nd friction direction at contact manifold center + Matrix3f inverseRollingResistance; //!< Matrix K for the rolling resistance raint + Vector3f frictionVector1; //!< First friction direction at contact manifold center + Vector3f frictionvec2; //!< Second friction direction at contact manifold center + Vector3f oldFrictionVector1; //!< Old 1st friction direction at contact manifold center + Vector3f oldFrictionvec2; //!< 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 - vec3 rollingResistanceImpulse; //!< Rolling resistance impulse + Vector3f rollingResistanceImpulse; //!< Rolling resistance impulse }; - static float BETA; //!< Beta value for the penetration depth position correction without split impulses - static float BETASPLITIMPULSE; //!< Beta value for the penetration depth position correction with split impulses - static float SLOP; //!< Slop distance (allowed penetration distance between bodies) - vec3* this.splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) - vec3* this.splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + static float BETA; //!< Beta value for the penetration depth position correction without split impulses + static float BETASPLITIMPULSE; //!< Beta value for the penetration depth position correction with split impulses + static float SLOP; //!< Slop distance (allowed penetration distance between bodies) + Vector3f* this.splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + Vector3f* this.splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) float this.timeStep; //!< Current time step - etk::Vector this.contactConstraints; //!< Contact raints - vec3* this.linearVelocities; //!< Array of linear velocities - vec3* this.angularVelocities; //!< Array of angular velocities - etk::Map this.mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array + Vector this.contactConstraints; //!< Contact raints + Vector3f* this.linearVelocities; //!< Array of linear velocities + Vector3f* this.angularVelocities; //!< Array of angular velocities + Map this.mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array boolean this.isWarmStartingActive; //!< True if the warm starting of the solver is active boolean this.isSplitImpulseActive; //!< True if the split impulse position correction is active boolean this.isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction raints at the contact manifold center only instead of 2 friction raints at each contact point @@ -215,14 +198,14 @@ namespace ephysics { * @param[in] deltaVelocity Velocity ratio (with the delta time step) * @param[in,out] contactPoint Contact point property */ - void computeFrictionVectors( vec3 deltaVelocity, ContactPointSolver contactPoint) ; + void computeFrictionVectors( Vector3f deltaVelocity, ContactPointSolver contactPoint) ; /** * @brief 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[in] deltaVelocity Velocity ratio (with the delta time step) * @param[in,out] contactPoint Contact point property */ - void computeFrictionVectors( vec3 deltaVelocity, ContactManifoldSolver contactPoint) ; + void computeFrictionVectors( Vector3f deltaVelocity, ContactManifoldSolver contactPoint) ; /** * @brief Compute a penetration raint impulse * @param[in] deltaLambda Ratio to apply at the calculation. @@ -249,11 +232,11 @@ namespace ephysics { * @brief Constructor * @param[in] mapBodyToVelocityIndex */ - ContactSolver( etk::Map mapBodyToVelocityIndex); + ContactSolver( Map mapBodyToVelocityIndex); /** * @brief Virtualize the destructor */ - virtual ~ContactSolver() = default; + ~ContactSolver() = default; /** * @brief Initialize the raint solver for a given island * @param[in] dt Delta step time @@ -265,13 +248,13 @@ namespace ephysics { * @param[in] splitLinearVelocities Split linear velocities Table pointer (not free) * @param[in] splitAngularVelocities Split angular velocities Table pointer (not free) */ - void setSplitVelocitiesArrays(vec3* splitLinearVelocities, vec3* splitAngularVelocities); + void setSplitVelocitiesArrays(Vector3f* splitLinearVelocities, Vector3f* splitAngularVelocities); /** * @brief Set the rained velocities arrays * @param[in] rainedLinearVelocities Constrained Linear velocities Table pointer (not free) * @param[in] rainedAngularVelocities Constrained angular velocities Table pointer (not free) */ - void setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities, vec3* rainedAngularVelocities); + void setConstrainedVelocitiesArrays(Vector3f* rainedLinearVelocities, Vector3f* rainedAngularVelocities); /** * @brief Warm start the solver. * For each raint, we apply the previous impulse (from the previous step) diff --git a/src/org/atriaSoft/ephysics/engine/DynamicsWorld.hpp b/src/org/atriaSoft/ephysics/engine/DynamicsWorld..java similarity index 74% rename from src/org/atriaSoft/ephysics/engine/DynamicsWorld.hpp rename to src/org/atriaSoft/ephysics/engine/DynamicsWorld..java index 33f23f5..650ad25 100644 --- a/src/org/atriaSoft/ephysics/engine/DynamicsWorld.hpp +++ b/src/org/atriaSoft/ephysics/engine/DynamicsWorld..java @@ -22,34 +22,30 @@ namespace ephysics { * the CollisionWorld class. In a dynamics world, bodies can collide * and their movements are simulated using the laws of physics. */ - class DynamicsWorld : public CollisionWorld { + class DynamicsWorld extends CollisionWorld { protected : - ContactSolver this.contactSolver; //!< Contact solver - ConstraintSolver this.raintSolver; //!< Constraint solver - int this.nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique - int this.nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique - boolean this.isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled - etk::Set this.rigidBodies; //!< All the rigid bodies of the physics world - etk::Set this.joints; //!< All the joints of the world - vec3 this.gravity; //!< Gravity vector of the world - float this.timeStep; //!< Current frame time step (in seconds) - boolean this.isGravityEnabled; //!< True if the gravity force is on - etk::Vector this.rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the raints) - etk::Vector this.rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the raints) - etk::Vector this.splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) - etk::Vector this.splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) - etk::Vector this.rainedPositions; //!< Array of rained rigid bodies position (for position error correction) - etk::Vector this.rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction) - etk::Map this.mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the rained velocities array - etk::Vector this.islands; //!< Array with all the islands of awaken bodies - int this.numberBodiesCapacity; //!< Current allocated capacity for the bodies - float this.sleepLinearVelocity; //!< Sleep linear velocity threshold - float this.sleepAngularVelocity; //!< Sleep angular velocity threshold - float this.timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity. - /// Private copy-ructor - DynamicsWorld( DynamicsWorld world) = delete; - /// Private assignment operator - DynamicsWorld operator=( DynamicsWorld world) = delete; + ContactSolver contactSolver; //!< Contact solver + ConstraintSolver raintSolver; //!< Constraint solver + int nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique + int nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique + boolean isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled + Set rigidBodies; //!< All the rigid bodies of the physics world + Set joints; //!< All the joints of the world + Vector3f gravity; //!< Gravity vector of the world + float timeStep; //!< Current frame time step (in seconds) + boolean isGravityEnabled; //!< True if the gravity force is on + Vector rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the raints) + Vector rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the raints) + Vector splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse) + Vector splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse) + Vector rainedPositions; //!< Array of rained rigid bodies position (for position error correction) + Vector rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction) + Map mapBodyToConstrainedVelocityIndex; //!< Map body to their index in the rained velocities array + Vector islands; //!< Array with all the islands of awaken bodies + int numberBodiesCapacity; //!< Current allocated capacity for the bodies + float sleepLinearVelocity; //!< Sleep linear velocity threshold + float sleepAngularVelocity; //!< Sleep angular velocity threshold + float timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity. /** * @brief Integrate position and orientation of the rigid bodies. * The positions and orientations of the bodies are integrated using @@ -110,11 +106,11 @@ namespace ephysics { * @brief Constructor * @param gravity Gravity vector in the world (in meters per second squared) */ - DynamicsWorld( vec3 gravity); + DynamicsWorld( Vector3f gravity); /** * @brief Vitualize the Destructor */ - virtual ~DynamicsWorld(); + ~DynamicsWorld(); /** * @brief Update the physics simulation * @param timeStep The amount of time to step the simulation by (in seconds) @@ -158,10 +154,10 @@ namespace ephysics { void setIsSolveFrictionAtContactManifoldCenterActive(boolean isActive); /** * @brief Create a rigid body into the physics world - * @param[in] transform etk::Transform3Dation from body local-space to world-space + * @param[in] transform Transform3Dation from body local-space to world-space * @return A pointer to the body that has been created in the world */ - RigidBody* createRigidBody( etk::Transform3D transform); + RigidBody* createRigidBody( Transform3D transform); /** * @brief Destroy a rigid body and all the joints which it belongs * @param[in,out] rigidBody Pointer to the body you want to destroy @@ -182,12 +178,12 @@ namespace ephysics { * @brief Get the gravity vector of the world * @return The current gravity vector (in meter per seconds squared) */ - vec3 getGravity() ; + Vector3f getGravity() ; /** * @brief Set the gravity vector of the world * @param[in] gravity The gravity vector (in meter per seconds squared) */ - void setGravity(vec3 gravity); + void setGravity(Vector3f gravity); /** * @brief Get if the gravity is enaled * @return True if the gravity is enabled in the world @@ -213,12 +209,12 @@ namespace ephysics { * @brief Get an iterator to the beginning of the bodies of the physics world * @return Starting iterator of the set of rigid bodies */ - etk::Set::Iterator getRigidBodiesBeginIterator(); + Set::Iterator getRigidBodiesBeginIterator(); /** * @brief Get an iterator to the end of the bodies of the physics world * @return Ending iterator of the set of rigid bodies */ - etk::Set::Iterator getRigidBodiesEndIterator(); + Set::Iterator getRigidBodiesEndIterator(); /** * @brief Get if the sleeping technique is enabled * @return True if the sleeping technique is enabled and false otherwise @@ -272,22 +268,22 @@ namespace ephysics { */ void setEventListener(EventListener* eventListener); void testCollision( ProxyShape* shape, - CollisionCallback* callback) override; - virtual void testCollision( ProxyShape* shape1, + CollisionCallback* callback) ; + void testCollision( ProxyShape* shape1, ProxyShape* shape2, - CollisionCallback* callback) override; - virtual void testCollision( CollisionBody* body, - CollisionCallback* callback) override; - virtual void testCollision( CollisionBody* body1, + CollisionCallback* callback) ; + void testCollision( CollisionBody* body, + CollisionCallback* callback) ; + void testCollision( CollisionBody* body1, CollisionBody* body2, - CollisionCallback* callback) override; + CollisionCallback* callback) ; /// Test and report collisions between all shapes of the world - virtual void testCollision(CollisionCallback* callback) override; + void testCollision(CollisionCallback* callback) ; /** * @brief Get list of all contacts. * @return The list of all contacts of the world */ - etk::Vector< ContactManifold*> getContactsList() ; + Vector< ContactManifold*> getContactsList() ; friend class RigidBody; }; diff --git a/src/org/atriaSoft/ephysics/engine/DynamicsWorld.cpp b/src/org/atriaSoft/ephysics/engine/DynamicsWorld.cpp index 74c7af5..d37fc73 100644 --- a/src/org/atriaSoft/ephysics/engine/DynamicsWorld.cpp +++ b/src/org/atriaSoft/ephysics/engine/DynamicsWorld.cpp @@ -13,7 +13,7 @@ #include #include -ephysics::DynamicsWorld::DynamicsWorld( vec3 gravity): +ephysics::DynamicsWorld::DynamicsWorld( Vector3f gravity): CollisionWorld(), this.contactSolver(this.mapBodyToConstrainedVelocityIndex), this.raintSolver(this.mapBodyToConstrainedVelocityIndex), @@ -31,16 +31,16 @@ ephysics::DynamicsWorld::DynamicsWorld( vec3 gravity): ephysics::DynamicsWorld::~DynamicsWorld() { // Destroy all the joints that have not been removed - etk::Set::Iterator itJoints; + Set::Iterator itJoints; for (itJoints = this.joints.begin(); itJoints != this.joints.end();) { - etk::Set::Iterator itToRemove = itJoints; + Set::Iterator itToRemove = itJoints; ++itJoints; destroyJoint(*itToRemove); } // Destroy all the rigid bodies that have not been removed - etk::Set::Iterator itRigidBodies; + Set::Iterator itRigidBodies; for (itRigidBodies = this.rigidBodies.begin(); itRigidBodies != this.rigidBodies.end();) { - etk::Set::Iterator itToRemove = itRigidBodies; + Set::Iterator itToRemove = itRigidBodies; ++itRigidBodies; destroyRigidBody(*itToRemove); } @@ -64,7 +64,7 @@ ephysics::DynamicsWorld::~DynamicsWorld() { assert(this.rigidBodies.size() == 0); #ifdef ISPROFILINGACTIVE // Print the profiling report - etk::Stream tmp; + Stream tmp; Profiler::printReport(tmp); Log.print(tmp.str()); // Destroy the profiler (release the allocated memory) @@ -81,7 +81,7 @@ void ephysics::DynamicsWorld::update(float timeStep) { this.timeStep = timeStep; // Notify the event listener about the beginning of an internal tick if (this.eventListener != null) { - this.eventListener->beginInternalTick(); + this.eventListener.beginInternalTick(); } // Reset all the contact manifolds lists of each body resetContactManifoldListsOfBodies(); @@ -108,7 +108,7 @@ void ephysics::DynamicsWorld::update(float timeStep) { } // Notify the event listener about the end of an internal tick if (this.eventListener != null) { - this.eventListener->endInternalTick(); + this.eventListener.endInternalTick(); } // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); @@ -118,13 +118,13 @@ void ephysics::DynamicsWorld::integrateRigidBodiesPositions() { PROFILE("ephysics::DynamicsWorld::integrateRigidBodiesPositions()"); // For each island of the world for (int i=0; i < this.islands.size(); i++) { - RigidBody** bodies = this.islands[i]->getBodies(); + RigidBody** bodies = this.islands[i].getBodies(); // For each body of the island - for (int b=0; b < this.islands[i]->getNbBodies(); b++) { + for (int b=0; b < this.islands[i].getNbBodies(); b++) { // Get the rained velocity - int indexArray = this.mapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - vec3 newLinVelocity = this.rainedLinearVelocities[indexArray]; - vec3 newAngVelocity = this.rainedAngularVelocities[indexArray]; + int indexArray = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second; + Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray]; + Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray]; // Add the split impulse velocity from Contact Solver (only used // to update the position) if (this.contactSolver.isSplitImpulseActive()) { @@ -132,12 +132,12 @@ void ephysics::DynamicsWorld::integrateRigidBodiesPositions() { newAngVelocity += this.splitAngularVelocities[indexArray]; } // Get current position and orientation of the body - vec3 currentPosition = bodies[b]->this.centerOfMassWorld; - etk::Quaternion currentOrientation = bodies[b]->getTransform().getOrientation(); + Vector3f currentPosition = bodies[b].this.centerOfMassWorld; + Quaternion currentOrientation = bodies[b].getTransform().getOrientation(); // Update the new rained position and orientation of the body this.rainedPositions[indexArray] = currentPosition + newLinVelocity * this.timeStep; this.rainedOrientations[indexArray] = currentOrientation; - this.rainedOrientations[indexArray] += etk::Quaternion(0, newAngVelocity) + this.rainedOrientations[indexArray] += Quaternion(0, newAngVelocity) * currentOrientation * 0.5f * this.timeStep; @@ -150,20 +150,20 @@ void ephysics::DynamicsWorld::updateBodiesState() { // For each island of the world for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { // For each body of the island - RigidBody** bodies = this.islands[islandIndex]->getBodies(); - for (int b=0; b < this.islands[islandIndex]->getNbBodies(); b++) { - int index = this.mapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + RigidBody** bodies = this.islands[islandIndex].getBodies(); + for (int b=0; b < this.islands[islandIndex].getNbBodies(); b++) { + int index = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second; // Update the linear and angular velocity of the body - bodies[b]->this.linearVelocity = this.rainedLinearVelocities[index]; - bodies[b]->this.angularVelocity = this.rainedAngularVelocities[index]; + bodies[b].this.linearVelocity = this.rainedLinearVelocities[index]; + bodies[b].this.angularVelocity = this.rainedAngularVelocities[index]; // Update the position of the center of mass of the body - bodies[b]->this.centerOfMassWorld = this.rainedPositions[index]; + bodies[b].this.centerOfMassWorld = this.rainedPositions[index]; // Update the orientation of the body - bodies[b]->this.transform.setOrientation(this.rainedOrientations[index].safeNormalized()); + bodies[b].this.transform.setOrientation(this.rainedOrientations[index].safeNormalized()); // Update the transform of the body (using the new center of mass and new orientation) - bodies[b]->updateTransformWithCenterOfMass(); + bodies[b].updateTransformWithCenterOfMass(); // Update the broad-phase state of the body - bodies[b]->updateBroadPhaseState(); + bodies[b].updateBroadPhaseState(); } } } @@ -171,7 +171,7 @@ void ephysics::DynamicsWorld::updateBodiesState() { void ephysics::DynamicsWorld::initVelocityArrays() { // Allocate memory for the bodies velocity arrays int nbBodies = this.rigidBodies.size(); - if (this.numberBodiesCapacity != nbBodies hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nbBodies > 0) { + if (this.numberBodiesCapacity != nbBodies && nbBodies > 0) { if (this.numberBodiesCapacity > 0) { this.splitLinearVelocities.clear(); this.splitAngularVelocities.clear(); @@ -183,12 +183,12 @@ void ephysics::DynamicsWorld::initVelocityArrays() { this.rainedAngularVelocities.clear(); this.rainedPositions.clear(); this.rainedOrientations.clear(); - this.splitLinearVelocities.resize(this.numberBodiesCapacity, vec3(0,0,0)); - this.splitAngularVelocities.resize(this.numberBodiesCapacity, vec3(0,0,0)); - this.rainedLinearVelocities.resize(this.numberBodiesCapacity, vec3(0,0,0)); - this.rainedAngularVelocities.resize(this.numberBodiesCapacity, vec3(0,0,0)); - this.rainedPositions.resize(this.numberBodiesCapacity, vec3(0,0,0)); - this.rainedOrientations.resize(this.numberBodiesCapacity, etk::Quaternion::identity()); + this.splitLinearVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0)); + this.splitAngularVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0)); + this.rainedLinearVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0)); + this.rainedAngularVelocities.resize(this.numberBodiesCapacity, Vector3f(0,0,0)); + this.rainedPositions.resize(this.numberBodiesCapacity, Vector3f(0,0,0)); + this.rainedOrientations.resize(this.numberBodiesCapacity, Quaternion::identity()); } // Reset the velocities arrays for (int i=0; i::Iterator it; + Set::Iterator it; int indexBody = 0; for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { // Add the body into the map @@ -212,22 +212,22 @@ void ephysics::DynamicsWorld::integrateRigidBodiesVelocities() { initVelocityArrays(); // For each island of the world for (int i=0; i < this.islands.size(); i++) { - RigidBody** bodies = this.islands[i]->getBodies(); + RigidBody** bodies = this.islands[i].getBodies(); // For each body of the island - for (int b=0; b < this.islands[i]->getNbBodies(); b++) { + for (int b=0; b < this.islands[i].getNbBodies(); b++) { // Insert the body into the map of rained velocities - int indexBody = this.mapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - assert(this.splitLinearVelocities[indexBody] == vec3(0, 0, 0)); - assert(this.splitAngularVelocities[indexBody] == vec3(0, 0, 0)); + int indexBody = this.mapBodyToConstrainedVelocityIndex.find(bodies[b]).second; + assert(this.splitLinearVelocities[indexBody] == Vector3f(0, 0, 0)); + assert(this.splitAngularVelocities[indexBody] == Vector3f(0, 0, 0)); // Integrate the external force to get the new velocity of the body - this.rainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity(); - this.rainedLinearVelocities[indexBody] += bodies[b]->this.massInverse * bodies[b]->this.externalForce * this.timeStep; - this.rainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity(); - this.rainedAngularVelocities[indexBody] += bodies[b]->getInertiaTensorInverseWorld() * bodies[b]->this.externalTorque * this.timeStep; + this.rainedLinearVelocities[indexBody] = bodies[b].getLinearVelocity(); + this.rainedLinearVelocities[indexBody] += bodies[b].this.massInverse * bodies[b].this.externalForce * this.timeStep; + this.rainedAngularVelocities[indexBody] = bodies[b].getAngularVelocity(); + this.rainedAngularVelocities[indexBody] += bodies[b].getInertiaTensorInverseWorld() * bodies[b].this.externalTorque * this.timeStep; // If the gravity has to be applied to this rigid body - if (bodies[b]->isGravityEnabled() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.isGravityEnabled) { + if (bodies[b].isGravityEnabled() && this.isGravityEnabled) { // Integrate the gravity force - this.rainedLinearVelocities[indexBody] += this.timeStep * bodies[b]->this.massInverse * bodies[b]->getMass() * this.gravity; + this.rainedLinearVelocities[indexBody] += this.timeStep * bodies[b].this.massInverse * bodies[b].getMass() * this.gravity; } // Apply the velocity damping // Damping force : Fc = -c' * v (c=damping factor) @@ -242,8 +242,8 @@ void ephysics::DynamicsWorld::integrateRigidBodiesVelocities() { // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... // => e^(-x) ~ 1 - x // => v2 = v1 * (1 - c * dt) - float linDampingFactor = bodies[b]->getLinearDamping(); - float angDampingFactor = bodies[b]->getAngularDamping(); + float linDampingFactor = bodies[b].getLinearDamping(); + float angDampingFactor = bodies[b].getAngularDamping(); float linearDamping = pow(1.0f - linDampingFactor, this.timeStep); float angularDamping = pow(1.0f - angDampingFactor, this.timeStep); this.rainedLinearVelocities[indexBody] *= linearDamping; @@ -267,9 +267,9 @@ void ephysics::DynamicsWorld::solveContactsAndConstraints() { // For each island of the world for (int islandIndex = 0; islandIndex < this.islands.size(); islandIndex++) { // Check if there are contacts and raints to solve - boolean isConstraintsToSolve = this.islands[islandIndex]->getNbJoints() > 0; - boolean isContactsToSolve = this.islands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !isContactsToSolve) { + boolean isConstraintsToSolve = this.islands[islandIndex].getNbJoints() > 0; + boolean isContactsToSolve = this.islands[islandIndex].getNbContactManifolds() > 0; + if (!isConstraintsToSolve && !isContactsToSolve) { continue; } // If there are contacts in the current island @@ -319,7 +319,7 @@ void ephysics::DynamicsWorld::solvePositionCorrection() { } } -ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody( etk::Transform3D transform) { +ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody( Transform3D transform) { // Compute the body ID ephysics::long bodyID = computeNextAvailableBodyID(); // Largest index cannot be used (it is used for invalid index) @@ -336,17 +336,17 @@ ephysics::RigidBody* ephysics::DynamicsWorld::createRigidBody( etk::Transform3D void ephysics::DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Remove all the collision shapes of the body - rigidBody->removeAllCollisionShapes(); + rigidBody.removeAllCollisionShapes(); // Add the body ID to the list of free IDs - this.freeBodiesIDs.pushBack(rigidBody->getID()); + this.freeBodiesIDs.pushBack(rigidBody.getID()); // Destroy all the joints in which the rigid body to be destroyed is involved - for (ephysics::JointListElement* element = rigidBody->this.jointsList; + for (ephysics::JointListElement* element = rigidBody.this.jointsList; element != null; - element = element->next) { - destroyJoint(element->joint); + element = element.next) { + destroyJoint(element.joint); } // Reset the contact manifold list of the body - rigidBody->resetContactManifoldsList(); + rigidBody.resetContactManifoldsList(); // Remove the rigid body from the list of rigid bodies this.bodies.erase(this.bodies.find(rigidBody)); this.rigidBodies.erase(this.rigidBodies.find(rigidBody)); @@ -398,19 +398,19 @@ void ephysics::DynamicsWorld::destroyJoint(Joint* joint) { return; } // If the collision between the two bodies of the raint was disabled - if (!joint->isCollisionEnabled()) { + 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()); + this.collisionDetection.removeNoCollisionPair(joint.getBody1(), joint.getBody2()); } // Wake up the two bodies of the joint - joint->getBody1()->setIsSleeping(false); - joint->getBody2()->setIsSleeping(false); + joint.getBody1().setIsSleeping(false); + joint.getBody2().setIsSleeping(false); // Remove the joint from the world this.joints.erase(this.joints.find(joint)); // Remove the joint from the joint list of the bodies involved in the joint - joint->this.body1->removeJointFrothis.jointsList(joint); - joint->this.body2->removeJointFrothis.jointsList(joint); - sizet nbBytes = joint->getSizeInBytes(); + joint.this.body1.removeJointFrothis.jointsList(joint); + joint.this.body2.removeJointFrothis.jointsList(joint); + long nbBytes = joint.getSizeInBytes(); // Call the destructor of the joint ETKDELETE(Joint, joint); joint = null; @@ -422,9 +422,9 @@ void ephysics::DynamicsWorld::addJointToBody(ephysics::Joint* joint) { return; } // Add the joint at the beginning of the linked list of joints of the first body - joint->this.body1->this.jointsList = ETKNEW(JointListElement, joint, joint->this.body1->this.jointsList); + joint.this.body1.this.jointsList = ETKNEW(JointListElement, joint, joint.this.body1.this.jointsList); // Add the joint at the beginning of the linked list of joints of the second body - joint->this.body2->this.jointsList = ETKNEW(JointListElement, joint, joint->this.body2->this.jointsList); + joint.this.body2.this.jointsList = ETKNEW(JointListElement, joint, joint.this.body2.this.jointsList); } void ephysics::DynamicsWorld::computeIslands() { @@ -439,36 +439,36 @@ void ephysics::DynamicsWorld::computeIslands() { this.islands.clear(); int nbContactManifolds = 0; // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds - for (etk::Set::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { - int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds(); + for (Set::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { + int nbBodyManifolds = (*it).resetIsAlreadyInIslandAndCountManifolds(); nbContactManifolds += nbBodyManifolds; } - for (etk::Set::Iterator it = this.joints.begin(); it != this.joints.end(); ++it) { - (*it)->this.isAlreadyInIsland = false; + for (Set::Iterator it = this.joints.begin(); it != this.joints.end(); ++it) { + (*it).this.isAlreadyInIsland = false; } // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search - etk::Vector stackBodiesToVisit; + Vector stackBodiesToVisit; stackBodiesToVisit.resize(nbBodies, null); // For each rigid body of the world - for (etk::Set::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { + for (Set::Iterator it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { ephysics::RigidBody* body = *it; // If the body has already been added to an island, we go to the next body - if (body->this.isAlreadyInIsland) { + if (body.this.isAlreadyInIsland) { continue; } // If the body is static, we go to the next body - if (body->getType() == STATIC) { + if (body.getType() == STATIC) { continue; } // If the body is sleeping or inactive, we go to the next body - if (body->isSleeping() || !body->isActive()) { + if (body.isSleeping() || !body.isActive()) { continue; } // Reset the stack of bodies to visit int stackIndex = 0; stackBodiesToVisit[stackIndex] = body; stackIndex++; - body->this.isAlreadyInIsland = true; + body.this.isAlreadyInIsland = true; // Create the new island this.islands.pushBack(ETKNEW(Island, nbBodies, nbContactManifolds, this.joints.size())); // While there are still some bodies to visit in the stack @@ -476,67 +476,67 @@ void ephysics::DynamicsWorld::computeIslands() { // Get the next body to visit from the stack stackIndex--; ephysics::RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex]; - assert(bodyToVisit->isActive()); + assert(bodyToVisit.isActive()); // Awake the body if it is slepping - bodyToVisit->setIsSleeping(false); + bodyToVisit.setIsSleeping(false); // Add the body into the island - this.islands.back()->addBody(bodyToVisit); + this.islands.back().addBody(bodyToVisit); // If the current body is static, we do not want to perform the DFS // search across that body - if (bodyToVisit->getType() == STATIC) { + if (bodyToVisit.getType() == STATIC) { continue; } // For each contact manifold in which the current body is involded ephysics::ContactManifoldListElement* contactElement; - for (contactElement = bodyToVisit->this.contactManifoldsList; + for (contactElement = bodyToVisit.this.contactManifoldsList; contactElement != null; - contactElement = contactElement->next) { - ephysics::ContactManifold* contactManifold = contactElement->contactManifold; - assert(contactManifold->getNbContactPoints() > 0); + contactElement = contactElement.next) { + ephysics::ContactManifold* contactManifold = contactElement.contactManifold; + assert(contactManifold.getNbContactPoints() > 0); // Check if the current contact manifold has already been added into an island - if (contactManifold->isAlreadyInIsland()) { + if (contactManifold.isAlreadyInIsland()) { continue; } // Add the contact manifold into the island - this.islands.back()->addContactManifold(contactManifold); - contactManifold->this.isAlreadyInIsland = true; + this.islands.back().addContactManifold(contactManifold); + contactManifold.this.isAlreadyInIsland = true; // Get the other body of the contact manifold - ephysics::RigidBody* body1 = staticcast(contactManifold->getBody1()); - ephysics::RigidBody* body2 = staticcast(contactManifold->getBody2()); - ephysics::RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + ephysics::RigidBody* body1 = staticcast(contactManifold.getBody1()); + ephysics::RigidBody* body2 = staticcast(contactManifold.getBody2()); + ephysics::RigidBody* otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1; // Check if the other body has already been added to the island - if (otherBody->this.isAlreadyInIsland) { + if (otherBody.this.isAlreadyInIsland) { continue; } // Insert the other body into the stack of bodies to visit stackBodiesToVisit[stackIndex] = otherBody; stackIndex++; - otherBody->this.isAlreadyInIsland = true; + otherBody.this.isAlreadyInIsland = true; } // For each joint in which the current body is involved ephysics::JointListElement* jointElement; - for (jointElement = bodyToVisit->this.jointsList; + for (jointElement = bodyToVisit.this.jointsList; jointElement != null; - jointElement = jointElement->next) { - ephysics::Joint* joint = jointElement->joint; + jointElement = jointElement.next) { + ephysics::Joint* joint = jointElement.joint; // Check if the current joint has already been added into an island - if (joint->isAlreadyInIsland()) continue; + if (joint.isAlreadyInIsland()) continue; // Add the joint into the island - this.islands.back()->addJoint(joint); - joint->this.isAlreadyInIsland = true; + this.islands.back().addJoint(joint); + joint.this.isAlreadyInIsland = true; // Get the other body of the contact manifold - ephysics::RigidBody* body1 = staticcast(joint->getBody1()); - ephysics::RigidBody* body2 = staticcast(joint->getBody2()); - ephysics::RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + ephysics::RigidBody* body1 = staticcast(joint.getBody1()); + ephysics::RigidBody* body2 = staticcast(joint.getBody2()); + ephysics::RigidBody* otherBody = (body1.getID() == bodyToVisit.getID()) ? body2 : body1; // Check if the other body has already been added to the island - if (otherBody->this.isAlreadyInIsland) continue; + if (otherBody.this.isAlreadyInIsland) continue; // Insert the other body into the stack of bodies to visit stackBodiesToVisit[stackIndex] = otherBody; stackIndex++; - otherBody->this.isAlreadyInIsland = true; + otherBody.this.isAlreadyInIsland = true; } } - this.islands.back()->resetStaticBobyNotInIsland(); + this.islands.back().resetStaticBobyNotInIsland(); } } @@ -548,22 +548,22 @@ void ephysics::DynamicsWorld::updateSleepingBodies() { for (int i=0; igetBodies(); - for (int b=0; b < this.islands[i]->getNbBodies(); b++) { + ephysics::RigidBody** bodies = this.islands[i].getBodies(); + for (int b=0; b < this.islands[i].getNbBodies(); b++) { // Skip static bodies - if (bodies[b]->getType() == STATIC) continue; + if (bodies[b].getType() == STATIC) continue; // If the body is velocity is large enough to stay awake - if (bodies[b]->getLinearVelocity().length2() > sleepLinearVelocitySquare || - bodies[b]->getAngularVelocity().length2() > sleepAngularVelocitySquare || - !bodies[b]->isAllowedToSleep()) { + if (bodies[b].getLinearVelocity().length2() > sleepLinearVelocitySquare || + bodies[b].getAngularVelocity().length2() > sleepAngularVelocitySquare || + !bodies[b].isAllowedToSleep()) { // Reset the sleep time of the body - bodies[b]->this.sleepTime = 0.0f; + bodies[b].this.sleepTime = 0.0f; minSleepTime = 0.0f; } else { // If the body velocity is bellow the sleeping velocity threshold // Increase the sleep time - bodies[b]->this.sleepTime += this.timeStep; - if (bodies[b]->this.sleepTime < minSleepTime) { - minSleepTime = bodies[b]->this.sleepTime; + bodies[b].this.sleepTime += this.timeStep; + if (bodies[b].this.sleepTime < minSleepTime) { + minSleepTime = bodies[b].this.sleepTime; } } } @@ -572,8 +572,8 @@ void ephysics::DynamicsWorld::updateSleepingBodies() { // 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[i]->getNbBodies(); b++) { - bodies[b]->setIsSleeping(true); + for (int b=0; b < this.islands[i].getNbBodies(); b++) { + bodies[b].setIsSleeping(true); } } } @@ -583,79 +583,79 @@ void ephysics::DynamicsWorld::enableSleeping(boolean isSleepingEnabled) { this.isSleepingEnabled = isSleepingEnabled; if (!this.isSleepingEnabled) { // For each body of the world - etk::Set::Iterator it; + Set::Iterator it; for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { // Wake up the rigid body - (*it)->setIsSleeping(false); + (*it).setIsSleeping(false); } } } void ephysics::DynamicsWorld::testCollision( ephysics::ProxyShape* shape, ephysics::CollisionCallback* callback) { // Create the sets of shapes - etk::Set shapes; - shapes.add(shape->this.broadPhaseID); - etk::Set emptySet; + Set shapes; + shapes.add(shape.this.broadPhaseID); + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet); } void ephysics::DynamicsWorld::testCollision( ephysics::ProxyShape* shape1, ephysics::ProxyShape* shape2, ephysics::CollisionCallback* callback) { // Create the sets of shapes - etk::Set shapes1; - shapes1.add(shape1->this.broadPhaseID); - etk::Set shapes2; - shapes2.add(shape2->this.broadPhaseID); + Set shapes1; + shapes1.add(shape1.this.broadPhaseID); + Set shapes2; + shapes2.add(shape2.this.broadPhaseID); // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } void ephysics::DynamicsWorld::testCollision( ephysics::CollisionBody* body, ephysics::CollisionCallback* callback) { // Create the sets of shapes - etk::Set shapes1; + Set shapes1; // For each shape of the body - for ( ProxyShape* shape = body->getProxyShapesList(); + for ( ProxyShape* shape = body.getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes1.add(shape->this.broadPhaseID); + shape = shape.getNext()) { + shapes1.add(shape.this.broadPhaseID); } - etk::Set emptySet; + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet); } void ephysics::DynamicsWorld::testCollision( ephysics::CollisionBody* body1, ephysics::CollisionBody* body2, ephysics::CollisionCallback* callback) { // Create the sets of shapes - etk::Set shapes1; - for ( ProxyShape* shape=body1->getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes1.add(shape->this.broadPhaseID); + Set shapes1; + for ( ProxyShape* shape=body1.getProxyShapesList(); shape != null; + shape = shape.getNext()) { + shapes1.add(shape.this.broadPhaseID); } - etk::Set shapes2; - for ( ProxyShape* shape=body2->getProxyShapesList(); shape != null; - shape = shape->getNext()) { - shapes2.add(shape->this.broadPhaseID); + Set shapes2; + for ( ProxyShape* shape=body2.getProxyShapesList(); shape != null; + shape = shape.getNext()) { + shapes2.add(shape.this.broadPhaseID); } // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2); } void ephysics::DynamicsWorld::testCollision(ephysics::CollisionCallback* callback) { - etk::Set emptySet; + Set emptySet; // Perform the collision detection and report contacts this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet); } -etk::Vector< ephysics::ContactManifold*> ephysics::DynamicsWorld::getContactsList() { - etk::Vector< ephysics::ContactManifold*> contactManifolds; +Vector< ephysics::ContactManifold*> ephysics::DynamicsWorld::getContactsList() { + Vector< ephysics::ContactManifold*> contactManifolds; // For each currently overlapping pair of bodies - etk::Map::Iterator it; - for (it = this.collisionDetection.this.overlappingPairs.begin(); - it != this.collisionDetection.this.overlappingPairs.end(); + Map::Iterator it; + for (it = this.collisionDetection.overlappingPairs.begin(); + it != this.collisionDetection.overlappingPairs.end(); ++it) { - ephysics::OverlappingPair* pair = it->second; + ephysics::OverlappingPair* pair = it.second; // For each contact manifold of the pair - ephysics::ContactManifoldSet manifoldSet = pair->getContactManifoldSet(); + ephysics::ContactManifoldSet manifoldSet = pair.getContactManifoldSet(); for (int i=0; i ephysics::DynamicsWorld::getContactsLis void ephysics::DynamicsWorld::resetBodiesForceAndTorque() { // For each body of the world - etk::Set::Iterator it; + Set::Iterator it; for (it = this.rigidBodies.begin(); it != this.rigidBodies.end(); ++it) { - (*it)->this.externalForce.setZero(); - (*it)->this.externalTorque.setZero(); + (*it).this.externalForce.setZero(); + (*it).this.externalTorque.setZero(); } } @@ -711,11 +711,11 @@ void ephysics::DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bo this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } -vec3 ephysics::DynamicsWorld::getGravity() { +Vector3f ephysics::DynamicsWorld::getGravity() { return this.gravity; } -void ephysics::DynamicsWorld::setGravity(vec3 gravity) { +void ephysics::DynamicsWorld::setGravity(Vector3f gravity) { this.gravity = gravity; } @@ -735,11 +735,11 @@ int ephysics::DynamicsWorld::getNbJoints() { return this.joints.size(); } -etk::Set::Iterator ephysics::DynamicsWorld::getRigidBodiesBeginIterator() { +Set::Iterator ephysics::DynamicsWorld::getRigidBodiesBeginIterator() { return this.rigidBodies.begin(); } -etk::Set::Iterator ephysics::DynamicsWorld::getRigidBodiesEndIterator() { +Set::Iterator ephysics::DynamicsWorld::getRigidBodiesEndIterator() { return this.rigidBodies.end(); } @@ -753,7 +753,7 @@ float ephysics::DynamicsWorld::getSleepLinearVelocity() { void ephysics::DynamicsWorld::setSleepLinearVelocity(float sleepLinearVelocity) { if(sleepLinearVelocity < 0.0f) { - Log.error("Can not set sleepLinearVelocity=" << sleepLinearVelocity << " < 0 "); + Log.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 "); return; } this.sleepLinearVelocity = sleepLinearVelocity; @@ -765,7 +765,7 @@ float ephysics::DynamicsWorld::getSleepAngularVelocity() { void ephysics::DynamicsWorld::setSleepAngularVelocity(float sleepAngularVelocity) { if(sleepAngularVelocity < 0.0f) { - Log.error("Can not set sleepAngularVelocity=" << sleepAngularVelocity << " < 0 "); + Log.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 "); return; } this.sleepAngularVelocity = sleepAngularVelocity; @@ -777,7 +777,7 @@ float ephysics::DynamicsWorld::getTimeBeforeSleep() { void ephysics::DynamicsWorld::setTimeBeforeSleep(float timeBeforeSleep) { if(timeBeforeSleep < 0.0f) { - Log.error("Can not set timeBeforeSleep=" << timeBeforeSleep << " < 0 "); + Log.error("Can not set timeBeforeSleep=" + timeBeforeSleep + " < 0 "); return; } this.timeBeforeSleep = timeBeforeSleep; diff --git a/src/org/atriaSoft/ephysics/engine/EventListener.hpp b/src/org/atriaSoft/ephysics/engine/EventListener.java similarity index 61% rename from src/org/atriaSoft/ephysics/engine/EventListener.hpp rename to src/org/atriaSoft/ephysics/engine/EventListener.java index ce9b237..93e92b9 100644 --- a/src/org/atriaSoft/ephysics/engine/EventListener.hpp +++ b/src/org/atriaSoft/ephysics/engine/EventListener.java @@ -1,20 +1,9 @@ -/** @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) - */ -#pragma once +package org.atriaSoft.ephysics.engine; -#include - -namespace ephysics { /** * @brief 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 override the methods you need. Then, you need to register your + * 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. */ @@ -24,33 +13,29 @@ namespace ephysics { * @brief Generic Constructor */ EventListener() {} - /** - * @brief Generic Desstructor take it virtual - */ - virtual ~EventListener() =default; /** * @brief Called when a new contact point is found between two bodies that were separated before * @param contact Information about the contact */ - virtual void beginContact( ContactPointInfo contact) {}; + void beginContact( ContactPointInfo contact) {}; /** * @brief Called when a new contact point is found between two bodies * @param contact Information about the contact */ - virtual void newContact( ContactPointInfo contact) {} + void newContact( ContactPointInfo contact) {} /** * @brief 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. */ - virtual void beginInternalTick() {} + void beginInternalTick() {} /** * @brief 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. */ - virtual void endInternalTick() {} + void endInternalTick() {} }; } diff --git a/src/org/atriaSoft/ephysics/engine/Impulse.hpp b/src/org/atriaSoft/ephysics/engine/Impulse.hpp deleted file mode 100644 index c31a1aa..0000000 --- a/src/org/atriaSoft/ephysics/engine/Impulse.hpp +++ /dev/null @@ -1,46 +0,0 @@ -/** @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) - */ -#pragma once - -#include - -namespace ephysics { - /** - * @brief Represents an impulse that we can apply to bodies in the contact or raint solver. - */ - class Impulse { - private: - /// Private assignment operator - Impulse operator=( Impulse impulse); - public: - vec3 linearImpulseBody1; //!< Linear impulse applied to the first body - vec3 angularImpulseBody1; //!< Angular impulse applied to the first body - vec3 linearImpulseBody2; //!< Linear impulse applied to the second body - vec3 angularImpulseBody2; //!< Angular impulse applied to the second body - /// Constructor - Impulse( vec3 initLinearImpulseBody1, - vec3 initAngularImpulseBody1, - vec3 initLinearImpulseBody2, - vec3 initAngularImpulseBody2): - linearImpulseBody1(initLinearImpulseBody1), - angularImpulseBody1(initAngularImpulseBody1), - linearImpulseBody2(initLinearImpulseBody2), - angularImpulseBody2(initAngularImpulseBody2) { - - } - /// Copy-ructor - Impulse( Impulse impulse): - linearImpulseBody1(impulse.linearImpulseBody1), - angularImpulseBody1(impulse.angularImpulseBody1), - linearImpulseBody2(impulse.linearImpulseBody2), - angularImpulseBody2(impulse.angularImpulseBody2) { - - } - }; -} diff --git a/src/org/atriaSoft/ephysics/engine/Impulse.java b/src/org/atriaSoft/ephysics/engine/Impulse.java new file mode 100644 index 0000000..eef4171 --- /dev/null +++ b/src/org/atriaSoft/ephysics/engine/Impulse.java @@ -0,0 +1,32 @@ +package org.atriaSoft.ephysics.engine; + + /** + * @brief Represents an impulse that we can apply to bodies in the contact or raint solver. + */ + class Impulse { + public: + Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body + Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body + Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body + Vector3f angularImpulseBody2; //!< Angular impulse applied to the second body + /// Constructor + Impulse( Vector3f initLinearImpulseBody1, + Vector3f initAngularImpulseBody1, + Vector3f initLinearImpulseBody2, + Vector3f initAngularImpulseBody2): + linearImpulseBody1(initLinearImpulseBody1), + angularImpulseBody1(initAngularImpulseBody1), + linearImpulseBody2(initLinearImpulseBody2), + angularImpulseBody2(initAngularImpulseBody2) { + + } + /// Copy-ructor + Impulse( Impulse impulse): + linearImpulseBody1(impulse.linearImpulseBody1), + angularImpulseBody1(impulse.angularImpulseBody1), + linearImpulseBody2(impulse.linearImpulseBody2), + angularImpulseBody2(impulse.angularImpulseBody2) { + + } + }; +} diff --git a/src/org/atriaSoft/ephysics/engine/Island.cpp b/src/org/atriaSoft/ephysics/engine/Island.cpp index 6f8e572..9f8ab05 100644 --- a/src/org/atriaSoft/ephysics/engine/Island.cpp +++ b/src/org/atriaSoft/ephysics/engine/Island.cpp @@ -11,9 +11,9 @@ using namespace ephysics; -ephysics::Island::Island(sizet nbMaxBodies, - sizet nbMaxContactManifolds, - sizet nbMaxJoints) { +ephysics::Island::Island(long nbMaxBodies, + long nbMaxContactManifolds, + long nbMaxJoints) { // Allocate memory for the arrays this.bodies.reserve(nbMaxBodies); this.contactManifolds.reserve(nbMaxContactManifolds); @@ -22,7 +22,7 @@ ephysics::Island::Island(sizet nbMaxBodies, void ephysics::Island::addBody(ephysics::RigidBody* body) { - if (body->isSleeping() == true) { + if (body.isSleeping() == true) { Log.error("Try to add a body that is sleeping ..."); return; } @@ -37,15 +37,15 @@ void ephysics::Island::addJoint(ephysics::Joint* joint) { this.joints.pushBack(joint); } -sizet ephysics::Island::getNbBodies() { +long ephysics::Island::getNbBodies() { return this.bodies.size(); } -sizet ephysics::Island::getNbContactManifolds() { +long ephysics::Island::getNbContactManifolds() { return this.contactManifolds.size(); } -sizet ephysics::Island::getNbJoints() { +long ephysics::Island::getNbJoints() { return this.joints.size(); } @@ -63,8 +63,8 @@ ephysics::Joint** ephysics::Island::getJoints() { void ephysics::Island::resetStaticBobyNotInIsland() { for (auto it: this.bodies) { - if (it->getType() == STATIC) { - it->this.isAlreadyInIsland = false; + if (it.getType() == STATIC) { + it.this.isAlreadyInIsland = false; } } } diff --git a/src/org/atriaSoft/ephysics/engine/Island.hpp b/src/org/atriaSoft/ephysics/engine/Island.java similarity index 54% rename from src/org/atriaSoft/ephysics/engine/Island.hpp rename to src/org/atriaSoft/ephysics/engine/Island.java index d44b8ae..ac98265 100644 --- a/src/org/atriaSoft/ephysics/engine/Island.hpp +++ b/src/org/atriaSoft/ephysics/engine/Island.java @@ -1,16 +1,4 @@ -/** @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) - */ -#pragma once - -#include -#include -#include +package org.atriaSoft.ephysics.engine; namespace ephysics { /** @@ -19,18 +7,14 @@ namespace ephysics { */ class Island { private: - etk::Vector this.bodies; //!< Array with all the bodies of the island - etk::Vector this.contactManifolds; //!< Array with all the contact manifolds between bodies of the island - etk::Vector this.joints; //!< Array with all the joints between bodies of the island - //! Remove assignment operator - Island operator=( Island island) = delete; - //! Remove copy-ructor - Island( Island island) = delete; + Vector bodies; //!< Array with all the bodies of the island + Vector contactManifolds; //!< Array with all the contact manifolds between bodies of the island + Vector joints; //!< Array with all the joints between bodies of the island public: /** * @brief Constructor */ - Island(sizet nbMaxBodies, sizet nbMaxContactManifolds, sizet nbMaxJoints); + Island(long nbMaxBodies, sizet nbMaxContactManifolds, sizet nbMaxJoints); /** * @brief Destructor */ @@ -51,16 +35,16 @@ namespace ephysics { * @brief Get the number of body * @return Number of bodies. */ - sizet getNbBodies() ; + long getNbBodies() ; /** * @ Get the number of contact manifolds * Return the number of contact manifolds in the island */ - sizet getNbContactManifolds() ; + long getNbContactManifolds() ; /** * Return the number of joints in the island */ - sizet getNbJoints() ; + long getNbJoints() ; /** * Return a pointer to the array of bodies */ diff --git a/src/org/atriaSoft/ephysics/engine/Material.cpp b/src/org/atriaSoft/ephysics/engine/Material.cpp index 68ec4f1..4b3954b 100644 --- a/src/org/atriaSoft/ephysics/engine/Material.cpp +++ b/src/org/atriaSoft/ephysics/engine/Material.cpp @@ -20,8 +20,8 @@ Material::Material() // Copy-ructor Material::Material( Material material) - : this.frictionCoefficient(material.this.frictionCoefficient), - this.rollingResistance(material.this.rollingResistance), this.bounciness(material.this.bounciness) { + : this.frictionCoefficient(material.frictionCoefficient), + this.rollingResistance(material.rollingResistance), this.bounciness(material.bounciness) { } @@ -40,7 +40,7 @@ float Material::getBounciness() { * @param bounciness Bounciness factor (between 0 and 1) where 1 is very bouncy */ void Material::setBounciness(float bounciness) { - assert(bounciness >= 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj bounciness <= 1.0f); + assert(bounciness >= 0.0f && bounciness <= 1.0f); this.bounciness = bounciness; } @@ -89,9 +89,9 @@ Material Material::operator=( Material material) { // Check for self-assignment if (this != material) { - this.frictionCoefficient = material.this.frictionCoefficient; - this.bounciness = material.this.bounciness; - this.rollingResistance = material.this.rollingResistance; + this.frictionCoefficient = material.frictionCoefficient; + this.bounciness = material.bounciness; + this.rollingResistance = material.rollingResistance; } // Return this material diff --git a/src/org/atriaSoft/ephysics/engine/Material.hpp b/src/org/atriaSoft/ephysics/engine/Material.java similarity index 56% rename from src/org/atriaSoft/ephysics/engine/Material.hpp rename to src/org/atriaSoft/ephysics/engine/Material.java index b297dfe..5f58a36 100644 --- a/src/org/atriaSoft/ephysics/engine/Material.hpp +++ b/src/org/atriaSoft/ephysics/engine/Material.java @@ -1,16 +1,4 @@ -/** @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) - */ -#pragma once -extern "C" { - #include -} -#include +package org.atriaSoft.ephysics.engine; namespace ephysics { /** @@ -20,9 +8,9 @@ namespace ephysics { */ class Material { private : - float this.frictionCoefficient; //!< Friction coefficient (positive value) - float this.rollingResistance; //!< Rolling resistance factor (positive value) - float this.bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body + float frictionCoefficient; //!< Friction coefficient (positive value) + float rollingResistance; //!< Rolling resistance factor (positive value) + float bounciness; //!< Bounciness during collisions (between 0 and 1) where 1 is for a very bouncy body public : /// Constructor Material(); diff --git a/src/org/atriaSoft/ephysics/engine/OverlappingPair.cpp b/src/org/atriaSoft/ephysics/engine/OverlappingPair.cpp index 7a23007..23a008e 100644 --- a/src/org/atriaSoft/ephysics/engine/OverlappingPair.cpp +++ b/src/org/atriaSoft/ephysics/engine/OverlappingPair.cpp @@ -32,11 +32,11 @@ void OverlappingPair::update() { this.contactManifoldSet.update(); } -vec3 OverlappingPair::getCachedSeparatingAxis() { +Vector3f OverlappingPair::getCachedSeparatingAxis() { return this.cachedSeparatingAxis; } -void OverlappingPair::setCachedSeparatingAxis( vec3 axis) { +void OverlappingPair::setCachedSeparatingAxis( Vector3f axis) { this.cachedSeparatingAxis = axis; } @@ -49,12 +49,12 @@ int OverlappingPair::getNbContactPoints() { } overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* shape2) { - assert( shape1->this.broadPhaseID >= 0 - hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shape2->this.broadPhaseID >= 0); + assert( shape1.this.broadPhaseID >= 0 + && shape2.this.broadPhaseID >= 0); // Construct the pair of body index - overlappingpairid pairID = shape1->this.broadPhaseID < shape2->this.broadPhaseID ? - etk::makePair(shape1->this.broadPhaseID, shape2->this.broadPhaseID) : - etk::makePair(shape2->this.broadPhaseID, shape1->this.broadPhaseID); + overlappingpairid pairID = shape1.this.broadPhaseID < shape2.this.broadPhaseID ? + makePair(shape1.this.broadPhaseID, shape2.this.broadPhaseID) : + makePair(shape2.this.broadPhaseID, shape1.this.broadPhaseID); assert(pairID.first != pairID.second); return pairID; } @@ -62,9 +62,9 @@ overlappingpairid OverlappingPair::computeID(ProxyShape* shape1, ProxyShape* sha longpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2) { // Construct the pair of body index - longpair indexPair = body1->getID() < body2->getID() ? - etk::makePair(body1->getID(), body2->getID()) : - etk::makePair(body2->getID(), body1->getID()); + longpair indexPair = body1.getID() < body2.getID() ? + makePair(body1.getID(), body2.getID()) : + makePair(body2.getID(), body1.getID()); assert(indexPair.first != indexPair.second); return indexPair; } diff --git a/src/org/atriaSoft/ephysics/engine/OverlappingPair.hpp b/src/org/atriaSoft/ephysics/engine/OverlappingPair.java similarity index 61% rename from src/org/atriaSoft/ephysics/engine/OverlappingPair.hpp rename to src/org/atriaSoft/ephysics/engine/OverlappingPair.java index b6545c0..b8a62f3 100644 --- a/src/org/atriaSoft/ephysics/engine/OverlappingPair.hpp +++ b/src/org/atriaSoft/ephysics/engine/OverlappingPair.java @@ -1,19 +1,7 @@ -/** @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) - */ -#pragma once -#include -#include -#include +package org.atriaSoft.ephysics.engine; -namespace ephysics { // Type for the overlapping pair ID - typedef etk::Pair overlappingpairid; + typedef Pair overlappingpairid; /** * @brief This class represents a pair of two proxy collision shapes that are overlapping * during the broad-phase collision detection. It is created when @@ -23,12 +11,8 @@ namespace ephysics { */ class OverlappingPair { private: - ContactManifoldSet this.contactManifoldSet; //!< Set of persistent contact manifolds - vec3 this.cachedSeparatingAxis; //!< Cached previous separating axis - /// Private copy-ructor - OverlappingPair( OverlappingPair pair); - /// Private assignment operator - OverlappingPair operator=( OverlappingPair pair); + ContactManifoldSet contactManifoldSet; //!< Set of persistent contact manifolds + Vector3f cachedSeparatingAxis; //!< Cached previous separating axis public: /// Constructor OverlappingPair(ProxyShape* shape1, @@ -43,9 +27,9 @@ namespace ephysics { /// Update the contact cache void update(); /// Return the cached separating axis - vec3 getCachedSeparatingAxis() ; + Vector3f getCachedSeparatingAxis() ; /// Set the cached separating axis - void setCachedSeparatingAxis( vec3 axis); + void setCachedSeparatingAxis( Vector3f axis); /// Return the number of contacts in the cache int getNbContactPoints() ; /// Return the a reference to the contact manifold set diff --git a/src/org/atriaSoft/ephysics/engine/Profiler.cpp b/src/org/atriaSoft/ephysics/engine/Profiler.cpp index 3be0bff..07907fe 100644 --- a/src/org/atriaSoft/ephysics/engine/Profiler.cpp +++ b/src/org/atriaSoft/ephysics/engine/Profiler.cpp @@ -42,16 +42,16 @@ ProfileNode* ProfileNode::findSubNode( char* name) { // Try to find the node among the child nodes ProfileNode* child = this.childNode; while (child != null) { - if (child->this.name == name) { + if (child.this.name == name) { return child; } - child = child->this.siblingNode; + child = child.this.siblingNode; } // The nose has not been found. Therefore, we create it // and add it to the profiler tree ProfileNode* newNode = ETKNEW(ProfileNode, name, this); - newNode->this.siblingNode = this.childNode; + newNode.this.siblingNode = this.childNode; this.childNode = newNode; return newNode; @@ -76,7 +76,7 @@ void ProfileNode::enterBlockOfCode() { boolean ProfileNode::exitBlockOfCode() { this.recursionCounter--; - if (this.recursionCounter == 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.numberTotalCalls != 0) { + if (this.recursionCounter == 0 && this.numberTotalCalls != 0) { // Get the current system time long double currentTime = Timer::getCurrentSystemTime() * 1000.0; @@ -96,12 +96,12 @@ void ProfileNode::reset() { // Reset the child node if (this.childNode != null) { - this.childNode->reset(); + this.childNode.reset(); } // Reset the sibling node if (this.siblingNode != null) { - this.siblingNode->reset(); + this.siblingNode.reset(); } } @@ -116,30 +116,30 @@ void ProfileNode::destroy() { // Constructor ProfileNodeIterator::ProfileNodeIterator(ProfileNode* startingNode) :this.currentParentNode(startingNode), - this.currentChildNode(this.currentParentNode->getChildNode()){ + this.currentChildNode(this.currentParentNode.getChildNode()){ } // Enter a given child node void ProfileNodeIterator::enterChild(int index) { - this.currentChildNode = this.currentParentNode->getChildNode(); - while ((this.currentChildNode != null) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (index != 0)) { + this.currentChildNode = this.currentParentNode.getChildNode(); + while ((this.currentChildNode != null) && (index != 0)) { index--; - this.currentChildNode = this.currentChildNode->getSiblingNode(); + this.currentChildNode = this.currentChildNode.getSiblingNode(); } if (this.currentChildNode != null) { this.currentParentNode = this.currentChildNode; - this.currentChildNode = this.currentParentNode->getChildNode(); + this.currentChildNode = this.currentParentNode.getChildNode(); } } // Enter a given parent node void ProfileNodeIterator::enterParent() { - if (this.currentParentNode->getParentNode() != null) { - this.currentParentNode = this.currentParentNode->getParentNode(); + if (this.currentParentNode.getParentNode() != null) { + this.currentParentNode = this.currentParentNode.getParentNode(); } - this.currentChildNode = this.currentParentNode->getChildNode(); + this.currentChildNode = this.currentParentNode.getChildNode(); } // Method called when we want to start profiling a block of code. @@ -147,12 +147,12 @@ void Profiler::startProfilingBlock( char* name) { // Look for the node in the tree that corresponds to the block of // code to profile - if (name != this.currentNode->getName()) { - this.currentNode = mCurrentNode->findSubNode(name); + if (name != this.currentNode.getName()) { + this.currentNode = mCurrentNode.findSubNode(name); } // Start profile the node - this.currentNode->enterBlockOfCode(); + this.currentNode.enterBlockOfCode(); } // Method called at the end of the scope where the @@ -161,8 +161,8 @@ void Profiler::stopProfilingBlock() { // Go to the parent node unless if the current block // of code is recursing - if (this.currentNode->exitBlockOfCode()) { - this.currentNode = mCurrentNode->getParentNode(); + if (this.currentNode.exitBlockOfCode()) { + this.currentNode = mCurrentNode.getParentNode(); } } @@ -175,7 +175,7 @@ void Profiler::reset() { } // Print the report of the profiler in a given output stream -void Profiler::printReport(etk::Stream stream) { +void Profiler::printReport(Stream stream) { ProfileNodeIterator* iterator = Profiler::getIterator(); // Recursively print the report of each node of the profiler tree @@ -188,61 +188,61 @@ void Profiler::printReport(etk::Stream stream) { // Recursively print the report of a given node of the profiler tree void Profiler::printRecursiveNodeReport(ProfileNodeIterator* iterator, int spacing, - etk::Stream stream) { - iterator->first(); + Stream stream) { + iterator.first(); // If we are at the end of a branch in the profiler tree - if (iterator->isEnd()) { + if (iterator.isEnd()) { return; } - long double parentTime = iterator->isRoot() ? getElapsedTimeSinceStart() : - iterator->getCurrentParentTotalTime(); + long double parentTime = iterator.isRoot() ? getElapsedTimeSinceStart() : + iterator.getCurrentParentTotalTime(); long double accumulatedTime = 0.0; int nbFrames = Profiler::getNbFrames(); for (int i=0; igetCurrentParentName() << - " (total running time : " << parentTime << " ms) ---\n"; + stream + "---------------\n"; + for (int i=0; iisEnd(); i++, iterator->next()) { + for (int i=0; !iterator.isEnd(); i++, iterator.next()) { nbChildren++; - long double currentTotalTime = iterator->getCurrentTotalTime(); + long double currentTotalTime = iterator.getCurrentTotalTime(); accumulatedTime += currentTotalTime; long double fraction = parentTime > DLBEPSILON ? (currentTotalTime / parentTime) * 100.0 : 0.0; - for (int j=0; jgetCurrentName() << " : " << - fraction << " % | " << (currentTotalTime / (long double) (nbFrames)) << - " ms/frame (" << iterator->getCurrentNbTotalCalls() << " calls)\n"; + for (int j=0; j DLBEPSILON ? ((parentTime - accumulatedTime) / parentTime) * 100.0 : 0.0; long double difference = parentTime - accumulatedTime; - stream << "| Unaccounted : " << difference << " ms (" << percentage << " %)\n"; + stream + "| Unaccounted : " + difference + " ms (" + percentage + " %)\n"; for (int i=0; ienterChild(i); + iterator.enterChild(i); printRecursiveNodeReport(iterator, spacing + 3, stream); - iterator->enterParent(); + iterator.enterParent(); } } // Return true if we are at the root of the profiler tree boolean ProfileNodeIterator::isRoot() { - return (this.currentParentNode->getParentNode() == null); + return (this.currentParentNode.getParentNode() == null); } // Return true if we are at the end of a branch of the profiler tree @@ -252,42 +252,42 @@ boolean ProfileNodeIterator::isEnd() { // Return the name of the current node char* ProfileNodeIterator::getCurrentName() { - return this.currentChildNode->getName(); + return this.currentChildNode.getName(); } // Return the total time of the current node long double ProfileNodeIterator::getCurrentTotalTime() { - return this.currentChildNode->getTotalTime(); + return this.currentChildNode.getTotalTime(); } // Return the total number of calls of the current node int ProfileNodeIterator::getCurrentNbTotalCalls() { - return this.currentChildNode->getNbTotalCalls(); + return this.currentChildNode.getNbTotalCalls(); } // Return the name of the current parent node char* ProfileNodeIterator::getCurrentParentName() { - return this.currentParentNode->getName(); + return this.currentParentNode.getName(); } // Return the total time of the current parent node long double ProfileNodeIterator::getCurrentParentTotalTime() { - return this.currentParentNode->getTotalTime(); + return this.currentParentNode.getTotalTime(); } // Return the total number of calls of the current parent node int ProfileNodeIterator::getCurrentParentNbTotalCalls() { - return this.currentParentNode->getNbTotalCalls(); + return this.currentParentNode.getNbTotalCalls(); } // Go to the first node void ProfileNodeIterator::first() { - this.currentChildNode = this.currentParentNode->getChildNode(); + this.currentChildNode = this.currentParentNode.getChildNode(); } // Go to the next node void ProfileNodeIterator::next() { - this.currentChildNode = this.currentChildNode->getSiblingNode(); + this.currentChildNode = this.currentChildNode.getSiblingNode(); } // Return a pointer to the parent node diff --git a/src/org/atriaSoft/ephysics/engine/Profiler.hpp b/src/org/atriaSoft/ephysics/engine/Profiler.java similarity index 79% rename from src/org/atriaSoft/ephysics/engine/Profiler.hpp rename to src/org/atriaSoft/ephysics/engine/Profiler.java index 0517407..f981deb 100644 --- a/src/org/atriaSoft/ephysics/engine/Profiler.hpp +++ b/src/org/atriaSoft/ephysics/engine/Profiler.java @@ -1,30 +1,17 @@ -/** @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.engine; +/* + * @brief It represents a profile sample in the profiler tree. */ -#pragma once - -#ifdef ISPROFILINGACTIVE -#include -#include -namespace ephysics { - /** - * @brief It represents a profile sample in the profiler tree. - */ class ProfileNode { private : - char* this.name; //!< Name of the node - int this.numberTotalCalls; //!< Total number of calls of this node - long double this.startTime; //!< Starting time of the sampling of corresponding block of code - long double this.totalTime; //!< Total time spent in the block of code - int this.recursionCounter; //!< Recursion counter - ProfileNode* this.parentNode; //!< Pointer to the parent node - ProfileNode* this.childNode; //!< Pointer to a child node - ProfileNode* this.siblingNode; //!< Pointer to a sibling node + char* name; //!< Name of the node + int numberTotalCalls; //!< Total number of calls of this node + long double startTime; //!< Starting time of the sampling of corresponding block of code + long double totalTime; //!< Total time spent in the block of code + int recursionCounter; //!< Recursion counter + ProfileNode* parentNode; //!< Pointer to the parent node + ProfileNode* childNode; //!< Pointer to a child node + ProfileNode* siblingNode; //!< Pointer to a sibling node public : /// Constructor ProfileNode( char* name, ProfileNode* parentNode); @@ -102,7 +89,7 @@ namespace ephysics { private: static void printRecursiveNodeReport(ProfileNodeIterator* iterator, int spacing, - etk::Stream outputStream); + Stream outputStream); public : /// Method called when we want to start profiling a block of code. static void startProfilingBlock( char *name); @@ -120,7 +107,7 @@ namespace ephysics { /// Return an iterator over the profiler tree starting at the root static ProfileNodeIterator* getIterator(); /// Print the report of the profiler in a given output stream - static void printReport(etk::Stream outputStream); + static void printReport(Stream outputStream); /// Destroy a previously allocated iterator static void destroyIterator(ProfileNodeIterator* iterator); /// Destroy the profiler (release the memory) diff --git a/src/org/atriaSoft/ephysics/engine/Timer.cpp b/src/org/atriaSoft/ephysics/engine/Timer.cpp deleted file mode 100644 index 4b9fba3..0000000 --- a/src/org/atriaSoft/ephysics/engine/Timer.cpp +++ /dev/null @@ -1,75 +0,0 @@ -/** @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) - */ -#include -#include - - -ephysics::Timer::Timer(double timeStep) : this.timeStep(timeStep), this.isRunning(false) { - assert(timeStep > 0.0); -} - -ephysics::Timer::~Timer() { - -} - -long double ephysics::Timer::getCurrentSystemTime() { - return (long double)(echrono::Clock::now().get()) / 1000000000.0; -} - -double ephysics::Timer::getTimeStep() { - return this.timeStep; -} - -void ephysics::Timer::setTimeStep(double timeStep) { - assert(timeStep > 0.0f); - this.timeStep = timeStep; -} - -long double ephysics::Timer::getPhysicsTime() { - return this.lastUpdateTime; -} - -boolean ephysics::Timer::getIsRunning() { - return this.isRunning; -} - -void ephysics::Timer::start() { - if (!this.isRunning) { - // Get the current system time - this.lastUpdateTime = getCurrentSystemTime(); - this.accumulator = 0.0; - this.isRunning = true; - } -} - -void ephysics::Timer::stop() { - this.isRunning = false; -} - -boolean ephysics::Timer::isPossibleToTakeStep() { - return (this.accumulator >= this.timeStep); -} - -void ephysics::Timer::nextStep() { - assert(this.isRunning); - // Update the accumulator value - this.accumulator -= this.timeStep; -} - -float ephysics::Timer::computeInterpolationFactor() { - return (float(this.accumulator / this.timeStep)); -} - -void ephysics::Timer::update() { - long double currentTime = getCurrentSystemTime(); - this.deltaTime = currentTime - this.lastUpdateTime; - this.lastUpdateTime = currentTime; - this.accumulator += this.deltaTime; -} - diff --git a/src/org/atriaSoft/ephysics/engine/Timer.hpp b/src/org/atriaSoft/ephysics/engine/Timer.hpp deleted file mode 100644 index 5d92561..0000000 --- a/src/org/atriaSoft/ephysics/engine/Timer.hpp +++ /dev/null @@ -1,58 +0,0 @@ -/** @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) - */ -#include -#include - -namespace ephysics { - /** - * @brief This class will take care of the time in the physics engine. It - * uses functions that depend on the current platform to get the - * current time. - */ - class Timer { - private : - double this.timeStep; //!< Timestep dt of the physics engine (timestep > 0.0) - long double this.lastUpdateTime; //!< Last time the timer has been updated - long double this.deltaTime; //!< Time difference between the two last timer update() calls - double this.accumulator; //!< Used to fix the time step and avoid strange time effects - boolean this.isRunning; //!< True if the timer is running - /// Private copy-ructor - Timer( Timer timer); - /// Private assignment operator - Timer operator=( Timer timer); - public : - /// Constructor - Timer(double timeStep); - /// Destructor - virtual ~Timer(); - /// Return the timestep of the physics engine - double getTimeStep() ; - /// Set the timestep of the physics engine - void setTimeStep(double timeStep); - /// Return the current time of the physics engine - long double getPhysicsTime() ; - /// Start the timer - void start(); - /// Stop the timer - void stop(); - /// Return true if the timer is running - boolean getIsRunning() ; - /// True if it's possible to take a new step - boolean isPossibleToTakeStep() ; - /// Compute the time since the last update() call and add it to the accumulator - void update(); - /// Take a new step => update the timer by adding the timeStep value to the current time - void nextStep(); - /// Compute the interpolation factor - float computeInterpolationFactor(); - /// Return the current time of the system in seconds - static long double getCurrentSystemTime(); - }; - -} diff --git a/src/org/atriaSoft/ephysics/engine/Timer.java b/src/org/atriaSoft/ephysics/engine/Timer.java new file mode 100644 index 0000000..328b10e --- /dev/null +++ b/src/org/atriaSoft/ephysics/engine/Timer.java @@ -0,0 +1,74 @@ +package org.atriaSoft.ephysics.engine; + +/** + * @brief This class will take care of the time in the physics engine. It + * uses functions that depend on the current platform to get the + * current time. + */ +class Timer { + private double timeStep; //!< Timestep dt of the physics engine (timestep > 0.0) + private double lastUpdateTime; //!< Last time the timer has been updated + private double deltaTime; //!< Time difference between the two last timer update() calls + private double accumulator; //!< Used to fix the time step and avoid strange time effects + private boolean isRunning; //!< True if the timer is running + /// Constructor + public Timer(double timeStep) { + this.timeStep = timeStep; + this.isRunning = false; + } + /// Return the timestep of the physics engine + public double getTimeStep() { + return this.timeStep; + } + /// Set the timestep of the physics engine + public void setTimeStep(double timeStep) { + assert(timeStep > 0.0f); + this.timeStep = timeStep; + } + /// Return the current time of the physics engine + public double getPhysicsTime() { + return this.lastUpdateTime; + } + /// Start the timer + public void start() { + if (!this.isRunning) { + // Get the current system time + this.lastUpdateTime = getCurrentSystemTime(); + this.accumulator = 0.0; + this.isRunning = true; + } + } + /// Stop the timer + public void stop() { + this.isRunning = false; + } + /// Return true if the timer is running + public boolean getIsRunning() { + return this.isRunning; + } + /// True if it's possible to take a new step + public boolean isPossibleToTakeStep() { + return (this.accumulator >= this.timeStep); + } + /// Compute the time since the last update() call and add it to the accumulator + public void update() { + double currentTime = getCurrentSystemTime(); + this.deltaTime = currentTime - this.lastUpdateTime; + this.lastUpdateTime = currentTime; + this.accumulator += this.deltaTime; + } + /// Take a new step => update the timer by adding the timeStep value to the current time + public void nextStep() { + assert(this.isRunning); + // Update the accumulator value + this.accumulator -= this.timeStep; + } + /// Compute the interpolation factor + public float computeInterpolationFactor() { + return (float)(this.accumulator / this.timeStep); + } + /// Return the current time of the system in seconds + public static double getCurrentSystemTime(){ + return (double)(System.currentTimeMillis()) / 1000.0; + } +} diff --git a/src/org/atriaSoft/ephysics/ephysics.hpp b/src/org/atriaSoft/ephysics/ephysics.hpp deleted file mode 100644 index 0e3dc8a..0000000 --- a/src/org/atriaSoft/ephysics/ephysics.hpp +++ /dev/null @@ -1,38 +0,0 @@ -/** @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) - */ -#pragma once - -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - diff --git a/src/org/atriaSoft/ephysics/mathematics/Mathematics.java b/src/org/atriaSoft/ephysics/mathematics/Mathematics.java new file mode 100644 index 0000000..4560cba --- /dev/null +++ b/src/org/atriaSoft/ephysics/mathematics/Mathematics.java @@ -0,0 +1,73 @@ +package org.atriaSoft.ephysics.mathematics; + +import org.atriaSoft.etk.math.Vector3f; +import org.atriaSoft.ephysics.Property; + +class Mathematics { + // ---------- Mathematics functions ---------- // + /// 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 boolean approxEqual(float a, float b, float epsilon) { + return (Math.abs(a - b) < epsilon); + } + public boolean approxEqual(float a, float b) { + return (Math.abs(a - b) < Property.FLTEPSILON); + } + + /// Function that returns the result of the "value" clamped by + /// two others values "lowerLimit" and "upperLimit" + public int clamp(int value, int lowerLimit, int upperLimit) { + assert(lowerLimit <= upperLimit); + return Math.min(Math.max(value, lowerLimit), upperLimit); + } + + /// Function that returns the result of the "value" clamped by + /// two others values "lowerLimit" and "upperLimit" + public float clamp(float value, float lowerLimit, float upperLimit) { + assert(lowerLimit <= upperLimit); + return Math.min(Math.max(value, lowerLimit), upperLimit); + } + + /// Return the minimum value among three values + public float min3(float a, float b, float c) { + return Math.min(Math.min(a, b), c); + } + + /// Return the maximum value among three values + public float max3(float a, float b, float c) { + return Math.max(Math.max(a, b), c); + } + + /// Return true if two values have the same sign + public boolean sameSign(float a, float b) { + return a * b >= 0.0f; + } + + /// Clamp a vector such that it is no longer than a given maximum length + public Vector3f clamp( Vector3f vector, float maxLength) { + if (vector.length2() > maxLength * maxLength) { + return vector.safeNormalize_new().multiply(maxLength); + } + return vector; + } + + /// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) + public void computeBarycentricCoordinatesInTriangle( Vector3f a, Vector3f b, Vector3f c, + Vector3f p, float u, float v, float w) { + Vector3f v0 = b.less_new(a); + Vector3f v1 = c.less_new(a); + Vector3f v2 = p.less_new(a); + + float d00 = v0.dot(v0); + float d01 = v0.dot(v1); + float d11 = v1.dot(v1); + float d20 = v2.dot(v0); + float d21 = v2.dot(v1); + + float denom = d00 * d11 - d01 * d01; + v = (d11 * d20 - d01 * d21) / denom; + w = (d00 * d21 - d01 * d20) / denom; + u = 1.0f - v - w; + } + +} diff --git a/src/org/atriaSoft/ephysics/mathematics/Ray.hpp b/src/org/atriaSoft/ephysics/mathematics/Ray.hpp deleted file mode 100644 index 449a150..0000000 --- a/src/org/atriaSoft/ephysics/mathematics/Ray.hpp +++ /dev/null @@ -1,48 +0,0 @@ -/** @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) - */ -#pragma once - -#include - -namespace ephysics { - /** - * This structure represents a 3D ray represented by two points. - * The ray goes from point1 to point1 + maxFraction * (point2 - point1). - * The points are specified in world-space coordinates. - */ - struct Ray { - public: - vec3 point1; //! 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) - */ -#pragma once - -// Libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - diff --git a/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.cpp b/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.cpp deleted file mode 100644 index 04244f2..0000000 --- a/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/** @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) - */ -#include -#include - -using namespace ephysics; - -/// 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. -void ephysics::computeBarycentricCoordinatesInTriangle( vec3 a, vec3 b, vec3 c, - vec3 p, float u, float v, float w) { - vec3 v0 = b - a; - vec3 v1 = c - a; - vec3 v2 = p - a; - - float d00 = v0.dot(v0); - float d01 = v0.dot(v1); - float d11 = v1.dot(v1); - float d20 = v2.dot(v0); - float d21 = v2.dot(v1); - - float denom = d00 * d11 - d01 * d01; - v = (d11 * d20 - d01 * d21) / denom; - w = (d00 * d21 - d01 * d20) / denom; - u = 1.0f - v - w; -} - -// Clamp a vector such that it is no longer than a given maximum length -vec3 ephysics::clamp( vec3 vector, float maxLength) { - if (vector.length2() > maxLength * maxLength) { - return vector.safeNormalized() * maxLength; - } - return vector; -} diff --git a/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.hpp b/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.hpp deleted file mode 100644 index 854d850..0000000 --- a/src/org/atriaSoft/ephysics/mathematics/mathematics_functions.hpp +++ /dev/null @@ -1,59 +0,0 @@ -/** @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) - */ -#include -#include - -/// ReactPhysics3D namespace -namespace ephysics { - -// ---------- Mathematics functions ---------- // - -/// 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] -inline boolean approxEqual(float a, float b, float epsilon = FLTEPSILON) { - return (etk::abs(a - b) < epsilon); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline int clamp(int value, int lowerLimit, int upperLimit) { - assert(lowerLimit <= upperLimit); - return etk::min(etk::max(value, lowerLimit), upperLimit); -} - -/// Function that returns the result of the "value" clamped by -/// two others values "lowerLimit" and "upperLimit" -inline float clamp(float value, float lowerLimit, float upperLimit) { - assert(lowerLimit <= upperLimit); - return etk::min(etk::max(value, lowerLimit), upperLimit); -} - -/// Return the minimum value among three values -inline float min3(float a, float b, float c) { - return etk::min(etk::min(a, b), c); -} - -/// Return the maximum value among three values -inline float max3(float a, float b, float c) { - return etk::max(etk::max(a, b), c); -} - -/// Return true if two values have the same sign -inline boolean sameSign(float a, float b) { - return a * b >= 0.0f; -} - -/// Clamp a vector such that it is no longer than a given maximum length -vec3 clamp( vec3 vector, float maxLength); - -/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c) -void computeBarycentricCoordinatesInTriangle( vec3 a, vec3 b, vec3 c, - vec3 p, float u, float v, float w); - -}