[DEV] port some element of ephysiscs

This commit is contained in:
Edouard DUPIN 2020-06-28 21:22:24 +02:00
parent 7b6855ec0e
commit f5ab47bade
128 changed files with 4000 additions and 5185 deletions

View File

@ -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,
}

View File

@ -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
}

View File

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

View File

@ -0,0 +1,70 @@
package org.atriaSoft.ephysics;
public class Property {
// ------------------- Type definitions ------------------- //
//typedef Pair<long, long> 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;
}

View File

@ -112,7 +112,7 @@ class Body {
* @return true if the current element is smaller * @return true if the current element is smaller
*/ */
public boolean isLess( Body obj) { public boolean isLess( Body obj) {
return (this.id < obj.this.id); return (this.id < obj.id);
} }
/** /**
* @brief Larger than operator * @brief Larger than operator
@ -120,7 +120,7 @@ class Body {
* @return true if the current element is Bigger * @return true if the current element is Bigger
*/ */
public boolean isUpper( Body obj) { public boolean isUpper( Body obj) {
return (this.id > obj.this.id); return (this.id > obj.id);
} }
/** /**
* @brief Equal operator * @brief Equal operator
@ -128,7 +128,7 @@ class Body {
* @return true if the curretn element is equal * @return true if the curretn element is equal
*/ */
public boolean isEqual( Body obj) { public boolean isEqual( Body obj) {
return (this.id == obj.this.id); return (this.id == obj.id);
} }
/** /**
* @brief Not equal operator * @brief Not equal operator
@ -136,6 +136,6 @@ class Body {
* @return true if the curretn element is NOT equal * @return true if the curretn element is NOT equal
*/ */
public boolean isDifferent( Body obj) { public boolean isDifferent( Body obj) {
return (this.id != obj.this.id); return (this.id != obj.id);
} }
} }

View File

@ -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.
}

View File

@ -1,238 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/CollisionBody.hpp>
#include <ephysics/engine/CollisionWorld.hpp>
#include <ephysics/collision/ContactManifold.hpp>
// 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;
}

View File

@ -1,209 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/Body.hpp>
#include <etk/math/Transform3D.hpp>
#include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/collision/RaycastInfo.hpp>
#include <ephysics/configuration.hpp>
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;
};
}

View File

@ -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;
}
}

View File

@ -1,316 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/RigidBody.hpp>
#include <ephysics/raint/Joint.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/engine/DynamicsWorld.hpp>
#include <ephysics/debug.hpp>
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<DynamicsWorld>(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;
}

View File

@ -1,294 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/CollisionBody.hpp>
#include <ephysics/engine/Material.hpp>
#include <ephysics/mathematics/mathematics.hpp>
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;
};
}

View File

@ -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<DynamicsWorld>(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);
}
}

View File

@ -40,7 +40,7 @@ void CollisionDetection::computeCollisionDetection() {
computeNarrowPhase(); computeNarrowPhase();
} }
void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) { void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
computeBroadPhase(); computeBroadPhase();
// Delete all the contact points in the currently overlapping pairs // Delete all the contact points in the currently overlapping pairs
@ -49,54 +49,54 @@ void CollisionDetection::testCollisionBetweenShapes(CollisionCallback* callback,
computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2); computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
} }
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) { void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
// For each possible collision pair of bodies // For each possible collision pair of bodies
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) { for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it.second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair.getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair.getShape2();
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if ( !shapes1.empty() if ( !shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty() && !shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0 && ( shapes1.count(shape1.this.broadPhaseID) == 0
|| shapes2.count(shape2->this.broadPhaseID) == 0 ) || shapes2.count(shape2.this.broadPhaseID) == 0 )
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0 && ( shapes1.count(shape2.this.broadPhaseID) == 0
|| shapes2.count(shape1->this.broadPhaseID) == 0 ) ) { || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
continue; continue;
} }
if ( !shapes1.empty() if ( !shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty() && shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0 && shapes1.count(shape1.this.broadPhaseID) == 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) { && shapes1.count(shape2.this.broadPhaseID) == 0) {
continue; continue;
} }
if ( !shapes2.empty() if ( !shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty() && shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0 && shapes2.count(shape1.this.broadPhaseID) == 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) { && shapes2.count(shape2.this.broadPhaseID) == 0) {
continue; continue;
} }
// For each contact manifold set of the overlapping pair // For each contact manifold set of the overlapping pair
ContactManifoldSet manifoldSet = pair->getContactManifoldSet(); ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) { for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
ContactManifold* manifold = manifoldSet.getContactManifold(j); ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set // For each contact manifold of the manifold set
for (int i=0; i<manifold->getNbContactPoints(); i++) { for (int i=0; i<manifold.getNbContactPoints(); i++) {
ContactPoint* contactPoint = manifold->getContactPoint(i); ContactPoint* contactPoint = manifold.getContactPoint(i);
// Create the contact info object for the contact // Create the contact info object for the contact
ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(), ContactPointInfo contactInfo(manifold.getShape1(), manifold.getShape2(),
manifold->getShape1()->getCollisionShape(), manifold.getShape1().getCollisionShape(),
manifold->getShape2()->getCollisionShape(), manifold.getShape2().getCollisionShape(),
contactPoint->getNormal(), contactPoint.getNormal(),
contactPoint->getPenetrationDepth(), contactPoint.getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(), contactPoint.getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2()); contactPoint.getLocalPointOnBody2());
// Notify the collision callback about this new contact // Notify the collision callback about this new contact
if (callback != null) { if (callback != null) {
callback->notifyContact(contactInfo); callback.notifyContact(contactInfo);
} }
} }
} }
@ -119,35 +119,35 @@ void CollisionDetection::computeNarrowPhase() {
// Clear the set of overlapping pairs in narrow-phase contact // Clear the set of overlapping pairs in narrow-phase contact
this.contactOverlappingPairs.clear(); this.contactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it.second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair.getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair.getShape2();
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if (((shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 || if (((shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0) || (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0) ||
!this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) { !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
ETKDELETE(OverlappingPair, it->second); ETKDELETE(OverlappingPair, it.second);
it->second = null; it.second = null;
it = this.overlappingPairs.erase(it); it = this.overlappingPairs.erase(it);
continue; continue;
} else { } else {
++it; ++it;
} }
CollisionBody* body1 = shape1->getBody(); CollisionBody* body1 = shape1.getBody();
CollisionBody* body2 = shape2->getBody(); CollisionBody* body2 = shape2.getBody();
// Update the contact cache of the overlapping pair // Update the contact cache of the overlapping pair
pair->update(); pair.update();
// Check that at least one body is awake and not static // Check that at least one body is awake and not static
boolean isBody1Active = !body1->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body1->getType() != STATIC; boolean isBody1Active = !body1.isSleeping() && body1.getType() != STATIC;
boolean isBody2Active = !body2->isSleeping() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj body2->getType() != STATIC; boolean isBody2Active = !body2.isSleeping() && body2.getType() != STATIC;
if (!isBody1Active hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !isBody2Active) { if (!isBody1Active && !isBody2Active) {
continue; continue;
} }
// Check if the bodies are in the set of bodies that cannot collide between each other // Check if the bodies are in the set of bodies that cannot collide between each other
@ -156,85 +156,85 @@ void CollisionDetection::computeNarrowPhase() {
continue; continue;
} }
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == null) { if (narrowPhaseAlgorithm == null) {
continue; continue;
} }
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(), CollisionShapeInfo shape1Info(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData()); pair, shape1.getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(), CollisionShapeInfo shape2Info(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData()); pair, shape2.getCachedCollisionData());
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the // if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called. // 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 // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, etk::Set<int> shapes1, etk::Set<int> shapes2) { void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback, Set<int> shapes1, Set<int> shapes2) {
this.contactOverlappingPairs.clear(); this.contactOverlappingPairs.clear();
// For each possible collision pair of bodies // For each possible collision pair of bodies
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
OverlappingPair* pair = it->second; OverlappingPair* pair = it.second;
ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape1 = pair.getShape1();
ProxyShape* shape2 = pair->getShape2(); ProxyShape* shape2 = pair.getShape2();
assert(shape1->this.broadPhaseID != shape2->this.broadPhaseID); assert(shape1.this.broadPhaseID != shape2.this.broadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that // If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one // shape1 is among on set and shape2 is among the other one
if ( !shapes1.empty() if ( !shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj !shapes2.empty() && !shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape1->this.broadPhaseID) == 0 && ( shapes1.count(shape1.this.broadPhaseID) == 0
|| shapes2.count(shape2->this.broadPhaseID) == 0 ) || shapes2.count(shape2.this.broadPhaseID) == 0 )
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj ( shapes1.count(shape2->this.broadPhaseID) == 0 && ( shapes1.count(shape2.this.broadPhaseID) == 0
|| shapes2.count(shape1->this.broadPhaseID) == 0 ) ) { || shapes2.count(shape1.this.broadPhaseID) == 0 ) ) {
++it; ++it;
continue; continue;
} }
if ( !shapes1.empty() if ( !shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.empty() && shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape1->this.broadPhaseID) == 0 && shapes1.count(shape1.this.broadPhaseID) == 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.count(shape2->this.broadPhaseID) == 0) { && shapes1.count(shape2.this.broadPhaseID) == 0) {
++it; ++it;
continue; continue;
} }
if ( !shapes2.empty() if ( !shapes2.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes1.empty() && shapes1.empty()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape1->this.broadPhaseID) == 0 && shapes2.count(shape1.this.broadPhaseID) == 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapes2.count(shape2->this.broadPhaseID) == 0) { && shapes2.count(shape2.this.broadPhaseID) == 0) {
++it; ++it;
continue; continue;
} }
// Check if the collision filtering allows collision between the two shapes and // Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the // that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair // overlapping pair
if ( ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 if ( ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0
|| (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0 ) || (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0 )
|| !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) { || !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2) ) {
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
ETKDELETE(OverlappingPair, it->second); ETKDELETE(OverlappingPair, it.second);
it->second = null; it.second = null;
it = this.overlappingPairs.erase(it); it = this.overlappingPairs.erase(it);
continue; continue;
} else { } else {
++it; ++it;
} }
CollisionBody* body1 = shape1->getBody(); CollisionBody* body1 = shape1.getBody();
CollisionBody* body2 = shape2->getBody(); CollisionBody* body2 = shape2.getBody();
// Update the contact cache of the overlapping pair // 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 // 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; continue;
} }
longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2); longpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
@ -242,48 +242,48 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
continue; continue;
} }
// Check if the two bodies are sleeping, if so, we do no test collision between them // 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; continue;
} }
// Select the narrow phase algorithm to use according to the two collision shapes // Select the narrow phase algorithm to use according to the two collision shapes
CollisionShapeType shape1Type = shape1->getCollisionShape()->getType(); CollisionShapeType shape1Type = shape1.getCollisionShape().getType();
CollisionShapeType shape2Type = shape2->getCollisionShape()->getType(); CollisionShapeType shape2Type = shape2.getCollisionShape().getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type]; NarrowPhaseAlgorithm* narrowPhaseAlgorithm = this.collisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes // If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == null) { if (narrowPhaseAlgorithm == null) {
continue; continue;
} }
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair); narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, CollisionShapeInfo shape1Info(shape1,
shape1->getCollisionShape(), shape1.getCollisionShape(),
shape1->getLocalToWorldTransform(), shape1.getLocalToWorldTransform(),
pair, pair,
shape1->getCachedCollisionData()); shape1.getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, CollisionShapeInfo shape2Info(shape2,
shape2->getCollisionShape(), shape2.getCollisionShape(),
shape2->getLocalToWorldTransform(), shape2.getLocalToWorldTransform(),
pair, pair,
shape2->getCachedCollisionData()); shape2.getCachedCollisionData());
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback); TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
// Use the narrow-phase collision detection algorithm to check // Use the narrow-phase collision detection algorithm to check
// if there really is a collision // if there really is a collision
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, narrowPhaseCallback); narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback);
} }
// Add all the contact manifolds (between colliding bodies) to the bodies // Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies(); addAllContactManifoldsToBodies();
} }
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) { 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 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; return;
} }
// Check if the collision filtering allows collision between the two shapes // Check if the collision filtering allows collision between the two shapes
if ( (shape1->getCollideWithMaskBits() shape2->getCollisionCategoryBits()) == 0 if ( (shape1.getCollideWithMaskBits() shape2.getCollisionCategoryBits()) == 0
|| (shape1->getCollisionCategoryBits() shape2->getCollideWithMaskBits()) == 0) { || (shape1.getCollisionCategoryBits() shape2.getCollideWithMaskBits()) == 0) {
return; return;
} }
// Compute the overlapping pair ID // Compute the overlapping pair ID
@ -291,27 +291,27 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Check if the overlapping pair already exists // Check if the overlapping pair already exists
if (this.overlappingPairs.find(pairID) != this.overlappingPairs.end()) return; if (this.overlappingPairs.find(pairID) != this.overlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair // Compute the maximum number of contact manifolds for this pair
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(), int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1.getCollisionShape().getType(),
shape2->getCollisionShape()->getType()); shape2.getCollisionShape().getType());
// Create the overlapping pair and add it into the set of overlapping pairs // Create the overlapping pair and add it into the set of overlapping pairs
OverlappingPair* newPair = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds); OverlappingPair* newPair = ETKNEW(OverlappingPair, shape1, shape2, nbMaxManifolds);
assert(newPair != null); assert(newPair != null);
this.overlappingPairs.set(pairID, newPair); this.overlappingPairs.set(pairID, newPair);
// Wake up the two bodies // Wake up the two bodies
shape1->getBody()->setIsSleeping(false); shape1.getBody().setIsSleeping(false);
shape2->getBody()->setIsSleeping(false); shape2.getBody().setIsSleeping(false);
} }
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Remove all the overlapping pairs involving this proxy shape // Remove all the overlapping pairs involving this proxy shape
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) { for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ) {
if (it->second->getShape1()->this.broadPhaseID == proxyShape->this.broadPhaseID|| if (it.second.getShape1().this.broadPhaseID == proxyShape.this.broadPhaseID||
it->second->getShape2()->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 // TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair // Destroy the overlapping pair
ETKDELETE(OverlappingPair, it->second); ETKDELETE(OverlappingPair, it.second);
it->second = null; it.second = null;
it = this.overlappingPairs.erase(it); it = this.overlappingPairs.erase(it);
} else { } else {
++it; ++it;
@ -323,17 +323,17 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) { void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
// If it is the first contact since the pairs are overlapping // If it is the first contact since the pairs are overlapping
if (overlappingPair->getNbContactPoints() == 0) { if (overlappingPair.getNbContactPoints() == 0) {
// Trigger a callback event // Trigger a callback event
if (this.world->this.eventListener != NULL) { if (this.world.this.eventListener != NULL) {
this.world->this.eventListener->beginContact(contactInfo); this.world.this.eventListener.beginContact(contactInfo);
} }
} }
// Create a new contact // Create a new contact
createContact(overlappingPair, contactInfo); createContact(overlappingPair, contactInfo);
// Trigger a callback event for the new contact // Trigger a callback event for the new contact
if (this.world->this.eventListener != NULL) { if (this.world.this.eventListener != NULL) {
this.world->this.eventListener->newContact(contactInfo); this.world.this.eventListener.newContact(contactInfo);
} }
} }
@ -341,46 +341,46 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, Contac
// Create a new contact // Create a new contact
ContactPoint* contact = ETKNEW(ContactPoint, contactInfo); ContactPoint* contact = ETKNEW(ContactPoint, contactInfo);
// Add the contact to the contact manifold set of the corresponding overlapping pair // 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 // Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(), overlappingpairid pairId = OverlappingPair::computeID(overlappingPair.getShape1(),
overlappingPair->getShape2()); overlappingPair.getShape2());
this.contactOverlappingPairs.set(pairId, overlappingPair); this.contactOverlappingPairs.set(pairId, overlappingPair);
} }
void CollisionDetection::addAllContactManifoldsToBodies() { void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase // For each overlapping pairs in contact during the narrow-phase
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.contactOverlappingPairs.begin(); it != this.contactOverlappingPairs.end(); ++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 // Add all the contact manifolds of the pair into the list of contact manifolds
// of the two bodies involved in the contact // of the two bodies involved in the contact
addContactManifoldToBody(it->second); addContactManifoldToBody(it.second);
} }
} }
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
assert(pair != null); assert(pair != null);
CollisionBody* body1 = pair->getShape1()->getBody(); CollisionBody* body1 = pair.getShape1().getBody();
CollisionBody* body2 = pair->getShape2()->getBody(); CollisionBody* body2 = pair.getShape2().getBody();
ContactManifoldSet manifoldSet = pair->getContactManifoldSet(); ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair // For each contact manifold in the set of manifolds in the pair
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) { for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i); ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
assert(contactManifold->getNbContactPoints() > 0); assert(contactManifold.getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked // Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body // 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 // Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body // 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() { void CollisionDetection::clearContactPoints() {
// For each overlapping pair // For each overlapping pair
etk::Map<overlappingpairid, OverlappingPair*>::Iterator it; Map<overlappingpairid, OverlappingPair*>::Iterator it;
for (it = this.overlappingPairs.begin(); it != this.overlappingPairs.end(); ++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 each possible type of collision shape
for (int i=0; i<NBCOLLISIONSHAPETYPES; i++) { for (int i=0; i<NBCOLLISIONSHAPETYPES; i++) {
for (int j=0; j<NBCOLLISIONSHAPETYPES; j++) { for (int j=0; j<NBCOLLISIONSHAPETYPES; j++) {
this.collisionMatrix[i][j] = this.collisionDispatch->selectAlgorithm(i, j); this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(i, j);
} }
} }
} }
EventListener* CollisionDetection::getWorldEventListener() { EventListener* CollisionDetection::getWorldEventListener() {
return this.world->this.eventListener; return this.world.this.eventListener;
} }
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) { void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) {
this.collisionCallback->notifyContact(contactInfo); this.collisionCallback.notifyContact(contactInfo);
} }
NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) { NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type) {
@ -407,7 +407,7 @@ NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeTy
void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) { void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
this.collisionDispatch = collisionDispatch; this.collisionDispatch = collisionDispatch;
this.collisionDispatch->init(this); this.collisionDispatch.init(this);
// Fill-in the collision matrix with the new algorithms to use // Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix(); fillInCollisionMatrix();
} }
@ -427,14 +427,14 @@ void CollisionDetection::removeNoCollisionPair(CollisionBody* body1, CollisionBo
} }
void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) { 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); 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()"); PROFILE("CollisionDetection::raycast()");
RaycastTest rayCastTest(raycastCallback); RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape() // 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) { boolean CollisionDetection::testAABBOverlap( ProxyShape* shape1, ProxyShape* shape2) {
// If one of the shape's body is not active, we return no overlap // If one of the shape's body is not active, we return no overlap
if ( !shape1->getBody()->isActive() if ( !shape1.getBody().isActive()
|| !shape2->getBody()->isActive()) { || !shape2.getBody().isActive()) {
return false; return false;
} }
return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2); return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);

View File

@ -1,32 +1,9 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/CollisionBody.hpp>
#include <ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp>
#include <ephysics/engine/OverlappingPair.hpp>
#include <ephysics/engine/EventListener.hpp>
#include <ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp>
#include <ephysics/raint/ContactPoint.hpp>
#include <etk/Vector.hpp>
#include <etk/Map.hpp>
#include <etk/Set.hpp>
namespace ephysics {
class BroadPhaseAlgorithm;
class CollisionWorld;
class CollisionCallback;
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { class TestCollisionBetweenShapesCallback extends NarrowPhaseCallback {
private: private:
CollisionCallback* this.collisionCallback; //!< CollisionCallback* collisionCallback; //!<
public: public:
// Constructor // Constructor
TestCollisionBetweenShapesCallback(CollisionCallback* callback): TestCollisionBetweenShapesCallback(CollisionCallback* callback):
@ -34,7 +11,7 @@ namespace ephysics {
} }
// Called by a narrow-phase collision algorithm when a new contact has been found // Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, void notifyContact(OverlappingPair* overlappingPair,
ContactPointInfo contactInfo); ContactPointInfo contactInfo);
}; };
@ -44,23 +21,19 @@ namespace ephysics {
* collide and then we run a narrow-phase algorithm to compute the * collide and then we run a narrow-phase algorithm to compute the
* collision contacts between bodies. * collision contacts between bodies.
*/ */
class CollisionDetection : public NarrowPhaseCallback { class CollisionDetection extends NarrowPhaseCallback {
private : private :
CollisionDispatch* this.collisionDispatch; //!< Collision Detection Dispatch configuration CollisionDispatch* collisionDispatch; //!< Collision Detection Dispatch configuration
DefaultCollisionDispatch this.defaultCollisionDispatch; //!< Default collision dispatch configuration DefaultCollisionDispatch defaultCollisionDispatch; //!< Default collision dispatch configuration
NarrowPhaseAlgorithm* this.collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use) NarrowPhaseAlgorithm* collisionMatrix[NBCOLLISIONSHAPETYPES][NBCOLLISIONSHAPETYPES]; //!< Collision detection matrix (algorithms to use)
CollisionWorld* this.world; //!< Pointer to the physics world CollisionWorld* world; //!< Pointer to the physics world
etk::Map<overlappingpairid, OverlappingPair*> this.overlappingPairs; //!< Broad-phase overlapping pairs Map<overlappingpairid, OverlappingPair*> overlappingPairs; //!< Broad-phase overlapping pairs
etk::Map<overlappingpairid, OverlappingPair*> this.contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection) Map<overlappingpairid, OverlappingPair*> contactOverlappingPairs; //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
BroadPhaseAlgorithm this.broadPhaseAlgorithm; //!< Broad-phase algorithm BroadPhaseAlgorithm broadPhaseAlgorithm; //!< Broad-phase algorithm
// TODO : Delete this // TODO : Delete this
GJKAlgorithm this.narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
etk::Set<longpair> this.noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other Set<longpair> noCollisionPairs; //!< Set of pair of bodies that cannot collide between each other
boolean this.isCollisionShapesAdded; //!< True if some collision shapes have been added previously boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
/// Private copy-ructor
CollisionDetection( CollisionDetection collisionDetection);
/// Private assignment operator
CollisionDetection operator=( CollisionDetection collisionDetection);
/// Compute the broad-phase collision detection /// Compute the broad-phase collision detection
void computeBroadPhase(); void computeBroadPhase();
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
@ -77,8 +50,6 @@ namespace ephysics {
public : public :
/// Constructor /// Constructor
CollisionDetection(CollisionWorld* world); CollisionDetection(CollisionWorld* world);
/// Destructor
~CollisionDetection();
/// Set the collision dispatch configuration /// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch); void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes /// Return the Narrow-phase collision detection algorithm to use between two types of shapes
@ -91,7 +62,7 @@ namespace ephysics {
/// Update a proxy collision shape (that has moved for instance) /// Update a proxy collision shape (that has moved for instance)
void updateProxyCollisionShape(ProxyShape* shape, void updateProxyCollisionShape(ProxyShape* shape,
AABB aabb, AABB aabb,
vec3 displacement = vec3(0, 0, 0), Vector3f displacement = Vector3f(0, 0, 0),
boolean forceReinsert = false); boolean forceReinsert = false);
/// Add a pair of bodies that cannot collide with each other /// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
@ -105,16 +76,16 @@ namespace ephysics {
void computeCollisionDetection(); void computeCollisionDetection();
/// Compute the collision detection /// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* callback, void testCollisionBetweenShapes(CollisionCallback* callback,
etk::Set<int> shapes1, Set<int> shapes1,
etk::Set<int> shapes2); Set<int> shapes2);
/// Report collision between two sets of shapes /// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* callback, void reportCollisionBetweenShapes(CollisionCallback* callback,
etk::Set<int> shapes1, Set<int> shapes1,
etk::Set<int> shapes2) ; Set<int> shapes2) ;
/// Ray casting method /// Ray casting method
void raycast(RaycastCallback* raycastCallback, void raycast(RaycastCallback* raycastCallback,
Ray ray, Ray ray,
unsigned short raycastWithCategoryMaskBits) ; int raycastWithCategoryMaskBits) ;
/// Test if the AABBs of two bodies overlap /// Test if the AABBs of two bodies overlap
boolean testAABBOverlap( CollisionBody* body1, boolean testAABBOverlap( CollisionBody* body1,
CollisionBody* body2) ; CollisionBody* body2) ;
@ -126,14 +97,14 @@ namespace ephysics {
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2); void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
/// Compute the narrow-phase collision detection /// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback, void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
etk::Set<int> shapes1, Set<int> shapes1,
etk::Set<int> shapes2); Set<int> shapes2);
/// Return a pointer to the world /// Return a pointer to the world
CollisionWorld* getWorld(); CollisionWorld* getWorld();
/// Return the world event listener /// Return the world event listener
EventListener* getWorldEventListener(); EventListener* getWorldEventListener();
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) override; void notifyContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo) ;
/// Create a new contact /// Create a new contact
void createContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo); void createContact(OverlappingPair* overlappingPair, ContactPointInfo contactInfo);
friend class DynamicsWorld; friend class DynamicsWorld;

View File

@ -11,7 +11,6 @@
#include <ephysics/collision/shapes/CollisionShape.hpp> #include <ephysics/collision/shapes/CollisionShape.hpp>
namespace ephysics { namespace ephysics {
class OverlappingPair;
/** /**
* @brief It regroups different things about a collision shape. This is * @brief It regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm. * used to pass information about a collision shape to a collision algorithm.
@ -21,12 +20,12 @@ namespace ephysics {
OverlappingPair* overlappingPair; //!< Broadphase overlapping pair OverlappingPair* overlappingPair; //!< Broadphase overlapping pair
ProxyShape* proxyShape; //!< Proxy shape ProxyShape* proxyShape; //!< Proxy shape
CollisionShape* collisionShape; //!< Pointer to the collision 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 void** cachedCollisionData; //!< Cached collision data of the proxy shape
/// Constructor /// Constructor
CollisionShapeInfo(ProxyShape* proxyCollisionShape, CollisionShapeInfo(ProxyShape* proxyCollisionShape,
CollisionShape* shape, CollisionShape* shape,
etk::Transform3D shapeLocalToWorldTransform, Transform3D shapeLocalToWorldTransform,
OverlappingPair* pair, OverlappingPair* pair,
void** cachedData): void** cachedData):
overlappingPair(pair), overlappingPair(pair),

View File

@ -16,9 +16,7 @@
namespace ephysics { namespace ephysics {
int MAXCONTACTPOINTSINMANIFOLD = 4; //!< Maximum number of contacts in the manifold 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 * @brief This structure represents a single element of a linked list of contact manifolds
*/ */
@ -54,25 +52,19 @@ namespace ephysics {
ContactManifold(ProxyShape* shape1, ContactManifold(ProxyShape* shape1,
ProxyShape* shape2, ProxyShape* shape2,
int16t normalDirectionId); int16t normalDirectionId);
/// Destructor
~ContactManifold();
/// DELETE copy-ructor
ContactManifold( ContactManifold contactManifold) = delete;
/// DELETE assignment operator
ContactManifold operator=( ContactManifold contactManifold) = delete;
private: private:
ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
ContactPoint* this.contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold ContactPoint* contactPoints[MAXCONTACTPOINTSINMANIFOLD]; //!< Contact points in the manifold
int16t this.normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction) int16t normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
int this.nbContactPoints; //!< Number of contacts in the cache int nbContactPoints; //!< Number of contacts in the cache
vec3 this.frictionVector1; //!< First friction vector of the contact manifold Vector3f frictionVector1; //!< First friction vector of the contact manifold
vec3 this.frictionvec2; //!< Second friction vector of the contact manifold Vector3f frictionvec2; //!< Second friction vector of the contact manifold
float this.frictionImpulse1; //!< First friction raint accumulated impulse float frictionImpulse1; //!< First friction raint accumulated impulse
float this.frictionImpulse2; //!< Second friction raint accumulated impulse float frictionImpulse2; //!< Second friction raint accumulated impulse
float this.frictionTwistImpulse; //!< Twist friction raint accumulated impulse float frictionTwistImpulse; //!< Twist friction raint accumulated impulse
vec3 this.rollingResistanceImpulse; //!< Accumulated rolling resistance impulse Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
boolean this.isAlreadyInIsland; //!< True if the contact manifold has already been added into an island boolean isAlreadyInIsland; //!< True if the contact manifold has already been added into an island
/// Return the index of maximum area /// Return the index of maximum area
int getMaxArea(float area0, float area1, float area2, float area3) ; 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 * 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). * 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 /// Remove a contact point from the manifold
void removeContactPoint(int index); void removeContactPoint(int index);
/// Return true if the contact manifold has already been added into an island /// 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 * the contacts with a too large distance between the contact points in the plane orthogonal to the
* contact normal. * contact normal.
*/ */
void update( etk::Transform3D transform1, void update( Transform3D transform1,
etk::Transform3D transform2); Transform3D transform2);
/// Clear the contact manifold /// Clear the contact manifold
void clear(); void clear();
/// Return the number of contact points in the manifold /// Return the number of contact points in the manifold
int getNbContactPoints() ; int getNbContactPoints() ;
/// Return the first friction vector at the center of the contact manifold /// 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 /// 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 /// 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 /// 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 /// Return the first friction accumulated impulse
float getFrictionImpulse1() ; float getFrictionImpulse1() ;
/// Set the first friction accumulated impulse /// Set the first friction accumulated impulse
@ -148,16 +140,13 @@ namespace ephysics {
/// Set the friction twist accumulated impulse /// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(float frictionTwistImpulse); void setFrictionTwistImpulse(float frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse /// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse( vec3 rollingResistanceImpulse); void setRollingResistanceImpulse( Vector3f rollingResistanceImpulse);
/// Return a contact point of the manifold /// Return a contact point of the manifold
ContactPoint* getContactPoint(int index) ; ContactPoint* getContactPoint(int index) ;
/// Return the normalized averaged normal vector /// Return the normalized averaged normal vector
vec3 getAverageContactNormal() ; Vector3f getAverageContactNormal() ;
/// Return the largest depth of all the contact points /// Return the largest depth of all the contact points
float getLargestContactDepth() ; float getLargestContactDepth();
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
}; };
} }

View File

@ -1,18 +1,8 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/ContactManifold.hpp>
using namespace ephysics;
ContactManifold::ContactManifold(ProxyShape* shape1, ContactManifold::ContactManifold(ProxyShape* shape1,
ProxyShape* shape2, ProxyShape* shape2,
short normalDirectionId): int normalDirectionId):
this.shape1(shape1), this.shape1(shape1),
this.shape2(shape2), this.shape2(shape2),
this.normalDirectionId(normalDirectionId), this.normalDirectionId(normalDirectionId),
@ -33,7 +23,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
for (int i=0; i<this.nbContactPoints; i++) { for (int i=0; i<this.nbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point // Check if the new point point does not correspond to a same contact point
// already in the manifold. // already in the manifold.
float distance = (this.contactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).length2(); float distance = (this.contactPoints[i].getWorldPointOnBody1() - contact.getWorldPointOnBody1()).length2();
if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) { if (distance <= PERSISTENTCONTACTDISTTHRESHOLD*PERSISTENTCONTACTDISTTHRESHOLD) {
// Delete the new contact // Delete the new contact
ETKDELETE(ContactPoint, contact); ETKDELETE(ContactPoint, contact);
@ -44,7 +34,7 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
// If the contact manifold is full // If the contact manifold is full
if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) { if (this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1());
removeContactPoint(indexToRemove); removeContactPoint(indexToRemove);
} }
// Add the new contact point in the manifold // Add the new contact point in the manifold
@ -67,30 +57,30 @@ void ContactManifold::removeContactPoint(int index) {
this.nbContactPoints--; this.nbContactPoints--;
} }
void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D transform2) { void ContactManifold::update( Transform3D transform1, Transform3D transform2) {
if (this.nbContactPoints == 0) { if (this.nbContactPoints == 0) {
return; return;
} }
// Update the world coordinates and penetration depth of the contact points in the manifold // Update the world coordinates and penetration depth of the contact points in the manifold
for (int i=0; i<this.nbContactPoints; i++) { for (int i=0; i<this.nbContactPoints; i++) {
this.contactPoints[i]->setWorldPointOnBody1(transform1 * this.contactPoints[i]->getLocalPointOnBody1()); this.contactPoints[i].setWorldPointOnBody1(transform1 * this.contactPoints[i].getLocalPointOnBody1());
this.contactPoints[i]->setWorldPointOnBody2(transform2 * this.contactPoints[i]->getLocalPointOnBody2()); 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].setPenetrationDepth((this.contactPoints[i].getWorldPointOnBody1() - this.contactPoints[i].getWorldPointOnBody2()).dot(this.contactPoints[i].getNormal()));
} }
float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD; float squarePersistentContactThreshold = PERSISTENTCONTACTDISTTHRESHOLD * PERSISTENTCONTACTDISTTHRESHOLD;
// Remove the contact points that don't represent very well the contact manifold // Remove the contact points that don't represent very well the contact manifold
for (int i=staticcast<int>(this.nbContactPoints)-1; i>=0; i--) { for (int i=staticcast<int>(this.nbContactPoints)-1; i>=0; i--) {
assert(i < staticcast<int>(this.nbContactPoints)); assert(i < staticcast<int>(this.nbContactPoints));
// Compute the distance between contact points in the normal direction // 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 the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) { if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i); removeContactPoint(i);
} else { } else {
// Compute the distance of the two contact points in the plane // Compute the distance of the two contact points in the plane
// orthogonal to the contact normal // orthogonal to the contact normal
vec3 projOfPoint1 = this.contactPoints[i]->getWorldPointOnBody1() + this.contactPoints[i]->getNormal() * distanceNormal; Vector3f projOfPoint1 = this.contactPoints[i].getWorldPointOnBody1() + this.contactPoints[i].getNormal() * distanceNormal;
vec3 projDifference = this.contactPoints[i]->getWorldPointOnBody2() - projOfPoint1; Vector3f projDifference = this.contactPoints[i].getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance // If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact // threshold, we remove the contact
if (projDifference.length2() > squarePersistentContactThreshold) { if (projDifference.length2() > squarePersistentContactThreshold) {
@ -103,12 +93,12 @@ void ContactManifold::update( etk::Transform3D transform1, etk::Transform3D tra
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) { int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) {
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD); assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
int indexMaxPenetrationDepth = -1; int indexMaxPenetrationDepth = -1;
float maxPenetrationDepth = newContact->getPenetrationDepth(); float maxPenetrationDepth = newContact.getPenetrationDepth();
// For each contact in the cache // For each contact in the cache
for (int i=0; i<this.nbContactPoints; i++) { for (int i=0; i<this.nbContactPoints; i++) {
// If the current contact has a larger penetration depth // If the current contact has a larger penetration depth
if (this.contactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { if (this.contactPoints[i].getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = this.contactPoints[i]->getPenetrationDepth(); maxPenetrationDepth = this.contactPoints[i].getPenetrationDepth();
indexMaxPenetrationDepth = i; indexMaxPenetrationDepth = i;
} }
} }
@ -116,7 +106,7 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) {
return indexMaxPenetrationDepth; return indexMaxPenetrationDepth;
} }
int ContactManifold::getIndexToRemove(int indexMaxPenetration, vec3 newPoint) { int ContactManifold::getIndexToRemove(int indexMaxPenetration, Vector3f newPoint) {
assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD); assert(this.nbContactPoints == MAXCONTACTPOINTSINMANIFOLD);
float area0 = 0.0f; // Area with contact 1,2,3 and newPoint float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
float area1 = 0.0f; // Area with contact 0,2,3 and newPoint float 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 float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) { if (indexMaxPenetration != 0) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - this.contactPoints[1]->getLocalPointOnBody1(); Vector3f vector1 = newPoint - this.contactPoints[1].getLocalPointOnBody1();
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1(); Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); Vector3f crossProduct = vector1.cross(vector2);
area0 = crossProduct.length2(); area0 = crossProduct.length2();
} }
if (indexMaxPenetration != 1) { if (indexMaxPenetration != 1) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[2]->getLocalPointOnBody1(); Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[2].getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); Vector3f crossProduct = vector1.cross(vector2);
area1 = crossProduct.length2(); area1 = crossProduct.length2();
} }
if (indexMaxPenetration != 2) { if (indexMaxPenetration != 2) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
vec3 vector2 = this.contactPoints[3]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1(); Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); Vector3f crossProduct = vector1.cross(vector2);
area2 = crossProduct.length2(); area2 = crossProduct.length2();
} }
if (indexMaxPenetration != 3) { if (indexMaxPenetration != 3) {
// Compute the area // Compute the area
vec3 vector1 = newPoint - this.contactPoints[0]->getLocalPointOnBody1(); Vector3f vector1 = newPoint - this.contactPoints[0].getLocalPointOnBody1();
vec3 vector2 = this.contactPoints[2]->getLocalPointOnBody1() - this.contactPoints[1]->getLocalPointOnBody1(); Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1() - this.contactPoints[1].getLocalPointOnBody1();
vec3 crossProduct = vector1.cross(vector2); Vector3f crossProduct = vector1.cross(vector2);
area3 = crossProduct.length2(); area3 = crossProduct.length2();
} }
// Return the index of the contact to remove // 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 // Return a pointer to the first body of the contact manifold
CollisionBody* ContactManifold::getBody1() { CollisionBody* ContactManifold::getBody1() {
return this.shape1->getBody(); return this.shape1.getBody();
} }
// Return a pointer to the second body of the contact manifold // Return a pointer to the second body of the contact manifold
CollisionBody* ContactManifold::getBody2() { CollisionBody* ContactManifold::getBody2() {
return this.shape2->getBody(); return this.shape2.getBody();
} }
// Return the normal direction Id // Return the normal direction Id
@ -223,22 +213,22 @@ int ContactManifold::getNbContactPoints() {
} }
// Return the first friction vector at the center of the contact manifold // Return the first friction vector at the center of the contact manifold
vec3 ContactManifold::getFrictionVector1() { Vector3f ContactManifold::getFrictionVector1() {
return this.frictionVector1; return this.frictionVector1;
} }
// set the first friction vector at the center of the contact manifold // 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; this.frictionVector1 = frictionVector1;
} }
// Return the second friction vector at the center of the contact manifold // Return the second friction vector at the center of the contact manifold
vec3 ContactManifold::getFrictionvec2() { Vector3f ContactManifold::getFrictionvec2() {
return this.frictionvec2; return this.frictionvec2;
} }
// set the second friction vector at the center of the contact manifold // 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; this.frictionvec2 = frictionvec2;
} }
@ -273,7 +263,7 @@ void ContactManifold::setFrictionTwistImpulse(float frictionTwistImpulse) {
} }
// Set the accumulated rolling resistance impulse // Set the accumulated rolling resistance impulse
void ContactManifold::setRollingResistanceImpulse( vec3 rollingResistanceImpulse) { void ContactManifold::setRollingResistanceImpulse( Vector3f rollingResistanceImpulse) {
this.rollingResistanceImpulse = rollingResistanceImpulse; this.rollingResistanceImpulse = rollingResistanceImpulse;
} }
@ -289,10 +279,10 @@ boolean ContactManifold::isAlreadyInIsland() {
} }
// Return the normalized averaged normal vector // Return the normalized averaged normal vector
vec3 ContactManifold::getAverageContactNormal() { Vector3f ContactManifold::getAverageContactNormal() {
vec3 averageNormal; Vector3f averageNormal;
for (int i=0; i<this.nbContactPoints; i++) { for (int i=0; i<this.nbContactPoints; i++) {
averageNormal += this.contactPoints[i]->getNormal(); averageNormal += this.contactPoints[i].getNormal();
} }
return averageNormal.safeNormalized(); return averageNormal.safeNormalized();
} }
@ -301,7 +291,7 @@ vec3 ContactManifold::getAverageContactNormal() {
float ContactManifold::getLargestContactDepth() { float ContactManifold::getLargestContactDepth() {
float largestDepth = 0.0f; float largestDepth = 0.0f;
for (int i=0; i<this.nbContactPoints; i++) { for (int i=0; i<this.nbContactPoints; i++) {
float depth = this.contactPoints[i]->getPenetrationDepth(); float depth = this.contactPoints[i].getPenetrationDepth();
if (depth > largestDepth) { if (depth > largestDepth) {
largestDepth = depth; largestDepth = depth;
} }

View File

@ -26,14 +26,14 @@ ContactManifoldSet::~ContactManifoldSet() {
void ContactManifoldSet::addContactPoint(ContactPoint* contact) { void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap) // 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 there is no contact manifold yet
if (this.nbManifolds == 0) { if (this.nbManifolds == 0) {
createManifold(normalDirectionId); createManifold(normalDirectionId);
this.manifolds[0]->addContactPoint(contact); this.manifolds[0].addContactPoint(contact);
assert(this.manifolds[this.nbManifolds-1]->getNbContactPoints() > 0); assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
assert(this.manifolds[i]->getNbContactPoints() > 0); assert(this.manifolds[i].getNbContactPoints() > 0);
} }
return; return;
} }
@ -45,17 +45,17 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// If a similar manifold has been found // If a similar manifold has been found
if (similarManifoldIndex != -1) { if (similarManifoldIndex != -1) {
// Add the contact point to that similar manifold // Add the contact point to that similar manifold
this.manifolds[similarManifoldIndex]->addContactPoint(contact); this.manifolds[similarManifoldIndex].addContactPoint(contact);
assert(this.manifolds[similarManifoldIndex]->getNbContactPoints() > 0); assert(this.manifolds[similarManifoldIndex].getNbContactPoints() > 0);
return; return;
} }
// If the maximum number of manifold has not been reached yet // If the maximum number of manifold has not been reached yet
if (this.nbManifolds < this.nbMaxManifolds) { if (this.nbManifolds < this.nbMaxManifolds) {
// Create a new manifold for the contact point // Create a new manifold for the contact point
createManifold(normalDirectionId); createManifold(normalDirectionId);
this.manifolds[this.nbManifolds-1]->addContactPoint(contact); this.manifolds[this.nbManifolds-1].addContactPoint(contact);
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
assert(this.manifolds[i]->getNbContactPoints() > 0); assert(this.manifolds[i].getNbContactPoints() > 0);
} }
return; return;
} }
@ -63,10 +63,10 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// manifolds condidates. We need to remove one. We choose to keep the manifolds // manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points // with the largest contact depth among their points
int smallestDepthIndex = -1; int smallestDepthIndex = -1;
float minDepth = contact->getPenetrationDepth(); float minDepth = contact.getPenetrationDepth();
assert(this.nbManifolds == this.nbMaxManifolds); assert(this.nbManifolds == this.nbMaxManifolds);
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
float depth = this.manifolds[i]->getLargestContactDepth(); float depth = this.manifolds[i].getLargestContactDepth();
if (depth < minDepth) { if (depth < minDepth) {
minDepth = depth; minDepth = depth;
smallestDepthIndex = i; smallestDepthIndex = i;
@ -80,15 +80,15 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
contact = null; contact = null;
return; 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 // Here we need to replace an existing manifold with a new one (that contains
// the new contact point) // the new contact point)
removeManifold(smallestDepthIndex); removeManifold(smallestDepthIndex);
createManifold(normalDirectionId); createManifold(normalDirectionId);
this.manifolds[this.nbManifolds-1]->addContactPoint(contact); this.manifolds[this.nbManifolds-1].addContactPoint(contact);
assert(this.manifolds[this.nbManifolds-1]->getNbContactPoints() > 0); assert(this.manifolds[this.nbManifolds-1].getNbContactPoints() > 0);
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
assert(this.manifolds[i]->getNbContactPoints() > 0); assert(this.manifolds[i].getNbContactPoints() > 0);
} }
return; return;
} }
@ -96,24 +96,24 @@ void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) { int ContactManifoldSet::selectManifoldWithSimilarNormal(int16t normalDirectionId) {
// Return the Id of the manifold with the same normal direction id (if exists) // Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
if (normalDirectionId == this.manifolds[i]->getNormalDirectionId()) { if (normalDirectionId == this.manifolds[i].getNormalDirectionId()) {
return i; return i;
} }
} }
return -1; return -1;
} }
int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) { int16t ContactManifoldSet::computeCubemapNormalId( Vector3f normal) {
assert(normal.length2() > FLTEPSILON); assert(normal.length2() > FLTEPSILON);
int faceNo; int faceNo;
float u, v; float u, v;
float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z())); float max = max3(fabs(normal.x()), fabs(normal.y()), fabs(normal.z()));
vec3 normalScaled = normal / max; Vector3f normalScaled = normal / max;
if (normalScaled.x() >= normalScaled.y() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj normalScaled.x() >= normalScaled.z()) { if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
faceNo = normalScaled.x() > 0 ? 0 : 1; faceNo = normalScaled.x() > 0 ? 0 : 1;
u = normalScaled.y(); u = normalScaled.y();
v = normalScaled.z(); 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; faceNo = normalScaled.y() > 0 ? 2 : 3;
u = normalScaled.x(); u = normalScaled.x();
v = normalScaled.z(); v = normalScaled.z();
@ -137,10 +137,10 @@ int16t ContactManifoldSet::computeCubemapNormalId( vec3 normal) {
void ContactManifoldSet::update() { void ContactManifoldSet::update() {
for (int i=this.nbManifolds-1; i>=0; i--) { for (int i=this.nbManifolds-1; i>=0; i--) {
// Update the contact manifold // Update the contact manifold
this.manifolds[i]->update(this.shape1->getBody()->getTransform() * this.shape1->getLocalToBodyTransform(), this.manifolds[i].update(this.shape1.getBody().getTransform() * this.shape1.getLocalToBodyTransform(),
this.shape2->getBody()->getTransform() * this.shape2->getLocalToBodyTransform()); this.shape2.getBody().getTransform() * this.shape2.getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore // Remove the contact manifold if has no contact points anymore
if (this.manifolds[i]->getNbContactPoints() == 0) { if (this.manifolds[i].getNbContactPoints() == 0) {
removeManifold(i); removeManifold(i);
} }
} }
@ -161,7 +161,7 @@ void ContactManifoldSet::createManifold(int16t normalDirectionId) {
void ContactManifoldSet::removeManifold(int index) { void ContactManifoldSet::removeManifold(int index) {
assert(this.nbManifolds > 0); assert(this.nbManifolds > 0);
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds); assert(index >= 0 && index < this.nbManifolds);
// Delete the new contact // Delete the new contact
ETKDELETE(ContactManifold, this.manifolds[index]); ETKDELETE(ContactManifold, this.manifolds[index]);
this.manifolds[index] = null; this.manifolds[index] = null;
@ -184,14 +184,14 @@ int ContactManifoldSet::getNbContactManifolds() {
} }
ContactManifold* ContactManifoldSet::getContactManifold(int index) { ContactManifold* ContactManifoldSet::getContactManifold(int index) {
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < this.nbManifolds); assert(index >= 0 && index < this.nbManifolds);
return this.manifolds[index]; return this.manifolds[index];
} }
int ContactManifoldSet::getTotalNbContactPoints() { int ContactManifoldSet::getTotalNbContactPoints() {
int nbPoints = 0; int nbPoints = 0;
for (int i=0; i<this.nbManifolds; i++) { for (int i=0; i<this.nbManifolds; i++) {
nbPoints += this.manifolds[i]->getNbContactPoints(); nbPoints += this.manifolds[i].getNbContactPoints();
} }
return nbPoints; return nbPoints;
} }

View File

@ -1,16 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/ContactManifold.hpp>
namespace ephysics {
int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3; // Maximum number of contact manifolds in the set 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 int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
/** /**
@ -21,13 +10,13 @@ namespace ephysics {
*/ */
class ContactManifoldSet { class ContactManifoldSet {
private: private:
int this.nbMaxManifolds; //!< Maximum number of contact manifolds in the set int nbMaxManifolds; //!< Maximum number of contact manifolds in the set
int this.nbManifolds; //!< Current number of contact manifolds in the set int nbManifolds; //!< Current number of contact manifolds in the set
ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
ContactManifold* this.manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set ContactManifold* manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
/// Create a new contact manifold and add it to 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 /// Remove a contact manifold from the set
void removeManifold(int index); void removeManifold(int index);
// Return the index of the contact manifold with a similar average normal. // 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) // 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 // 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 // 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: public:
/// Constructor /// Constructor
ContactManifoldSet(ProxyShape* shape1, ContactManifoldSet(ProxyShape* shape1,
ProxyShape* shape2, ProxyShape* shape2,
int nbMaxManifolds); int nbMaxManifolds);
/// Destructor
~ContactManifoldSet();
/// Return the first proxy shape /// Return the first proxy shape
ProxyShape* getShape1() ; ProxyShape* getShape1() ;
/// Return the second proxy shape /// Return the second proxy shape

View File

@ -14,10 +14,10 @@ using namespace ephysics;
/** /**
* @param body Pointer to the parent body * @param body Pointer to the parent body
* @param shape Pointer to the collision shape * @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) * @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.body(body), this.collisionShape(shape), this.localToBodyTransform(transform), this.mass(mass),
this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL), this.next(NULL), this.broadPhaseID(-1), this.cachedCollisionData(NULL), this.userData(NULL),
this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) { this.collisionCategoryBits(0x0001), this.collideWithMaskBits(0xFFFF) {
@ -37,10 +37,10 @@ ProxyShape::~ProxyShape() {
* @param worldPoint Point to test in world-space coordinates * @param worldPoint Point to test in world-space coordinates
* @return True if the point is inside the collision shape * @return True if the point is inside the collision shape
*/ */
boolean ProxyShape::testPointInside( vec3 worldPoint) { boolean ProxyShape::testPointInside( Vector3f worldPoint) {
etk::Transform3D localToWorld = this.body->getTransform() * this.localToBodyTransform; Transform3D localToWorld = this.body.getTransform() * this.localToBodyTransform;
vec3 localPoint = localToWorld.getInverse() * worldPoint; Vector3f localPoint = localToWorld.getInverse() * worldPoint;
return this.collisionShape->testPointInside(localPoint, this); return this.collisionShape.testPointInside(localPoint, this);
} }
// Raycast method with feedback information // Raycast method with feedback information
@ -53,16 +53,16 @@ boolean ProxyShape::testPointInside( vec3 worldPoint) {
boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) { boolean ProxyShape::raycast( Ray ray, RaycastInfo raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays // 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 // Convert the ray into the local-space of the collision shape
etk::Transform3D localToWorldTransform = getLocalToWorldTransform(); Transform3D localToWorldTransform = getLocalToWorldTransform();
etk::Transform3D worldToLocalTransform = localToWorldTransform.getInverse(); Transform3D worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1, Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2, worldToLocalTransform * ray.point2,
ray.maxFraction); ray.maxFraction);
boolean isHit = this.collisionShape->raycast(rayLocal, raycastInfo, this); boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this);
if (isHit == true) { if (isHit == true) {
// Convert the raycast info into world-space // Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint; 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 * @return The transformation that transforms the local-space of the collision shape
* to the local-space of the parent body * to the local-space of the parent body
*/ */
etk::Transform3D ProxyShape::getLocalToBodyTransform() { Transform3D ProxyShape::getLocalToBodyTransform() {
return this.localToBodyTransform; return this.localToBodyTransform;
} }
// Set the local to parent body transform // Set the local to parent body transform
void ProxyShape::setLocalToBodyTransform( etk::Transform3D transform) { void ProxyShape::setLocalToBodyTransform( Transform3D transform) {
this.localToBodyTransform = 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 // 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 // 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 * @return The transformation that transforms the local-space of the collision
* shape to the world-space * shape to the world-space
*/ */
etk::Transform3D ProxyShape::getLocalToWorldTransform() { Transform3D ProxyShape::getLocalToWorldTransform() {
return this.body->this.transform * this.localToBodyTransform; return this.body.this.transform * this.localToBodyTransform;
} }
// Return the next proxy shape in the linked list of proxy shapes // 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 * @return The collision category bits mask of the proxy shape
*/ */
unsigned short ProxyShape::getCollisionCategoryBits() { int ProxyShape::getCollisionCategoryBits() {
return this.collisionCategoryBits; return this.collisionCategoryBits;
} }
@ -174,7 +174,7 @@ unsigned short ProxyShape::getCollisionCategoryBits() {
/** /**
* @param collisionCategoryBits The collision category bits mask of the proxy shape * @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; 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 * @return The bits mask that specifies with which collision category this shape will collide
*/ */
unsigned short ProxyShape::getCollideWithMaskBits() { int ProxyShape::getCollideWithMaskBits() {
return this.collideWithMaskBits; 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 * @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; this.collideWithMaskBits = collideWithMaskBits;
} }
@ -198,21 +198,21 @@ void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBits) {
/** /**
* @return The local scaling vector * @return The local scaling vector
*/ */
vec3 ProxyShape::getLocalScaling() { Vector3f ProxyShape::getLocalScaling() {
return this.collisionShape->getScaling(); return this.collisionShape.getScaling();
} }
// Set the local scaling vector of the collision shape // Set the local scaling vector of the collision shape
/** /**
* @param scaling The new local scaling vector * @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 // 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 // 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);
} }

View File

@ -1,17 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/CollisionBody.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
namespace ephysics {
/** /**
* @breif The CollisionShape instances are supposed to be unique for memory optimization. For instance, * @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 * consider two rigid bodies with the same sphere collision shape. In this situation, we will have
@ -23,14 +11,14 @@ namespace ephysics {
*/ */
class ProxyShape { class ProxyShape {
protected: protected:
CollisionBody* this.body; //!< Pointer to the parent body CollisionBody* body; //!< Pointer to the parent body
CollisionShape* this.collisionShape; //!< Internal collision shape CollisionShape* collisionShape; //!< Internal collision shape
etk::Transform3D this.localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time) Transform3D localToBodyTransform; //!< Local-space to parent body-space transform (does not change over time)
float this.mass; //!< Mass (in kilogramms) of the corresponding collision shape float mass; //!< Mass (in kilogramms) of the corresponding collision shape
ProxyShape* this.next; //!< Pointer to the next proxy shape of the body (linked list) ProxyShape* 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) int broadPhaseID; //!< Broad-phase ID (node ID in the dynamic AABB tree)
void* this.cachedCollisionData; //!< Cached collision data void* cachedCollisionData; //!< Cached collision data
void* this.userData; //!< Pointer to user data void* userData; //!< Pointer to user data
/** /**
* @brief Bits used to define the collision category of this shape. * @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 * 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 * categories of shapes collide with each other and do not collide with
* other categories. * other categories.
*/ */
unsigned short this.collisionCategoryBits; int collisionCategoryBits;
/** /**
* @brief Bits mask used to state which collision categories this shape can * @brief Bits mask used to state which collision categories this shape can
* collide with. This value is 0xFFFF by default. It means that this * collide with. This value is 0xFFFF by default. It means that this
* proxy shape will collide with every collision categories by default. * proxy shape will collide with every collision categories by default.
*/ */
unsigned short this.collideWithMaskBits; int collideWithMaskBits;
/// Private copy-ructor
ProxyShape( ProxyShape) = delete;
/// Private assignment operator
ProxyShape operator=( ProxyShape) = delete;
public: public:
/// Constructor /// Constructor
ProxyShape(CollisionBody* body, ProxyShape(CollisionBody* body,
CollisionShape* shape, CollisionShape* shape,
etk::Transform3D transform, Transform3D transform,
float mass); float mass);
/// Destructor
virtual ~ProxyShape();
/// Return the collision shape /// Return the collision shape
CollisionShape* getCollisionShape() ; CollisionShape* getCollisionShape() ;
/// Return the parent body /// Return the parent body
CollisionBody* getBody() ; CollisionBody* getBody() ;
@ -76,31 +56,31 @@ namespace ephysics {
void setUserData(void* userData); void setUserData(void* userData);
/// Return the local to parent body transform /// Return the local to parent body transform
etk::Transform3D getLocalToBodyTransform() ; Transform3D getLocalToBodyTransform() ;
/// Set the local to parent body transform /// Set the local to parent body transform
void setLocalToBodyTransform( etk::Transform3D transform); void setLocalToBodyTransform( Transform3D transform);
/// Return the local to world transform /// Return the local to world transform
etk::Transform3D getLocalToWorldTransform() ; Transform3D getLocalToWorldTransform() ;
/// Return true if a point is inside the collision shape /// Return true if a point is inside the collision shape
boolean testPointInside( vec3 worldPoint); boolean testPointInside( Vector3f worldPoint);
/// Raycast method with feedback information /// Raycast method with feedback information
boolean raycast( Ray ray, RaycastInfo raycastInfo); boolean raycast( Ray ray, RaycastInfo raycastInfo);
/// Return the collision bits mask /// Return the collision bits mask
unsigned short getCollideWithMaskBits() ; int getCollideWithMaskBits() ;
/// Set the collision bits mask /// Set the collision bits mask
void setCollideWithMaskBits(unsigned short collideWithMaskBits); void setCollideWithMaskBits(int collideWithMaskBits);
/// Return the collision category bits /// Return the collision category bits
unsigned short getCollisionCategoryBits() ; int getCollisionCategoryBits() ;
/// Set the collision category bits /// 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 /// Return the next proxy shape in the linked list of proxy shapes
ProxyShape* getNext(); ProxyShape* getNext();
@ -112,10 +92,10 @@ namespace ephysics {
void** getCachedCollisionData(); void** getCachedCollisionData();
/// Return the local scaling vector of the collision shape /// Return the local scaling vector of the collision shape
vec3 getLocalScaling() ; Vector3f getLocalScaling() ;
/// Set the local scaling vector of the collision shape /// Set the local scaling vector of the collision shape
virtual void setLocalScaling( vec3 scaling); void setLocalScaling( Vector3f scaling);
friend class OverlappingPair; friend class OverlappingPair;
friend class CollisionBody; friend class CollisionBody;

View File

@ -16,14 +16,14 @@ float RaycastTest::raycastAgainstShape(ProxyShape* shape, Ray ray) {
// Ray casting test against the collision shape // Ray casting test against the collision shape
RaycastInfo raycastInfo; RaycastInfo raycastInfo;
boolean isHit = shape->raycast(ray, raycastInfo); boolean isHit = shape.raycast(ray, raycastInfo);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isHit) { if (isHit) {
// Report the hit to the user and return the // Report the hit to the user and return the
// user hit fraction value // user hit fraction value
return userCallback->notifyRaycastHit(raycastInfo); return userCallback.notifyRaycastHit(raycastInfo);
} }
return ray.maxFraction; return ray.maxFraction;

View File

@ -1,20 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <etk/math/Vector3D.hpp>
#include <ephysics/mathematics/Ray.hpp>
namespace ephysics {
class CollisionBody;
class ProxyShape;
class CollisionShape;
/** /**
* @brief It contains the information about a raycast hit. * @brief It contains the information about a raycast hit.
*/ */
@ -25,8 +10,8 @@ namespace ephysics {
/// Private assignment operator /// Private assignment operator
RaycastInfo operator=( RaycastInfo) = delete; RaycastInfo operator=( RaycastInfo) = delete;
public: public:
vec3 worldPoint; //!< Hit point in world-space coordinates Vector3f worldPoint; //!< Hit point in world-space coordinates
vec3 worldNormal; //!< Surface normal at 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) 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 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) int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
@ -40,9 +25,6 @@ namespace ephysics {
proxyShape(null) { proxyShape(null) {
} }
/// Destructor
virtual ~RaycastInfo() = default;
}; };
/** /**
@ -53,8 +35,6 @@ namespace ephysics {
*/ */
class RaycastCallback { class RaycastCallback {
public: public:
/// Destructor
virtual ~RaycastCallback() = default;
/** /**
* @brief This method will be called for each ProxyShape that is hit by the * @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 * 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 * @param[in] raycastInfo Information about the raycast hit
* @return Value that controls the continuation of the ray after a 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 { struct RaycastTest {

View File

@ -1,16 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <etk/Vector.hpp>
#include <ephysics/collision/TriangleVertexArray.hpp>
namespace ephysics {
/** /**
* @brief Represents a mesh made of triangles. A TriangleMesh contains * @brief Represents a mesh made of triangles. A TriangleMesh contains
* one or several parts. Each part is a set of triangles represented in a * one or several parts. Each part is a set of triangles represented in a
@ -20,16 +9,12 @@ namespace ephysics {
*/ */
class TriangleMesh { class TriangleMesh {
protected: protected:
etk::Vector<TriangleVertexArray*> this.triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part) Vector<TriangleVertexArray*> triangleArrays; //!< All the triangle arrays of the mesh (one triangle array per part)
public: public:
/** /**
* @brief Constructor * @brief Constructor
*/ */
TriangleMesh(); TriangleMesh();
/**
* @brief Virtualisation of Destructor
*/
virtual ~TriangleMesh() = default;
/** /**
* @brief Add a subpart of the mesh * @brief Add a subpart of the mesh
*/ */

View File

@ -9,25 +9,25 @@
#include <ephysics/collision/TriangleVertexArray.hpp> #include <ephysics/collision/TriangleVertexArray.hpp>
ephysics::TriangleVertexArray::TriangleVertexArray( etk::Vector<vec3> vertices, etk::Vector<int> triangles): ephysics::TriangleVertexArray::TriangleVertexArray( Vector<Vector3f> vertices, Vector<int> triangles):
this.vertices(vertices), this.vertices(vertices),
this.triangles(triangles) { this.triangles(triangles) {
} }
sizet ephysics::TriangleVertexArray::getNbVertices() { long ephysics::TriangleVertexArray::getNbVertices() {
return this.vertices.size(); return this.vertices.size();
} }
sizet ephysics::TriangleVertexArray::getNbTriangles() { long ephysics::TriangleVertexArray::getNbTriangles() {
return this.triangles.size()/3; return this.triangles.size()/3;
} }
etk::Vector<vec3> ephysics::TriangleVertexArray::getVertices() { Vector<Vector3f> ephysics::TriangleVertexArray::getVertices() {
return this.vertices; return this.vertices;
} }
etk::Vector<int> ephysics::TriangleVertexArray::getIndices() { Vector<int> ephysics::TriangleVertexArray::getIndices() {
return this.triangles; return this.triangles;
} }

View File

@ -1,21 +1,9 @@
/** @file package org.atriaSoft.ephysics.collision;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/configuration.hpp>
#include <etk/math/Vector3D.hpp>
namespace ephysics {
class Triangle { class Triangle {
public: public:
vec3 value[3]; Vector3f value[3];
vec3 operator[] (sizet id) { Vector3f operator[] (long id) {
return value[id]; return value[id];
} }
}; };
@ -30,36 +18,36 @@ namespace ephysics {
*/ */
class TriangleVertexArray { class TriangleVertexArray {
protected: protected:
etk::Vector<vec3> this.vertices; //!< Vertice list Vector<Vector3f> vertices; //!< Vertice list
etk::Vector<int> this.triangles; //!< List of triangle (3 pos for each triangle) Vector<int> triangles; //!< List of triangle (3 pos for each triangle)
public: public:
/** /**
* @brief Constructor * @brief Constructor
* @param[in] vertices List Of all vertices * @param[in] vertices List Of all vertices
* @param[in] triangles List of all linked points * @param[in] triangles List of all linked points
*/ */
TriangleVertexArray( etk::Vector<vec3> vertices, TriangleVertexArray( Vector<Vector3f> vertices,
etk::Vector<int> triangles); Vector<int> triangles);
/** /**
* @brief Get the number of vertices * @brief Get the number of vertices
* @return Number of vertices * @return Number of vertices
*/ */
sizet getNbVertices() ; long getNbVertices() ;
/** /**
* @brief Get the number of triangle * @brief Get the number of triangle
* @return Number of triangles * @return Number of triangles
*/ */
sizet getNbTriangles() ; long getNbTriangles() ;
/** /**
* @brief Get The table of the vertices * @brief Get The table of the vertices
* @return reference on the vertices * @return reference on the vertices
*/ */
etk::Vector<vec3> getVertices() ; Vector<Vector3f> getVertices() ;
/** /**
* @brief Get The table of the triangle indice * @brief Get The table of the triangle indice
* @return reference on the triangle indice * @return reference on the triangle indice
*/ */
etk::Vector<int> getIndices() ; Vector<int> getIndices() ;
/** /**
* @brief Get a triangle at the specific ID * @brief Get a triangle at the specific ID
* @return Buffer of 3 points * @return Buffer of 3 points

View File

@ -41,7 +41,7 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
// If less than the quarter of allocated elements of the non-static shapes IDs array // If less than the quarter of allocated elements of the non-static shapes IDs array
// are used, we release some allocated memory // 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 > 8) {
this.numberAllocatedMovedShapes /= 2; 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 // Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape); int nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape // 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) // Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step // during the last simulation step
addMovedCollisionShape(proxyShape->this.broadPhaseID); addMovedCollisionShape(proxyShape.this.broadPhaseID);
} }
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->this.broadPhaseID; int broadPhaseID = proxyShape.this.broadPhaseID;
// Remove the collision shape from the dynamic AABB tree // Remove the collision shape from the dynamic AABB tree
this.dynamicAABBTree.removeObject(broadPhaseID); this.dynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape into the array of shapes that have moved (or have been created) // 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, void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
AABB aabb, AABB aabb,
vec3 displacement, Vector3f displacement,
boolean forceReinsert) { boolean forceReinsert) {
int broadPhaseID = proxyShape->this.broadPhaseID; int broadPhaseID = proxyShape.this.broadPhaseID;
assert(broadPhaseID >= 0); assert(broadPhaseID >= 0);
// Update the dynamic AABB tree according to the movement of the collision shape // Update the dynamic AABB tree according to the movement of the collision shape
boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert); boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
@ -107,7 +107,7 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape,
} }
} }
static boolean sortFunction( etk::Pair<int,int> pair1, etk::Pair<int,int> pair2) { static boolean sortFunction( Pair<int,int> pair1, Pair<int,int> pair2) {
if (pair1.first < pair2.first) { if (pair1.first < pair2.first) {
return true; return true;
} }
@ -137,19 +137,19 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
return; return;
} }
// Add the new potential pair into the array of potential overlapping pairs // 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 // Reset the array of collision shapes that have move (or have been created) during the last simulation step
this.movedShapes.clear(); this.movedShapes.clear();
// Sort the array of potential overlapping pairs in order to remove duplicate pairs // Sort the array of potential overlapping pairs in order to remove duplicate pairs
etk::algorithm::quickSort(this.potentialPairs, sortFunction); algorithm::quickSort(this.potentialPairs, sortFunction);
// Check all the potential overlapping pairs avoiding duplicates to report unique // Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs // overlapping pairs
int iii=0; int iii=0;
while (iii < this.potentialPairs.size()) { while (iii < this.potentialPairs.size()) {
// Get a potential overlapping pair // Get a potential overlapping pair
etk::Pair<int,int> pair = (this.potentialPairs[iii]); Pair<int,int> pair = (this.potentialPairs[iii]);
++iii; ++iii;
// Get the two collision shapes of the pair // Get the two collision shapes of the pair
ProxyShape* shape1 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.first)); ProxyShape* shape1 = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(pair.first));
@ -159,7 +159,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Skip the duplicate overlapping pairs // Skip the duplicate overlapping pairs
while (iii < this.potentialPairs.size()) { while (iii < this.potentialPairs.size()) {
// Get the next pair // Get the next pair
etk::Pair<int,int> nextPair = this.potentialPairs[iii]; Pair<int,int> nextPair = this.potentialPairs[iii];
// If the next pair is different from the previous one, we stop skipping pairs // If the next pair is different from the previous one, we stop skipping pairs
if ( nextPair.first != pair.first if ( nextPair.first != pair.first
|| nextPair.second != pair.second) { || nextPair.second != pair.second) {
@ -175,7 +175,7 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) {
// Get the proxy shape from the node // Get the proxy shape from the node
ProxyShape* proxyShape = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(nodeId)); ProxyShape* proxyShape = staticcast<ProxyShape*>(this.dynamicAABBTree.getNodeDataPointer(nodeId));
// Check if the raycast filtering mask allows raycast against this shape // 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 // Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping // the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase // with the shape in the broad-phase
@ -187,15 +187,15 @@ float BroadPhaseRaycastCallback::operator()(int nodeId, Ray ray) {
boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1, boolean BroadPhaseAlgorithm::testOverlappingShapes( ProxyShape* shape1,
ProxyShape* shape2) { ProxyShape* shape2) {
// Get the two AABBs of the collision shapes // Get the two AABBs of the collision shapes
AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1->this.broadPhaseID); AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.this.broadPhaseID);
AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2->this.broadPhaseID); AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2.this.broadPhaseID);
// Check if the two AABBs are overlapping // Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2); return aabb1.testCollision(aabb2);
} }
void BroadPhaseAlgorithm::raycast( Ray ray, void BroadPhaseAlgorithm::raycast( Ray ray,
RaycastTest raycastTest, RaycastTest raycastTest,
unsigned short raycastWithCategoryMaskBits) { int raycastWithCategoryMaskBits) {
PROFILE("BroadPhaseAlgorithm::raycast()"); PROFILE("BroadPhaseAlgorithm::raycast()");
BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest); BroadPhaseRaycastCallback broadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback); this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);

View File

@ -1,23 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.broadphase;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <etk/Vector.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/collision/ProxyShape.hpp>
#include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
#include <ephysics/engine/Profiler.hpp>
namespace ephysics {
class CollisionDetection;
class BroadPhaseAlgorithm;
// TODO : remove this as callback ... DynamicAABBTreeOverlapCallback { // TODO : remove this as callback ... DynamicAABBTreeOverlapCallback {
/** /**
@ -27,12 +9,12 @@ namespace ephysics {
class BroadPhaseRaycastCallback { class BroadPhaseRaycastCallback {
private : private :
DynamicAABBTree this.dynamicAABBTree; DynamicAABBTree this.dynamicAABBTree;
unsigned short this.raycastWithCategoryMaskBits; int this.raycastWithCategoryMaskBits;
RaycastTest this.raycastTest; RaycastTest this.raycastTest;
public: public:
// Constructor // Constructor
BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree, BroadPhaseRaycastCallback( DynamicAABBTree dynamicAABBTree,
unsigned short raycastWithCategoryMaskBits, int raycastWithCategoryMaskBits,
RaycastTest raycastTest): RaycastTest raycastTest):
this.dynamicAABBTree(dynamicAABBTree), this.dynamicAABBTree(dynamicAABBTree),
this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits), this.raycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
@ -53,8 +35,8 @@ namespace ephysics {
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
protected : protected :
DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree DynamicAABBTree this.dynamicAABBTree; //!< Dynamic AABB tree
etk::Vector<int> 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<int> 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<etk::Pair<int,int>> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates) Vector<Pair<int,int>> this.potentialPairs; //!< Temporary array of potential overlapping pairs (with potential duplicates)
CollisionDetection this.collisionDetection; //!< Reference to the collision detection object CollisionDetection this.collisionDetection; //!< Reference to the collision detection object
/// Private copy-ructor /// Private copy-ructor
BroadPhaseAlgorithm( BroadPhaseAlgorithm obj); BroadPhaseAlgorithm( BroadPhaseAlgorithm obj);
@ -64,7 +46,7 @@ namespace ephysics {
/// Constructor /// Constructor
BroadPhaseAlgorithm(CollisionDetection collisionDetection); BroadPhaseAlgorithm(CollisionDetection collisionDetection);
/// Destructor /// Destructor
virtual ~BroadPhaseAlgorithm(); ~BroadPhaseAlgorithm();
/// Add a proxy collision shape into the broad-phase collision detection /// Add a proxy collision shape into the broad-phase collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, AABB aabb); void addProxyCollisionShape(ProxyShape* proxyShape, AABB aabb);
/// Remove a proxy collision shape from the broad-phase collision detection /// 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 /// Notify the broad-phase that a collision shape has moved and need to be updated
void updateProxyCollisionShape(ProxyShape* proxyShape, void updateProxyCollisionShape(ProxyShape* proxyShape,
AABB aabb, AABB aabb,
vec3 displacement, Vector3f displacement,
boolean forceReinsert = false); boolean forceReinsert = false);
/// Add a collision shape in the array of shapes that have moved in the last simulation step /// Add a collision shape in the array of shapes that have moved in the last simulation step
/// and that need to be tested again for broad-phase overlapping. /// and that need to be tested again for broad-phase overlapping.
@ -87,7 +69,7 @@ namespace ephysics {
/// Ray casting method /// Ray casting method
void raycast( Ray ray, void raycast( Ray ray,
RaycastTest raycastTest, RaycastTest raycastTest,
unsigned short raycastWithCategoryMaskBits) ; int raycastWithCategoryMaskBits) ;
}; };
} }

View File

@ -85,7 +85,7 @@ int DynamicAABBTree::allocateNode() {
// Release a node // Release a node
void DynamicAABBTree::releaseNode(int nodeID) { void DynamicAABBTree::releaseNode(int nodeID) {
assert(this.numberNodes > 0); assert(this.numberNodes > 0);
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
assert(this.nodes[nodeID].height >= 0); assert(this.nodes[nodeID].height >= 0);
this.nodes[nodeID].nextNodeID = this.freeNodeID; this.nodes[nodeID].nextNodeID = this.freeNodeID;
this.nodes[nodeID].height = -1; 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) // Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode(); int nodeID = allocateNode();
// Create the fat aabb to use in the tree // 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.setMin(aabb.getMin() - gap);
this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap); this.nodes[nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the height of the node in the tree // Set the height of the node in the tree
@ -113,7 +113,7 @@ int DynamicAABBTree::addObjectInternal( AABB aabb) {
// Remove an object from the tree // Remove an object from the tree
void DynamicAABBTree::removeObject(int nodeID) { void DynamicAABBTree::removeObject(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
assert(this.nodes[nodeID].isLeaf()); assert(this.nodes[nodeID].isLeaf());
// Remove the node from the tree // Remove the node from the tree
removeLeafNode(nodeID); 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 /// 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 /// 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). /// (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()"); 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].isLeaf());
assert(this.nodes[nodeID].height >= 0); assert(this.nodes[nodeID].height >= 0);
Log.verbose(" compare : " << this.nodes[nodeID].aabb.this.minCoordinates << " " << this.nodes[nodeID].aabb.this.maxCoordinates); Log.verbose(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
Log.verbose(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates); Log.verbose(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
// If the new AABB is still inside the fat AABB of the node // If the new AABB is still inside the fat AABB of the node
if ( forceReinsert == false if ( forceReinsert == false
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.nodes[nodeID].aabb.contains(newAABB)) { && this.nodes[nodeID].aabb.contains(newAABB)) {
return false; return false;
} }
// If the new AABB is outside the fat AABB, we remove the corresponding node // If the new AABB is outside the fat AABB, we remove the corresponding node
removeLeafNode(nodeID); removeLeafNode(nodeID);
// Compute the fat AABB by inflating the AABB with a ant gap // Compute the fat AABB by inflating the AABB with a ant gap
this.nodes[nodeID].aabb = newAABB; this.nodes[nodeID].aabb = newAABB;
vec3 gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap); Vector3f gap(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
this.nodes[nodeID].aabb.this.minCoordinates -= gap; this.nodes[nodeID].aabb.minCoordinates -= gap;
this.nodes[nodeID].aabb.this.maxCoordinates += gap; this.nodes[nodeID].aabb.maxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB // Inflate the fat AABB in direction of the linear motion of the AABB
if (displacement.x() < 0.0f) { 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 { } 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) { 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 { } 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) { 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 { } 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(" compare : " + this.nodes[nodeID].aabb.minCoordinates + " " + this.nodes[nodeID].aabb.maxCoordinates);
Log.error(" : " << newAABB.this.minCoordinates << " " << newAABB.this.maxCoordinates); Log.error(" : " + newAABB.minCoordinates + " " + newAABB.maxCoordinates);
if (this.nodes[nodeID].aabb.contains(newAABB) == false) { if (this.nodes[nodeID].aabb.contains(newAABB) == false) {
//Log.critical("ERROR"); //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 // 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 // the cost of going down into the left or right child
if (costS < costLeft hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj costS < costRight) { if (costS < costLeft && costS < costRight) {
break; break;
} }
// It is cheaper to go down into a child of the current node, choose the best child // 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(leftChild != TreeNode::NULLTREENODE);
assert(rightChild != TreeNode::NULLTREENODE); assert(rightChild != TreeNode::NULLTREENODE);
// Recompute the height of the node in the tree // 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; this.nodes[rightChild].height) + 1;
assert(this.nodes[currentNodeID].height > 0); assert(this.nodes[currentNodeID].height > 0);
// Recompute the AABB of the node // Recompute the AABB of the node
@ -285,7 +285,7 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
// Remove a leaf node from the tree // Remove a leaf node from the tree
void DynamicAABBTree::removeLeafNode(int nodeID) { void DynamicAABBTree::removeLeafNode(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
assert(this.nodes[nodeID].isLeaf()); assert(this.nodes[nodeID].isLeaf());
// If we are removing the root node (root node is a leaf in this case) // If we are removing the root node (root node is a leaf in this case)
if (this.rootNodeID == nodeID) { if (this.rootNodeID == nodeID) {
@ -324,7 +324,7 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Recompute the AABB and the height of the current node // Recompute the AABB and the height of the current node
this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb, this.nodes[currentNodeID].aabb.mergeTwoAABBs(this.nodes[leftChildID].aabb,
this.nodes[rightChildID].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; this.nodes[rightChildID].height) + 1;
assert(this.nodes[currentNodeID].height > 0); assert(this.nodes[currentNodeID].height > 0);
currentNodeID = this.nodes[currentNodeID].parentID; currentNodeID = this.nodes[currentNodeID].parentID;
@ -344,123 +344,123 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeID != TreeNode::NULLTREENODE); assert(nodeID != TreeNode::NULLTREENODE);
TreeNode* nodeA = this.nodes + nodeID; TreeNode* nodeA = this.nodes + nodeID;
// If the node is a leaf or the height of A's sub-tree is less than 2 // 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 // Do not perform any rotation
return nodeID; return nodeID;
} }
// Get the two children nodes // Get the two children nodes
int nodeBID = nodeA->children[0]; int nodeBID = nodeA.children[0];
int nodeCID = nodeA->children[1]; int nodeCID = nodeA.children[1];
assert(nodeBID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeBID < this.numberAllocatedNodes); assert(nodeBID >= 0 && nodeBID < this.numberAllocatedNodes);
assert(nodeCID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeCID < this.numberAllocatedNodes); assert(nodeCID >= 0 && nodeCID < this.numberAllocatedNodes);
TreeNode* nodeB = this.nodes + nodeBID; TreeNode* nodeB = this.nodes + nodeBID;
TreeNode* nodeC = this.nodes + nodeCID; TreeNode* nodeC = this.nodes + nodeCID;
// Compute the factor of the left and right sub-trees // 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 the right node C is 2 higher than left node B
if (balanceFactor > 1) { if (balanceFactor > 1) {
assert(!nodeC->isLeaf()); assert(!nodeC.isLeaf());
int nodeFID = nodeC->children[0]; int nodeFID = nodeC.children[0];
int nodeGID = nodeC->children[1]; int nodeGID = nodeC.children[1];
assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes); assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes);
assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
TreeNode* nodeF = this.nodes + nodeFID; TreeNode* nodeF = this.nodes + nodeFID;
TreeNode* nodeG = this.nodes + nodeGID; TreeNode* nodeG = this.nodes + nodeGID;
nodeC->children[0] = nodeID; nodeC.children[0] = nodeID;
nodeC->parentID = nodeA->parentID; nodeC.parentID = nodeA.parentID;
nodeA->parentID = nodeCID; nodeA.parentID = nodeCID;
if (nodeC->parentID != TreeNode::NULLTREENODE) { if (nodeC.parentID != TreeNode::NULLTREENODE) {
if (this.nodes[nodeC->parentID].children[0] == nodeID) { if (this.nodes[nodeC.parentID].children[0] == nodeID) {
this.nodes[nodeC->parentID].children[0] = nodeCID; this.nodes[nodeC.parentID].children[0] = nodeCID;
} else { } else {
assert(this.nodes[nodeC->parentID].children[1] == nodeID); assert(this.nodes[nodeC.parentID].children[1] == nodeID);
this.nodes[nodeC->parentID].children[1] = nodeCID; this.nodes[nodeC.parentID].children[1] = nodeCID;
} }
} else { } else {
this.rootNodeID = nodeCID; this.rootNodeID = nodeCID;
} }
assert(!nodeC->isLeaf()); assert(!nodeC.isLeaf());
assert(!nodeA->isLeaf()); assert(!nodeA.isLeaf());
// If the right node C was higher than left node B because of the F node // If the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) { if (nodeF.height > nodeG.height) {
nodeC->children[1] = nodeFID; nodeC.children[1] = nodeFID;
nodeA->children[1] = nodeGID; nodeA.children[1] = nodeGID;
nodeG->parentID = nodeID; nodeG.parentID = nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb); nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
// Recompute the height of node A and C // Recompute the height of node A and C
nodeA->height = etk::max(nodeB->height, nodeG->height) + 1; nodeA.height = max(nodeB.height, nodeG.height) + 1;
nodeC->height = etk::max(nodeA->height, nodeF->height) + 1; nodeC.height = max(nodeA.height, nodeF.height) + 1;
assert(nodeA->height > 0); assert(nodeA.height > 0);
assert(nodeC->height > 0); assert(nodeC.height > 0);
} else { } else {
// If the right node C was higher than left node B because of node G // If the right node C was higher than left node B because of node G
nodeC->children[1] = nodeGID; nodeC.children[1] = nodeGID;
nodeA->children[1] = nodeFID; nodeA.children[1] = nodeFID;
nodeF->parentID = nodeID; nodeF.parentID = nodeID;
// Recompute the AABB of node A and C // Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb); nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
// Recompute the height of node A and C // Recompute the height of node A and C
nodeA->height = etk::max(nodeB->height, nodeF->height) + 1; nodeA.height = max(nodeB.height, nodeF.height) + 1;
nodeC->height = etk::max(nodeA->height, nodeG->height) + 1; nodeC.height = max(nodeA.height, nodeG.height) + 1;
assert(nodeA->height > 0); assert(nodeA.height > 0);
assert(nodeC->height > 0); assert(nodeC.height > 0);
} }
// Return the new root of the sub-tree // Return the new root of the sub-tree
return nodeCID; return nodeCID;
} }
// If the left node B is 2 higher than right node C // If the left node B is 2 higher than right node C
if (balanceFactor < -1) { if (balanceFactor < -1) {
assert(!nodeB->isLeaf()); assert(!nodeB.isLeaf());
int nodeFID = nodeB->children[0]; int nodeFID = nodeB.children[0];
int nodeGID = nodeB->children[1]; int nodeGID = nodeB.children[1];
assert(nodeFID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeFID < this.numberAllocatedNodes); assert(nodeFID >= 0 && nodeFID < this.numberAllocatedNodes);
assert(nodeGID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeGID < this.numberAllocatedNodes); assert(nodeGID >= 0 && nodeGID < this.numberAllocatedNodes);
TreeNode* nodeF = this.nodes + nodeFID; TreeNode* nodeF = this.nodes + nodeFID;
TreeNode* nodeG = this.nodes + nodeGID; TreeNode* nodeG = this.nodes + nodeGID;
nodeB->children[0] = nodeID; nodeB.children[0] = nodeID;
nodeB->parentID = nodeA->parentID; nodeB.parentID = nodeA.parentID;
nodeA->parentID = nodeBID; nodeA.parentID = nodeBID;
if (nodeB->parentID != TreeNode::NULLTREENODE) { if (nodeB.parentID != TreeNode::NULLTREENODE) {
if (this.nodes[nodeB->parentID].children[0] == nodeID) { if (this.nodes[nodeB.parentID].children[0] == nodeID) {
this.nodes[nodeB->parentID].children[0] = nodeBID; this.nodes[nodeB.parentID].children[0] = nodeBID;
} else { } else {
assert(this.nodes[nodeB->parentID].children[1] == nodeID); assert(this.nodes[nodeB.parentID].children[1] == nodeID);
this.nodes[nodeB->parentID].children[1] = nodeBID; this.nodes[nodeB.parentID].children[1] = nodeBID;
} }
} else { } else {
this.rootNodeID = nodeBID; this.rootNodeID = nodeBID;
} }
assert(!nodeB->isLeaf()); assert(!nodeB.isLeaf());
assert(!nodeA->isLeaf()); assert(!nodeA.isLeaf());
// If the left node B was higher than right node C because of the F node // If the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) { if (nodeF.height > nodeG.height) {
nodeB->children[1] = nodeFID; nodeB.children[1] = nodeFID;
nodeA->children[0] = nodeGID; nodeA.children[0] = nodeGID;
nodeG->parentID = nodeID; nodeG.parentID = nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb); nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb); nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
// Recompute the height of node A and B // Recompute the height of node A and B
nodeA->height = etk::max(nodeC->height, nodeG->height) + 1; nodeA.height = max(nodeC.height, nodeG.height) + 1;
nodeB->height = etk::max(nodeA->height, nodeF->height) + 1; nodeB.height = max(nodeA.height, nodeF.height) + 1;
assert(nodeA->height > 0); assert(nodeA.height > 0);
assert(nodeB->height > 0); assert(nodeB.height > 0);
} else { } else {
// If the left node B was higher than right node C because of node G // If the left node B was higher than right node C because of node G
nodeB->children[1] = nodeGID; nodeB.children[1] = nodeGID;
nodeA->children[0] = nodeFID; nodeA.children[0] = nodeFID;
nodeF->parentID = nodeID; nodeF.parentID = nodeID;
// Recompute the AABB of node A and B // Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb); nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb); nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
// Recompute the height of node A and B // Recompute the height of node A and B
nodeA->height = etk::max(nodeC->height, nodeF->height) + 1; nodeA.height = max(nodeC.height, nodeF.height) + 1;
nodeB->height = etk::max(nodeA->height, nodeG->height) + 1; nodeB.height = max(nodeA.height, nodeG.height) + 1;
assert(nodeA->height > 0); assert(nodeA.height > 0);
assert(nodeB->height > 0); assert(nodeB.height > 0);
} }
// Return the new root of the sub-tree // Return the new root of the sub-tree
return nodeBID; return nodeBID;
@ -470,7 +470,7 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
} }
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function<void(int nodeId)> callback) { void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) {
if (callback == null) { if (callback == null) {
Log.error("call with null callback"); Log.error("call with null callback");
return; return;
@ -489,23 +489,23 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB( AABB aabb, etk::Functi
// Get the corresponding node // Get the corresponding node
TreeNode* nodeToVisit = this.nodes + nodeIDToVisit; TreeNode* nodeToVisit = this.nodes + nodeIDToVisit;
// If the AABB in parameter overlaps with the AABB of the node to visit // If the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit->aabb)) { if (aabb.testCollision(nodeToVisit.aabb)) {
// If the node is a leaf // If the node is a leaf
if (nodeToVisit->isLeaf()) { if (nodeToVisit.isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair // Notify the broad-phase about a new potential overlapping pair
callback(nodeIDToVisit); callback(nodeIDToVisit);
} else { } else {
// If the node is not a leaf // If the node is not a leaf
// We need to visit its children // We need to visit its children
stack.push(nodeToVisit->children[0]); stack.push(nodeToVisit.children[0]);
stack.push(nodeToVisit->children[1]); stack.push(nodeToVisit.children[1]);
} }
} }
} }
} }
// Ray casting method // Ray casting method
void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId, ephysics::Ray ray)> callback) { void DynamicAABBTree::raycast( ephysics::Ray ray, Function<float(int nodeId, ephysics::Ray ray)> callback) {
PROFILE("DynamicAABBTree::raycast()"); PROFILE("DynamicAABBTree::raycast()");
if (callback == null) { if (callback == null) {
Log.error("call with null callback"); Log.error("call with null callback");
@ -527,11 +527,11 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId
TreeNode* node = this.nodes + nodeID; TreeNode* node = this.nodes + nodeID;
Ray rayTemp(ray.point1, ray.point2, maxFraction); Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Test if the ray intersects with the current node AABB // Test if the ray intersects with the current node AABB
if (node->aabb.testRayIntersect(rayTemp) == false) { if (node.aabb.testRayIntersect(rayTemp) == false) {
continue; continue;
} }
// If the node is a leaf of the tree // 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 // Call the callback that will raycast again the broad-phase shape
float hitFraction = callback(nodeID, rayTemp); float hitFraction = callback(nodeID, rayTemp);
// If the user returned a hitFraction of zero, it means that // If the user returned a hitFraction of zero, it means that
@ -551,8 +551,8 @@ void DynamicAABBTree::raycast( ephysics::Ray ray, etk::Function<float(int nodeId
// the raycasting as if the proxy shape did not exist // the raycasting as if the proxy shape did not exist
} else { // If the node has children } else { // If the node has children
// Push its children in the stack of nodes to explore // Push its children in the stack of nodes to explore
stack.push(node->children[0]); stack.push(node.children[0]);
stack.push(node->children[1]); stack.push(node.children[1]);
} }
} }
} }
@ -564,20 +564,20 @@ boolean TreeNode::isLeaf() {
// Return the fat AABB corresponding to a given node ID // Return the fat AABB corresponding to a given node ID
AABB DynamicAABBTree::getFatAABB(int nodeID) { AABB DynamicAABBTree::getFatAABB(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
return this.nodes[nodeID].aabb; return this.nodes[nodeID].aabb;
} }
// Return the pointer to the data array of a given leaf node of the tree // Return the pointer to the data array of a given leaf node of the tree
int* DynamicAABBTree::getNodeDataInt(int nodeID) { int* DynamicAABBTree::getNodeDataInt(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
assert(this.nodes[nodeID].isLeaf()); assert(this.nodes[nodeID].isLeaf());
return this.nodes[nodeID].dataInt; return this.nodes[nodeID].dataInt;
} }
// Return the pointer to the data pointer of a given leaf node of the tree // Return the pointer to the data pointer of a given leaf node of the tree
void* DynamicAABBTree::getNodeDataPointer(int nodeID) { void* DynamicAABBTree::getNodeDataPointer(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
assert(this.nodes[nodeID].isLeaf()); assert(this.nodes[nodeID].isLeaf());
return this.nodes[nodeID].dataPointer; return this.nodes[nodeID].dataPointer;
} }
@ -615,7 +615,7 @@ void DynamicAABBTree::check() {
int freeNodeID = this.freeNodeID; int freeNodeID = this.freeNodeID;
// Check the free nodes // Check the free nodes
while(freeNodeID != TreeNode::NULLTREENODE) { while(freeNodeID != TreeNode::NULLTREENODE) {
assert(0 <= freeNodeID hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj freeNodeID < this.numberAllocatedNodes); assert(0 <= freeNodeID && freeNodeID < this.numberAllocatedNodes);
freeNodeID = this.nodes[freeNodeID].nextNodeID; freeNodeID = this.nodes[freeNodeID].nextNodeID;
nbFreeNodes++; nbFreeNodes++;
} }
@ -633,26 +633,26 @@ void DynamicAABBTree::checkNode(int nodeID) {
} }
// Get the children nodes // Get the children nodes
TreeNode* pNode = this.nodes + nodeID; TreeNode* pNode = this.nodes + nodeID;
assert(!pNode->isLeaf()); assert(!pNode.isLeaf());
int leftChild = pNode->children[0]; int leftChild = pNode.children[0];
int rightChild = pNode->children[1]; int rightChild = pNode.children[1];
assert(pNode->height >= 0); assert(pNode.height >= 0);
assert(pNode->aabb.getVolume() > 0); assert(pNode.aabb.getVolume() > 0);
// If the current node is a leaf // If the current node is a leaf
if (pNode->isLeaf()) { if (pNode.isLeaf()) {
// Check that there are no children // Check that there are no children
assert(leftChild == TreeNode::NULLTREENODE); assert(leftChild == TreeNode::NULLTREENODE);
assert(rightChild == TreeNode::NULLTREENODE); assert(rightChild == TreeNode::NULLTREENODE);
assert(pNode->height == 0); assert(pNode.height == 0);
} else { } else {
// Check that the children node IDs are valid // Check that the children node IDs are valid
assert(0 <= leftChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj leftChild < this.numberAllocatedNodes); assert(0 <= leftChild && leftChild < this.numberAllocatedNodes);
assert(0 <= rightChild hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj rightChild < this.numberAllocatedNodes); assert(0 <= rightChild && rightChild < this.numberAllocatedNodes);
// Check that the children nodes have the correct parent node // Check that the children nodes have the correct parent node
assert(this.nodes[leftChild].parentID == nodeID); assert(this.nodes[leftChild].parentID == nodeID);
assert(this.nodes[rightChild].parentID == nodeID); assert(this.nodes[rightChild].parentID == nodeID);
// Check the height of node // 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); assert(this.nodes[nodeID].height == height);
// Check the AABB of the node // Check the AABB of the node
AABB aabb; AABB aabb;
@ -672,17 +672,17 @@ int DynamicAABBTree::computeHeight() {
// Compute the height of a given node in the tree // Compute the height of a given node in the tree
int DynamicAABBTree::computeHeight(int nodeID) { int DynamicAABBTree::computeHeight(int nodeID) {
assert(nodeID >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nodeID < this.numberAllocatedNodes); assert(nodeID >= 0 && nodeID < this.numberAllocatedNodes);
TreeNode* node = this.nodes + nodeID; TreeNode* node = this.nodes + nodeID;
// If the node is a leaf, its height is zero // If the node is a leaf, its height is zero
if (node->isLeaf()) { if (node.isLeaf()) {
return 0; return 0;
} }
// Compute the height of the left and right sub-tree // Compute the height of the left and right sub-tree
int leftHeight = computeHeight(node->children[0]); int leftHeight = computeHeight(node.children[0]);
int rightHeight = computeHeight(node->children[1]); int rightHeight = computeHeight(node.children[1]);
// Return the height of the node // Return the height of the node
return 1 + etk::max(leftHeight, rightHeight); return 1 + max(leftHeight, rightHeight);
} }
#endif #endif

View File

@ -1,19 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.broadphase;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/configuration.hpp>
#include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <etk/Function.hpp>
namespace ephysics {
// TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T> // TODO: to replace this, create a Tree<T> template (multiple child) or TreeRedBlack<T>
/** /**
* @brief It represents a node of the dynamic AABB tree. * @brief It represents a node of the dynamic AABB tree.
@ -85,7 +71,7 @@ namespace ephysics {
/// Constructor /// Constructor
DynamicAABBTree(float extraAABBGap = 0.0f); DynamicAABBTree(float extraAABBGap = 0.0f);
/// Destructor /// Destructor
virtual ~DynamicAABBTree(); ~DynamicAABBTree();
/// Add an object into the tree (where node data are two integers) /// Add an object into the tree (where node data are two integers)
int addObject( AABB aabb, int data1, int data2); int addObject( AABB aabb, int data1, int data2);
/// Add an object into the tree (where node data is a pointer) /// Add an object into the tree (where node data is a pointer)
@ -93,7 +79,7 @@ namespace ephysics {
/// Remove an object from the tree /// Remove an object from the tree
void removeObject(int nodeID); void removeObject(int nodeID);
/// Update the dynamic tree after an object has moved. /// 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 /// Return the fat AABB corresponding to a given node ID
AABB getFatAABB(int nodeID) ; AABB getFatAABB(int nodeID) ;
/// Return the pointer to the data array of a given leaf node of the tree /// 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 /// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) ; void* getNodeDataPointer(int nodeID) ;
/// Report all shapes overlapping with the AABB given in parameter. /// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function<void(int nodeId)> callback) ; void reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) ;
/// Ray casting method /// Ray casting method
void raycast( Ray ray, etk::Function<float(int nodeId, ephysics::Ray ray)> callback) ; void raycast( Ray ray, Function<float(int nodeId, ephysics::Ray ray)> callback) ;
/// Compute the height of the tree /// Compute the height of the tree
int computeHeight(); int computeHeight();
/// Return the root AABB of the tree /// Return the root AABB of the tree

View File

@ -1,37 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
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;
};
}

View File

@ -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;
};
}

View File

@ -27,7 +27,7 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info,
ConvexShape* convexShape; ConvexShape* convexShape;
ConcaveShape* concaveShape; ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave // Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) { if (shape1Info.collisionShape.isConvex()) {
convexProxyShape = shape1Info.proxyShape; convexProxyShape = shape1Info.proxyShape;
convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape); convexShape = staticcast< ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape; concaveProxyShape = shape2Info.proxyShape;
@ -48,48 +48,48 @@ void ConcaveVsConvexAlgorithm::testCollision( CollisionShapeInfo shape1Info,
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair); convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape // Compute the convex shape AABB in the local-space of the convex shape
AABB aabb; AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform()); convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform());
// If smooth mesh collision is enabled for the concave mesh // If smooth mesh collision is enabled for the concave mesh
if (concaveShape->getIsSmoothMeshCollisionEnabled()) { if (concaveShape.getIsSmoothMeshCollisionEnabled()) {
etk::Vector<SmoothMeshContactInfo> contactPoints; Vector<SmoothMeshContactInfo> contactPoints;
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints); SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback); convexVsTriangleCallback.setNarrowPhaseCallback(smoothNarrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape // 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 // Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, callback); processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, callback);
} else { } else {
convexVsTriangleCallback.setNarrowPhaseCallback(callback); convexVsTriangleCallback.setNarrowPhaseCallback(callback);
// Call the convex vs triangle callback for each triangle of the concave shape // 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 // Create a triangle collision shape
float margin = this.concaveShape->getTriangleMargin(); float margin = this.concaveShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape // 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 there is no collision algorithm between those two kinds of shapes
if (algo == null) { if (algo == null) {
return; return;
} }
// Notify the narrow-phase algorithm about the overlapping pair we are going to test // Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(this.overlappingPair); algo.setCurrentOverlappingPair(this.overlappingPair);
// Create the CollisionShapeInfo objects // Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(this.convexProxyShape, CollisionShapeInfo shapeConvexInfo(this.convexProxyShape,
this.convexShape, this.convexShape,
this.convexProxyShape->getLocalToWorldTransform(), this.convexProxyShape.getLocalToWorldTransform(),
this.overlappingPair, this.overlappingPair,
this.convexProxyShape->getCachedCollisionData()); this.convexProxyShape.getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape, CollisionShapeInfo shapeConcaveInfo(this.concaveProxyShape,
triangleShape, triangleShape,
this.concaveProxyShape->getLocalToWorldTransform(), this.concaveProxyShape.getLocalToWorldTransform(),
this.overlappingPair, this.overlappingPair,
this.concaveProxyShape->getCachedCollisionData()); this.concaveProxyShape.getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape // 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) { static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactInfo contact2) {
@ -97,17 +97,17 @@ static boolean sortFunction( SmoothMeshContactInfo contact1, SmoothMeshContactI
} }
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair, void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
etk::Vector<SmoothMeshContactInfo> contactPoints, Vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* callback) { NarrowPhaseCallback* callback) {
// Set with the triangle vertices already processed to void further contacts with same triangle // Set with the triangle vertices already processed to void further contacts with same triangle
etk::Vector<etk::Pair<int, vec3>> processTriangleVertices; Vector<Pair<int, Vector3f>> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth // 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) // For each contact point (from smaller penetration depth to larger)
etk::Vector<SmoothMeshContactInfo>::Iterator it; Vector<SmoothMeshContactInfo>::Iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) { for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
SmoothMeshContactInfo info = *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 // Compute the barycentric coordinates of the point in the triangle
float u, v, w; float u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
@ -129,21 +129,21 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
} }
// If it is a vertex contact // If it is a vertex contact
if (nbZeros == 2) { 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 // Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) { if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
callback->notifyContact(overlappingPair, info.contactInfo); callback.notifyContact(overlappingPair, info.contactInfo);
} }
} else if (nbZeros == 1) { } else if (nbZeros == 1) {
// If it is an edge contact // If it is an edge contact
vec3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]); Vector3f 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 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet // Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) { !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it // Keep the contact as it is and report it
callback->notifyContact(overlappingPair, info.contactInfo); callback.notifyContact(overlappingPair, info.contactInfo);
} }
} else { } else {
// If it is a face contact // If it is a face contact
@ -151,19 +151,19 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
ProxyShape* firstShape; ProxyShape* firstShape;
ProxyShape* secondShape; ProxyShape* secondShape;
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1(); firstShape = overlappingPair.getShape1();
secondShape = overlappingPair->getShape2(); secondShape = overlappingPair.getShape2();
} else { } else {
firstShape = overlappingPair->getShape2(); firstShape = overlappingPair.getShape2();
secondShape = overlappingPair->getShape1(); secondShape = overlappingPair.getShape1();
} }
// We use the triangle normal as the contact normal // We use the triangle normal as the contact normal
vec3 a = info.triangleVertices[1] - info.triangleVertices[0]; Vector3f a = info.triangleVertices[1] - info.triangleVertices[0];
vec3 b = info.triangleVertices[2] - info.triangleVertices[0]; Vector3f b = info.triangleVertices[2] - info.triangleVertices[0];
vec3 localNormal = a.cross(b); Vector3f localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal; newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation() * localNormal;
vec3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2; Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
vec3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint; Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize(); newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) { if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal; 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 // 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 // the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque // Dirk Gregorius) to avoid adding torque
etk::Transform3D worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse(); Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) { if (info.isFirstShapeTriangle) {
vec3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal; Vector3f newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
} else { } else {
vec3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal; Vector3f newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint; newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
} }
// Report the contact // Report the contact
callback->notifyContact(overlappingPair, newContactInfo); callback.notifyContact(overlappingPair, newContactInfo);
} }
// Add the three vertices of the triangle to the set of processed // Add the three vertices of the triangle to the set of processed
// triangle vertices // triangle vertices
@ -190,14 +190,14 @@ void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overl
} }
} }
boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<int, vec3>> processTriangleVertices, vec3 vertex) { boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices, Vector3f vertex) {
/* TODO : etk::Vector<etk::Pair<int, vec3>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good... /* TODO : Vector<Pair<int, Vector3f>> 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()); int key = int(vertex.x() * vertex.y() * vertex.z());
auto range = processTriangleVertices.equalrange(key); auto range = processTriangleVertices.equalrange(key);
for (auto it = range.first; it != range.second; ++it) { for (auto it = range.first; it != range.second; ++it) {
if ( vertex.x() == it->second.x() if ( vertex.x() == it.second.x()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.y() == it->second.y() && vertex.y() == it.second.y()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.z() == it->second.z()) { && vertex.z() == it.second.z()) {
return true; return true;
} }
} }
@ -206,8 +206,8 @@ boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<
// TODO : This is not really the same ... // TODO : This is not really the same ...
for (auto it: processTriangleVertices) { for (auto it: processTriangleVertices) {
if ( vertex.x() == it.second.x() if ( vertex.x() == it.second.x()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.y() == it.second.y() && vertex.y() == it.second.y()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vertex.z() == it.second.z()) { && vertex.z() == it.second.z()) {
return true; return true;
} }
} }
@ -216,22 +216,22 @@ boolean ConcaveVsConvexAlgorithm::hasVertexBeenProcessed( etk::Vector<etk::Pair<
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair, void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
ContactPointInfo contactInfo) { ContactPointInfo contactInfo) {
vec3 triangleVertices[3]; Vector3f triangleVertices[3];
boolean isFirstShapeTriangle; boolean isFirstShapeTriangle;
// If the collision shape 1 is the triangle // If the collision shape 1 is the triangle
if (contactInfo.collisionShape1->getType() == TRIANGLE) { if (contactInfo.collisionShape1.getType() == TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != TRIANGLE); assert(contactInfo.collisionShape2.getType() != TRIANGLE);
TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape1); TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape1);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape.getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape.getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape.getVertex(2);
isFirstShapeTriangle = true; isFirstShapeTriangle = true;
} else { // If the collision shape 2 is the triangle } 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); TriangleShape* triangleShape = staticcast< TriangleShape*>(contactInfo.collisionShape2);
triangleVertices[0] = triangleShape->getVertex(0); triangleVertices[0] = triangleShape.getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1); triangleVertices[1] = triangleShape.getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2); triangleVertices[2] = triangleShape.getVertex(2);
isFirstShapeTriangle = false; isFirstShapeTriangle = false;
} }
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]); SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);

View File

@ -1,25 +1,12 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/shapes/ConcaveShape.hpp>
namespace ephysics {
/** /**
* @brief This class is used to encapsulate a callback method for * @brief This class is used to encapsulate a callback method for
* collision detection between the triangle of a concave mesh shape * collision detection between the triangle of a concave mesh shape
* and a convex shape. * and a convex shape.
*/ */
class ConvexVsTriangleCallback : public TriangleCallback { class ConvexVsTriangleCallback extends TriangleCallback {
protected: protected:
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback NarrowPhaseCallback* this.narrowPhaseCallback; //!< Narrow-phase collision callback
@ -57,7 +44,7 @@ namespace ephysics {
this.concaveProxyShape = concaveProxyShape; this.concaveProxyShape = concaveProxyShape;
} }
/// Test collision between a triangle and the convex mesh shape /// 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: public:
ContactPointInfo contactInfo; ContactPointInfo contactInfo;
boolean isFirstShapeTriangle; boolean isFirstShapeTriangle;
vec3 triangleVertices[3]; Vector3f triangleVertices[3];
/// Constructor /// Constructor
SmoothMeshContactInfo( ContactPointInfo contact, SmoothMeshContactInfo( ContactPointInfo contact,
boolean firstShapeTriangle, boolean firstShapeTriangle,
vec3 trianglePoint1, Vector3f trianglePoint1,
vec3 trianglePoint2, Vector3f trianglePoint2,
vec3 trianglePoint3): Vector3f trianglePoint3):
contactInfo(contact) { contactInfo(contact) {
isFirstShapeTriangle = firstShapeTriangle; isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1; triangleVertices[0] = trianglePoint1;
@ -82,7 +69,7 @@ namespace ephysics {
triangleVertices[2] = trianglePoint3; triangleVertices[2] = trianglePoint3;
} }
SmoothMeshContactInfo() { 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 * 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. * the smooth mesh collision algorithm if this one is enabled.
*/ */
class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback { class SmoothCollisionNarrowPhaseCallback extends NarrowPhaseCallback {
private: private:
etk::Vector<SmoothMeshContactInfo> this.contactPoints; Vector<SmoothMeshContactInfo> this.contactPoints;
public: public:
// Constructor // Constructor
SmoothCollisionNarrowPhaseCallback(etk::Vector<SmoothMeshContactInfo> contactPoints): SmoothCollisionNarrowPhaseCallback(Vector<SmoothMeshContactInfo> contactPoints):
this.contactPoints(contactPoints) { this.contactPoints(contactPoints) {
} }
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
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 * to use the GJK collision detection algorithm to compute the collision between
* the convex shape and each of the triangles in the concave shape. * the convex shape and each of the triangles in the concave shape.
*/ */
class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm { class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
protected : protected :
/// Private copy-ructor /// Private copy-ructor
ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm); ConcaveVsConvexAlgorithm( ConcaveVsConvexAlgorithm algorithm);
@ -126,20 +113,20 @@ namespace ephysics {
ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm); ConcaveVsConvexAlgorithm operator=( ConcaveVsConvexAlgorithm algorithm);
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm /// Process the concave triangle mesh collision using the smooth mesh collision algorithm
void processSmoothMeshCollision(OverlappingPair* overlappingPair, void processSmoothMeshCollision(OverlappingPair* overlappingPair,
etk::Vector<SmoothMeshContactInfo> contactPoints, Vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles /// Add a triangle vertex into the set of processed triangles
void addProcessedVertex(etk::Vector<etk::Pair<int, vec3>> processTriangleVertices, vec3 vertex) { void addProcessedVertex(Vector<Pair<int, Vector3f>> processTriangleVertices, Vector3f vertex) {
processTriangleVertices.pushBack(etk::makePair(int(vertex.x() * vertex.y() * vertex.z()), 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 /// Return true if the vertex is in the set of already processed vertices
boolean hasVertexBeenProcessed( etk::Vector<etk::Pair<int, vec3>> processTriangleVertices, boolean hasVertexBeenProcessed( Vector<Pair<int, Vector3f>> processTriangleVertices,
vec3 vertex) ; Vector3f vertex) ;
public : public :
/// Constructor /// Constructor
ConcaveVsConvexAlgorithm(); ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision( CollisionShapeInfo shape1Info, void testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };

View File

@ -30,15 +30,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
CollisionShapeType shape1Type = staticcast<CollisionShapeType>(type1); CollisionShapeType shape1Type = staticcast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = staticcast<CollisionShapeType>(type2); CollisionShapeType shape2Type = staticcast<CollisionShapeType>(type2);
// Sphere vs Sphere algorithm // Sphere vs Sphere algorithm
if (shape1Type == SPHERE hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shape2Type == SPHERE) { if (shape1Type == SPHERE && shape2Type == SPHERE) {
return this.sphereVsSphereAlgorithm; return this.sphereVsSphereAlgorithm;
} else if ( ( !CollisionShape::isConvex(shape1Type) } else if ( ( !CollisionShape::isConvex(shape1Type)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape2Type) ) && CollisionShape::isConvex(shape2Type) )
|| ( !CollisionShape::isConvex(shape2Type) || ( !CollisionShape::isConvex(shape2Type)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj CollisionShape::isConvex(shape1Type) ) ) { && CollisionShape::isConvex(shape1Type) ) ) {
// Concave vs Convex algorithm // Concave vs Convex algorithm
return this.concaveVsConvexAlgorithm; 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) // Convex vs Convex algorithm (GJK algorithm)
return this.GJKAlgorithm; return this.GJKAlgorithm;
} else { } else {

View File

@ -1,37 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/CollisionDispatch.hpp>
#include <ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp>
#include <ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
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;
};
}

View File

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

View File

@ -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 // 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)) { if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4; return 4;
} }
// Check vertex 2 // 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)) { if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1; return 1;
} }
// Check vertex 3 // 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)) { if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2; return 2;
} }
// Check vertex 4 // 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)) { if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3; return 3;
} }
@ -48,32 +48,32 @@ int EPAAlgorithm::isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4)
void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex, void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo shape1Info,
etk::Transform3D transform1, Transform3D transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
etk::Transform3D transform2, Transform3D transform2,
vec3 vector, Vector3f vector,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape.isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape.isConvex());
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
vec3 suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates Vector3f suppPointsA[MAXSUPPORTPOINTS]; // Support points of object A in local coordinates
vec3 suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates Vector3f suppPointsB[MAXSUPPORTPOINTS]; // Support points of object B in local coordinates
vec3 points[MAXSUPPORTPOINTS]; // Current points Vector3f points[MAXSUPPORTPOINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
etk::Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist Set<TriangleEPA*> triangleHeap; // list of face candidate of the EPA algorithm sorted lower square dist to upper square dist
triangleHeap.setComparator([](TriangleEPA * face1, TriangleEPA * face2) { 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) // 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 // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // 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 // Get the simplex computed previously by the GJK algorithm
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance // Compute the tolerance
@ -98,32 +98,32 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
// ruct the polytope are the three support points in those three directions // ruct the polytope are the three support points in those three directions
// v1, v2 and v3. // v1, v2 and v3.
// Direction of the segment // 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 // Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.absolute().getMinAxis(); int minAxis = d.absolute().getMinAxis();
// Compute sin(60) // Compute sin(60)
float sin60 = float(sqrt(3.0)) * 0.5f; float sin60 = float(sqrt(3.0)) * 0.5f;
// Create a rotation quaternion to rotate the vector v1 to get the vectors // Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3 // 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 // Compute the vector v1, v2, v3
vec3 v1 = d.cross(vec3(minAxis == 0, minAxis == 1, minAxis == 2)); Vector3f v1 = d.cross(Vector3f(minAxis == 0, minAxis == 1, minAxis == 2));
vec3 v2 = rotationQuat * v1; Vector3f v2 = rotationQuat * v1;
vec3 v3 = rotationQuat * v2; Vector3f v3 = rotationQuat * v2;
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData); suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 * suppPointsB[2] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData); shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2]; points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2 // Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData); suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData); shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3 // Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData); suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData); shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the // 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 // 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* face2 = triangleStore.newTriangle(points, 0, 2, 3);
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2); TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the ructed tetrahedron is not correct // If the ructed tetrahedron is not correct
if (!((face0 != NULL) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL) if (!((face0 != NULL) && (face1 != NULL) hjkhjkhjkhkj (face2 != NULL) hjkhjkhjkhkj (face3 != NULL)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0 hjkhjkhjkhkj face1->getDistSquare() > 0.0 && face0.getDistSquare() > 0.0 hjkhjkhjkhkj face1.getDistSquare() > 0.0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0 hjkhjkhjkhkj face3->getDistSquare() > 0.0)) { && face2.getDistSquare() > 0.0 hjkhjkhjkhkj face3.getDistSquare() > 0.0)) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // 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 // where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin. // tetrahedron that contains the origin.
// Compute the normal of the triangle // Compute the normal of the triangle
vec3 v1 = points[1] - points[0]; Vector3f v1 = points[1] - points[0];
vec3 v2 = points[2] - points[0]; Vector3f v2 = points[2] - points[0];
vec3 n = v1.cross(v2); Vector3f n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData); suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 * suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData); shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData); suppPointsA[4] = shape1.getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 * suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData); shape2.getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = null; TriangleEPA* face0 = null;
TriangleEPA* face1 = null; TriangleEPA* face1 = null;
@ -245,13 +245,13 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
} }
// If the ructed tetrahedron is not correct // If the ructed tetrahedron is not correct
if (!( face0 != null if (!( face0 != null
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1 != null && face1 != null
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2 != null && face2 != null
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3 != null && face3 != null
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face0->getDistSquare() > 0.0 && face0.getDistSquare() > 0.0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face1->getDistSquare() > 0.0 && face1.getDistSquare() > 0.0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face2->getDistSquare() > 0.0 && face2.getDistSquare() > 0.0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj face3->getDistSquare() > 0.0) ) { && face3.getDistSquare() > 0.0) ) {
return; return;
} }
// Associate the edges of neighbouring triangle faces // Associate the edges of neighbouring triangle faces
@ -281,11 +281,11 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
triangle = triangleHeap[0]; triangle = triangleHeap[0];
triangleHeap.popFront(); triangleHeap.popFront();
Log.info("rm from heap:"); Log.info("rm from heap:");
for (sizet iii=0; iii<triangleHeap.size(); ++iii) { for (long iii=0; iii<triangleHeap.size(); ++iii) {
Log.info(" [" << iii << "] " << triangleHeap[iii]->getDistSquare()); Log.info(" [" + iii + "] " + triangleHeap[iii].getDistSquare());
} }
// If the candidate face in the heap is not obsolete // If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) { if (!triangle.getIsObsolete()) {
// If we have reached the maximum number of support points // If we have reached the maximum number of support points
if (nbVertices == MAXSUPPORTPOINTS) { if (nbVertices == MAXSUPPORTPOINTS) {
assert(false); assert(false);
@ -293,28 +293,28 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
} }
// Compute the support point of the Minkowski // Compute the support point of the Minkowski
// difference (A-B) in the closest point direction // difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(triangle->getClosestPoint(), shape1CachedCollisionData); suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint()), shape2CachedCollisionData); suppPointsB[nbVertices] = body2Tobody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * (-triangle.getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices; int indexNewVertex = nbVertices;
nbVertices++; nbVertices++;
// Update the upper bound of the penetration depth // Update the upper bound of the penetration depth
float wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
Log.info(" point=" << points[indexNewVertex]); Log.info(" point=" + points[indexNewVertex]);
Log.info("close point=" << triangle->getClosestPoint()); Log.info("close point=" + triangle.getClosestPoint());
Log.info(" ==>" << wDotv); Log.info(" ==>" + wDotv);
if (wDotv < 0.0) { if (wDotv < 0.0) {
Log.error("depth penetration error " << wDotv); Log.error("depth penetration error " + wDotv);
continue; continue;
} }
EPHYASSERT(wDotv >= 0.0, "depth penetration error " << wDotv); EPHYASSERT(wDotv >= 0.0, "depth penetration error " + wDotv);
float wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); float wDotVSquare = wDotv * wDotv / triangle.getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) { if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare; upperBoundSquarePenDepth = wDotVSquare;
} }
// Compute the error // Compute the error
float error = wDotv - triangle->getDistSquare(); float error = wDotv - triangle.getDistSquare();
if (error <= etk::max(tolerance, RELERRORSQUARE * wDotv) || if (error <= max(tolerance, RELERRORSQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] || points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] || points[indexNewVertex] == points[(*triangle)[1]] ||
points[indexNewVertex] == points[(*triangle)[2]]) { 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 // 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 // face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face. // algorithm from the current triangle face.
sizet i = triangleStore.getNbTriangles(); long i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) { if (!triangle.computeSilhouette(points, indexNewVertex, triangleStore)) {
break; break;
} }
// Add all the new triangle faces computed with the silhouette algorithm // Add all the new triangle faces computed with the silhouette algorithm
@ -336,12 +336,12 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
} }
} }
} while( triangleHeap.size() > 0 } while( triangleHeap.size() > 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); && triangleHeap[0].getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info // Compute the contact info
vector = transform1.getOrientation() * triangle->getClosestPoint(); vector = transform1.getOrientation() * triangle.getClosestPoint();
vec3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
vec3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3f pBLocal = body2Tobody1.getInverse() * triangle.computeClosestPointOfObject(suppPointsB);
vec3 normal = vector.safeNormalized(); Vector3f normal = vector.safeNormalized();
float penetrationDepth = vector.length(); float penetrationDepth = vector.length();
EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0"); EPHYASSERT(penetrationDepth >= 0.0, "penetration depth <0");
if (normal.length2() < FLTEPSILON) { if (normal.length2() < FLTEPSILON) {
@ -349,5 +349,5 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints( Simplex simplex,
} }
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
} }

View File

@ -1,23 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.EPA;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/GJK/Simplex.hpp>
#include <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp>
#include <ephysics/raint/ContactPoint.hpp>
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/debug.hpp>
#include <etk/Set.hpp>
namespace ephysics {
/// Maximum number of support points of the polytope /// Maximum number of support points of the polytope
int MAXSUPPORTPOINTS = 100; int MAXSUPPORTPOINTS = 100;
/// Maximum number of facets of the polytope /// Maximum number of facets of the polytope
@ -44,31 +26,29 @@ namespace ephysics {
EPAAlgorithm operator=( EPAAlgorithm algorithm); EPAAlgorithm operator=( EPAAlgorithm algorithm);
/// Add a triangle face in the candidate triangle heap /// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, void addFaceCandidate(TriangleEPA* triangle,
etk::Set<TriangleEPA*> heap, Set<TriangleEPA*> heap,
float upperBoundSquarePenDepth) { float upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle // If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance // points is internal to the triangle and if the distance
// of the closest point from the origin is at most the // of the closest point from the origin is at most the
// penetration depth upper bound // penetration depth upper bound
if ( triangle->isClosestPointInternalToTriangle() if ( triangle.isClosestPointInternalToTriangle()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj triangle->getDistSquare() <= upperBoundSquarePenDepth) { && triangle.getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates // Add the triangle face to the list of candidates
heap.add(triangle); heap.add(triangle);
Log.info("add in heap:"); Log.info("add in heap:");
for (sizet iii=0; iii<heap.size(); ++iii) { for (long iii=0; iii<heap.size(); ++iii) {
Log.info(" [" << iii << "] " << heap[iii]->getDistSquare()); Log.info(" [" + iii + "] " + heap[iii].getDistSquare());
} }
} }
} }
// Decide if the origin is in the tetrahedron. // Decide if the origin is in the tetrahedron.
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of /// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron /// the vertex that is wrong if the origin is not in the tetrahedron
int isOriginInTetrahedron( vec3 p1, vec3 p2, vec3 p3, vec3 p4) ; int isOriginInTetrahedron( Vector3f p1, Vector3f p2, Vector3f p3, Vector3f p4) ;
public: public:
/// Constructor /// Constructor
EPAAlgorithm(); EPAAlgorithm();
/// Destructor
~EPAAlgorithm();
/// Initalize the algorithm /// Initalize the algorithm
void init() { void init() {
@ -81,10 +61,10 @@ namespace ephysics {
/// the correct penetration depth /// the correct penetration depth
void computePenetrationDepthAndContactPoints( Simplex simplex, void computePenetrationDepthAndContactPoints( Simplex simplex,
CollisionShapeInfo shape1Info, CollisionShapeInfo shape1Info,
etk::Transform3D transform1, Transform3D transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
etk::Transform3D transform2, Transform3D transform2,
vec3 v, Vector3f v,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
}; };
} }

View File

@ -21,19 +21,19 @@ EdgeEPA::EdgeEPA() {
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index): EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index):
this.ownerTriangle(ownerTriangle), this.ownerTriangle(ownerTriangle),
this.index(index) { this.index(index) {
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); assert(index >= 0 && index < 3);
} }
EdgeEPA::EdgeEPA( EdgeEPA obj): EdgeEPA::EdgeEPA( EdgeEPA obj):
this.ownerTriangle(obj.this.ownerTriangle), this.ownerTriangle(obj.ownerTriangle),
this.index(obj.this.index) { this.index(obj.index) {
} }
EdgeEPA::EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj): EdgeEPA::EdgeEPA(EdgeEPA&& obj):
this.ownerTriangle(null) { this.ownerTriangle(null) {
etk::swap(this.ownerTriangle, obj.this.ownerTriangle); swap(this.ownerTriangle, obj.ownerTriangle);
etk::swap(this.index, obj.this.index); swap(this.index, obj.index);
} }
int EdgeEPA::getSourceVertexIndex() { int EdgeEPA::getSourceVertexIndex() {
@ -44,12 +44,12 @@ int EdgeEPA::getTargetVertexIndex() {
return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)]; return (*this.ownerTriangle)[indexOfNextCounterClockwiseEdge(this.index)];
} }
boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex, boolean EdgeEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
TrianglesStore triangleStore) { TrianglesStore triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!this.ownerTriangle->getIsObsolete()) { if (!this.ownerTriangle.getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point
if (!this.ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) { if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
@ -61,12 +61,12 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
return false; return false;
} else { } else {
// The current triangle is visible and therefore obsolete // The current triangle is visible and therefore obsolete
this.ownerTriangle->setIsObsolete(true); this.ownerTriangle.setIsObsolete(true);
int backup = triangleStore.getNbTriangles(); 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, indexNewVertex,
triangleStore)) { triangleStore)) {
this.ownerTriangle->setIsObsolete(false); this.ownerTriangle.setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),
getSourceVertexIndex()); getSourceVertexIndex());
@ -76,10 +76,10 @@ boolean EdgeEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
return true; return true;
} }
return false; 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, indexNewVertex,
triangleStore)) { triangleStore)) {
this.ownerTriangle->setIsObsolete(false); this.ownerTriangle.setIsObsolete(false);
triangleStore.resize(backup); triangleStore.resize(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(), getTargetVertexIndex(),

View File

@ -1,17 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.EPA;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
class TriangleEPA;
class TrianglesStore;
/** /**
* @brief Class EdgeEPA * @brief Class EdgeEPA
* This class represents an edge of the current polytope in the EPA algorithm. * This class represents an edge of the current polytope in the EPA algorithm.
@ -31,7 +19,7 @@ class EdgeEPA {
/// Copy-ructor /// Copy-ructor
EdgeEPA( EdgeEPA obj); EdgeEPA( EdgeEPA obj);
/// Move-ructor /// Move-ructor
EdgeEPA(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj); EdgeEPA(EdgeEPA obj);
/// Return the pointer to the owner triangle /// Return the pointer to the owner triangle
TriangleEPA* getOwnerTriangle() { TriangleEPA* getOwnerTriangle() {
return this.ownerTriangle; return this.ownerTriangle;
@ -45,17 +33,17 @@ class EdgeEPA {
/// Return the index of the target vertex of the edge /// Return the index of the target vertex of the edge
int getTargetVertexIndex() ; int getTargetVertexIndex() ;
/// Execute the recursive silhouette algorithm from this edge /// 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 /// Assignment operator
EdgeEPA operator=( EdgeEPA obj) { EdgeEPA operator=( EdgeEPA obj) {
this.ownerTriangle = obj.this.ownerTriangle; this.ownerTriangle = obj.ownerTriangle;
this.index = obj.this.index; this.index = obj.index;
return *this; return *this;
} }
/// Move operator /// Move operator
EdgeEPA operator=(EdgeEPAhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj obj) { EdgeEPA operator=(EdgeEPA obj) {
etk::swap(this.ownerTriangle, obj.this.ownerTriangle); swap(this.ownerTriangle, obj.ownerTriangle);
etk::swap(this.index, obj.this.index); swap(this.index, obj.index);
return *this; return *this;
} }
}; };

View File

@ -34,10 +34,10 @@ TriangleEPA::~TriangleEPA() {
} }
boolean TriangleEPA::computeClosestPoint( vec3* vertices) { boolean TriangleEPA::computeClosestPoint( Vector3f* vertices) {
vec3 p0 = vertices[this.indicesVertices[0]]; Vector3f p0 = vertices[this.indicesVertices[0]];
vec3 v1 = vertices[this.indicesVertices[1]] - p0; Vector3f v1 = vertices[this.indicesVertices[1]] - p0;
vec3 v2 = vertices[this.indicesVertices[2]] - p0; Vector3f v2 = vertices[this.indicesVertices[2]] - p0;
float v1Dotv1 = v1.dot(v1); float v1Dotv1 = v1.dot(v1);
float v1Dotv2 = v1.dot(v2); float v1Dotv2 = v1.dot(v2);
float v2Dotv2 = v2.dot(v2); float v2Dotv2 = v2.dot(v2);
@ -61,9 +61,9 @@ boolean TriangleEPA::computeClosestPoint( vec3* vertices) {
boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) { boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) {
if ( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() if ( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) { && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex() ) {
edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1; edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
edge1.getOwnerTriangle()->this.adjacentEdges[edge1.getIndex()] = edge0; edge1.getOwnerTriangle().this.adjacentEdges[edge1.getIndex()] = edge0;
return true; return true;
} }
return false; return false;
@ -71,20 +71,20 @@ boolean ephysics::link( EdgeEPA edge0, EdgeEPA edge1) {
void ephysics::halfLink( EdgeEPA edge0, EdgeEPA edge1) { void ephysics::halfLink( EdgeEPA edge0, EdgeEPA edge1) {
assert( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() assert( edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()); && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
edge0.getOwnerTriangle()->this.adjacentEdges[edge0.getIndex()] = edge1; edge0.getOwnerTriangle().this.adjacentEdges[edge0.getIndex()] = edge1;
} }
boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex, boolean TriangleEPA::computeSilhouette( Vector3f* vertices, int indexNewVertex,
TrianglesStore triangleStore) { TrianglesStore triangleStore) {
int first = triangleStore.getNbTriangles(); int first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it // Mark the current triangle as obsolete because it
setIsObsolete(true); setIsObsolete(true);
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring // Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle // triangles of the current triangle
boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
this.adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore); this.adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) { if (result) {
int i,j; int i,j;
@ -92,7 +92,7 @@ boolean TriangleEPA::computeSilhouette( vec3* vertices, int indexNewVertex,
for (i=first, j=triangleStore.getNbTriangles()-1; for (i=first, j=triangleStore.getNbTriangles()-1;
i != triangleStore.getNbTriangles(); j = i++) { i != triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = triangleStore[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))) { if (!link(EdgeEPA(triangle, 0), EdgeEPA(triangleStore[j], 2))) {
return false; return false;
} }

View File

@ -1,16 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.EPA;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
namespace ephysics {
boolean link( EdgeEPA edge0, EdgeEPA edge1); boolean link( EdgeEPA edge0, EdgeEPA edge1);
void halfLink( 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) 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 boolean this.isObsolete; //!< True if the triangle face is visible from the new support point
float this.determinant; //!< Determinant 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.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.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 float this.distSquare; //!< Square distance of the point closest point v to the origin
public: public:
/// Private copy-ructor /// Private copy-ructor
TriangleEPA( TriangleEPA triangle) { TriangleEPA( TriangleEPA triangle) {
this.indicesVertices[0] = triangle.this.indicesVertices[0]; this.indicesVertices[0] = triangle.indicesVertices[0];
this.indicesVertices[1] = triangle.this.indicesVertices[1]; this.indicesVertices[1] = triangle.indicesVertices[1];
this.indicesVertices[2] = triangle.this.indicesVertices[2]; this.indicesVertices[2] = triangle.indicesVertices[2];
this.adjacentEdges[0] = triangle.this.adjacentEdges[0]; this.adjacentEdges[0] = triangle.adjacentEdges[0];
this.adjacentEdges[1] = triangle.this.adjacentEdges[1]; this.adjacentEdges[1] = triangle.adjacentEdges[1];
this.adjacentEdges[2] = triangle.this.adjacentEdges[2]; this.adjacentEdges[2] = triangle.adjacentEdges[2];
this.isObsolete = triangle.this.isObsolete; this.isObsolete = triangle.isObsolete;
this.determinant = triangle.this.determinant; this.determinant = triangle.determinant;
this.closestPoint = triangle.this.closestPoint; this.closestPoint = triangle.closestPoint;
this.lambda1 = triangle.this.lambda1; this.lambda1 = triangle.lambda1;
this.lambda2 = triangle.this.lambda2; this.lambda2 = triangle.lambda2;
this.distSquare = triangle.this.distSquare; this.distSquare = triangle.distSquare;
} }
/// Private assignment operator /// Private assignment operator
TriangleEPA operator=( TriangleEPA triangle) { TriangleEPA operator=( TriangleEPA triangle) {
this.indicesVertices[0] = triangle.this.indicesVertices[0]; this.indicesVertices[0] = triangle.indicesVertices[0];
this.indicesVertices[1] = triangle.this.indicesVertices[1]; this.indicesVertices[1] = triangle.indicesVertices[1];
this.indicesVertices[2] = triangle.this.indicesVertices[2]; this.indicesVertices[2] = triangle.indicesVertices[2];
this.adjacentEdges[0] = triangle.this.adjacentEdges[0]; this.adjacentEdges[0] = triangle.adjacentEdges[0];
this.adjacentEdges[1] = triangle.this.adjacentEdges[1]; this.adjacentEdges[1] = triangle.adjacentEdges[1];
this.adjacentEdges[2] = triangle.this.adjacentEdges[2]; this.adjacentEdges[2] = triangle.adjacentEdges[2];
this.isObsolete = triangle.this.isObsolete; this.isObsolete = triangle.isObsolete;
this.determinant = triangle.this.determinant; this.determinant = triangle.determinant;
this.closestPoint = triangle.this.closestPoint; this.closestPoint = triangle.closestPoint;
this.lambda1 = triangle.this.lambda1; this.lambda1 = triangle.lambda1;
this.lambda2 = triangle.this.lambda2; this.lambda2 = triangle.lambda2;
this.distSquare = triangle.this.distSquare; this.distSquare = triangle.distSquare;
return *this; return *this;
} }
/// Constructor /// Constructor
@ -65,16 +54,14 @@ namespace ephysics {
TriangleEPA(int v1, int v2, int v3); TriangleEPA(int v1, int v2, int v3);
/// Constructor /// Constructor
void set(int v1, int v2, int v3); void set(int v1, int v2, int v3);
/// Destructor
~TriangleEPA();
/// Return an adjacent edge of the triangle /// Return an adjacent edge of the triangle
EdgeEPA getAdjacentEdge(int index) { EdgeEPA getAdjacentEdge(int index) {
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); assert(index >= 0 && index < 3);
return this.adjacentEdges[index]; return this.adjacentEdges[index];
} }
/// Set an adjacent edge of the triangle /// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA edge) { void setAdjacentEdge(int index, EdgeEPA edge) {
assert(index >=0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); assert(index >=0 && index < 3);
this.adjacentEdges[index] = edge; this.adjacentEdges[index] = edge;
} }
/// Return the square distance of the closest point to origin /// Return the square distance of the closest point to origin
@ -90,23 +77,23 @@ namespace ephysics {
return this.isObsolete; return this.isObsolete;
} }
/// Return the point closest to the origin /// Return the point closest to the origin
vec3 getClosestPoint() { Vector3f getClosestPoint() {
return this.closestPoint; return this.closestPoint;
} }
// Return true if the closest point on affine hull is inside the triangle // Return true if the closest point on affine hull is inside the triangle
boolean isClosestPointInternalToTriangle() { 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 /// Return true if the triangle is visible from a given vertex
boolean isVisibleFromVertex( vec3* vertices, int index) { boolean isVisibleFromVertex( Vector3f* vertices, int index) {
vec3 closestToVert = vertices[index] - this.closestPoint; Vector3f closestToVert = vertices[index] - this.closestPoint;
return (this.closestPoint.dot(closestToVert) > 0.0); return (this.closestPoint.dot(closestToVert) > 0.0);
} }
/// Compute the point v closest to the origin of this triangle /// 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 /// Compute the point of an object closest to the origin
vec3 computeClosestPointOfObject( vec3* supportPointsOfObject) { Vector3f computeClosestPointOfObject( Vector3f* supportPointsOfObject) {
vec3 p0 = supportPointsOfObject[this.indicesVertices[0]]; Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) + return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) +
this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - 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 /// 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 /// 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. /// 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 /// Access operator
int operator[](int pos) { int operator[](int pos) {
assert(pos >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj pos <3); assert(pos >= 0 && pos <3);
return this.indicesVertices[pos]; return this.indicesVertices[pos];
} }
/// Link an edge with another one. It means that the current edge of a triangle will /// Link an edge with another one. It means that the current edge of a triangle will

View File

@ -1,28 +1,12 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.EPA;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/EPA/TriangleEPA.hpp>
#include <ephysics/debug.hpp>
#include <etk/Array.hpp>
namespace ephysics {
int MAXTRIANGLES = 200; // Maximum number of triangles int MAXTRIANGLES = 200; // Maximum number of triangles
/** /**
* @brief This class stores several triangles of the polytope in the EPA algorithm. * @brief This class stores several triangles of the polytope in the EPA algorithm.
*/ */
class TrianglesStore { class TrianglesStore {
private: private:
etk::Array<TriangleEPA, MAXTRIANGLES> this.triangles; //!< Triangles Array<TriangleEPA, MAXTRIANGLES> this.triangles; //!< Triangles
/// Private copy-ructor
TrianglesStore( TrianglesStore triangleStore) = delete;
/// Private assignment operator
TrianglesStore operator=( TrianglesStore triangleStore) = delete;
public: public:
/// Constructor /// Constructor
TrianglesStore() = default; TrianglesStore() = default;
@ -31,13 +15,13 @@ namespace ephysics {
this.triangles.clear(); this.triangles.clear();
} }
/// Return the number of triangles /// Return the number of triangles
sizet getNbTriangles() { long getNbTriangles() {
return this.triangles.size(); return this.triangles.size();
} }
/// Set the number of triangles /// Set the number of triangles
void resize(int backup) { void resize(int backup) {
if (backup > this.triangles.size()) { 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); this.triangles.resize(backup);
} }
@ -46,14 +30,14 @@ namespace ephysics {
return this.triangles.back(); return this.triangles.back();
} }
/// Create a new triangle /// 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 we have not reached the maximum number of triangles
if (this.triangles.size() < MAXTRIANGLES) { if (this.triangles.size() < MAXTRIANGLES) {
TriangleEPA tmp(v0, v1, v2); TriangleEPA tmp(v0, v1, v2);
if (!tmp.computeClosestPoint(vertices)) { if (!tmp.computeClosestPoint(vertices)) {
return null; return null;
} }
this.triangles.pushBack(etk::move(tmp)); this.triangles.pushBack(move(tmp));
return this.triangles.back(); return this.triangles.back();
} }
// We are at the limit (internal) // We are at the limit (internal)

View File

@ -26,50 +26,50 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) { NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("GJKAlgorithm::testCollision()"); PROFILE("GJKAlgorithm::testCollision()");
vec3 suppA; // Support point of object A Vector3f suppA; // Support point of object A
vec3 suppB; // Support point of object B Vector3f suppB; // Support point of object B
vec3 w; // Support point of Minkowski difference A-B Vector3f w; // Support point of Minkowski difference A-B
vec3 pA; // Closest point of object A Vector3f pA; // Closest point of object A
vec3 pB; // Closest point of object B Vector3f pB; // Closest point of object B
float vDotw; float vDotw;
float prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape.isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape.isConvex());
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; Transform3D transform1 = shape1Info.shapeToWorldTransform;
etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; Transform3D transform2 = shape2Info.shapeToWorldTransform;
// 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) // 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 // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // 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(); transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects) // 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; float marginSquare = margin * margin;
assert(margin > 0.0); assert(margin > 0.0);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Get the previous point V (last cached separating axis) // 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 // Initialize the upper bound for the square distance
float distSquare = FLTMAX; float distSquare = FLTMAX;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData); suppA = shape1.getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData); suppB = body2Tobody1 * shape2.getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect // 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 // Cache the current separating axis for frame coherence
this.currentOverlappingPair->setCachedSeparatingAxis(v); this.currentOverlappingPair.setCachedSeparatingAxis(v);
// No intersection, we return // No intersection, we return
return; return;
} }
@ -81,17 +81,17 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1.getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return; if (penetrationDepth <= 0.0) return;
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an intersection, therefore we return
return; return;
} }
@ -105,10 +105,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1.getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
@ -117,7 +117,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an intersection, therefore we return
return; return;
} }
@ -130,10 +130,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1.getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
@ -142,7 +142,7 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an intersection, therefore we return
return; return;
} }
@ -161,10 +161,10 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// object with the margins // object with the margins
float dist = sqrt(distSquare); float dist = sqrt(distSquare);
assert(dist > 0.0); assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v); pA = (pA - (shape1.getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); pB = body2Tobody1.getInverse() * (pB + (shape2.getMargin() / dist) * v);
// Compute the contact info // Compute the contact info
vec3 normal = transform1.getOrientation() * (-v.safeNormalized()); Vector3f normal = transform1.getOrientation() * (-v.safeNormalized());
float penetrationDepth = margin - dist; float penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors) // Reject the contact if the penetration depth is negative (due too numerical errors)
@ -173,11 +173,11 @@ void GJKAlgorithm::testCollision( CollisionShapeInfo shape1Info,
// Create the contact info object // Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB); shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return // There is an intersection, therefore we return
return; return;
} }
} while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * } while(!simplex.isFull() && distSquare > FLTEPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm // The objects (without margins) intersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains // 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, void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
etk::Transform3D transform1, Transform3D transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
etk::Transform3D transform2, Transform3D transform2,
NarrowPhaseCallback* narrowPhaseCallback, NarrowPhaseCallback* narrowPhaseCallback,
vec3 v) { Vector3f v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex; Simplex simplex;
vec3 suppA; Vector3f suppA;
vec3 suppB; Vector3f suppB;
vec3 w; Vector3f w;
float vDotw; float vDotw;
float distSquare = FLTMAX; float distSquare = FLTMAX;
float prevDistSquare; float prevDistSquare;
assert(shape1Info.collisionShape->isConvex()); assert(shape1Info.collisionShape.isConvex());
assert(shape2Info.collisionShape->isConvex()); assert(shape2Info.collisionShape.isConvex());
ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape); ConvexShape* shape1 = staticcast< ConvexShape*>(shape1Info.collisionShape);
ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape); ConvexShape* shape2 = staticcast< ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.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) // 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 // 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(); transform1.getOrientation().getMatrix();
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData); suppA = shape1.getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData); suppB = body2ToBody1 * shape2.getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
vDotw = v.dot(w); vDotw = v.dot(w);
@ -239,7 +239,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo
if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= FLTEPSILON * prevDistSquare) {
return; return;
} }
} while(!simplex.isFull() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * } while(!simplex.isFull() && distSquare > FLTEPSILON *
simplex.getMaxLengthSquareOfAPoint()); simplex.getMaxLengthSquareOfAPoint());
// Give the simplex computed with GJK algorithm to the EPA algorithm // Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points // which will compute the correct penetration depth and contact points
@ -249,24 +249,24 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects( CollisionShapeInfo
v, narrowPhaseCallback); v, narrowPhaseCallback);
} }
boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { boolean GJKAlgorithm::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
vec3 suppA; // Support point of object A Vector3f suppA; // Support point of object A
vec3 w; // Support point of Minkowski difference A-B Vector3f w; // Support point of Minkowski difference A-B
float prevDistSquare; float prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape.getCollisionShape().isConvex());
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape()); ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
// Support point of object B (object B is a single point) // Support point of object B (object B is a single point)
vec3 suppB(localPoint); Vector3f suppB(localPoint);
// Create a simplex set // Create a simplex set
Simplex simplex; Simplex simplex;
// Initial supporting direction // Initial supporting direction
vec3 v(1, 1, 1); Vector3f v(1, 1, 1);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
float distSquare = FLTMAX; float distSquare = FLTMAX;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData); suppA = shape.getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
w = suppA - suppB; w = suppA - suppB;
// Add the new support point to the simplex // Add the new support point to the simplex
@ -288,38 +288,38 @@ boolean GJKAlgorithm::testPointInside( vec3 localPoint, ProxyShape* proxyShape)
return false; return false;
} }
} while( !simplex.isFull() } while( !simplex.isFull()
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint()); && distSquare > FLTEPSILON * simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape // The point is inside the collision shape
return true; return true;
} }
boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) { boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo raycastInfo) {
assert(proxyShape->getCollisionShape()->isConvex()); assert(proxyShape.getCollisionShape().isConvex());
ConvexShape* shape = staticcast< ConvexShape*>(proxyShape->getCollisionShape()); ConvexShape* shape = staticcast< ConvexShape*>(proxyShape.getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData(); void** shapeCachedCollisionData = proxyShape.getCachedCollisionData();
vec3 suppA; // Current lower bound point on the ray (starting at ray's origin) Vector3f suppA; // Current lower bound point on the ray (starting at ray's origin)
vec3 suppB; // Support point on the collision shape Vector3f suppB; // Support point on the collision shape
float machineEpsilonSquare = FLTEPSILON * FLTEPSILON; float machineEpsilonSquare = FLTEPSILON * FLTEPSILON;
float epsilon = float(0.0001); float epsilon = float(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape // 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 the points of the segment are two close, return no hit
if (rayDirection.length2() < machineEpsilonSquare) return false; if (rayDirection.length2() < machineEpsilonSquare) return false;
vec3 w; Vector3f w;
// Create a simplex set // Create a simplex set
Simplex simplex; 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; float lambda = 0.0f;
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin) suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData); suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
vec3 v = suppA - suppB; Vector3f v = suppA - suppB;
float vDotW, vDotR; float vDotW, vDotR;
float distSquare = v.length2(); float distSquare = v.length2();
int nbIterations = 0; int nbIterations = 0;
// GJK Algorithm loop // GJK Algorithm loop
while (distSquare > epsilon hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj nbIterations < MAXITERATIONSGJKRAYCAST) { while (distSquare > epsilon && nbIterations < MAXITERATIONSGJKRAYCAST) {
// Compute the support points // Compute the support points
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData); suppB = shape.getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
w = suppA - suppB; w = suppA - suppB;
vDotW = v.dot(w); vDotW = v.dot(w);
if (vDotW > float(0)) { if (vDotW > float(0)) {
@ -353,18 +353,18 @@ boolean GJKAlgorithm::raycast( Ray ray, ProxyShape* proxyShape, RaycastInfo rayc
return false; return false;
} }
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
vec3 pointA; Vector3f pointA;
vec3 pointB; Vector3f pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB); simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info // A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda; raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = pointB; raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid if (n.length2() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = n; raycastInfo.worldNormal = n;
} else { // Degenerated normal vector, we return a zero normal vector } 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; return true;
} }

View File

@ -1,19 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.GJK;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
#include <ephysics/raint/ContactPoint.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp>
namespace ephysics {
float RELERROR = float(1.0e-3); float RELERROR = float(1.0e-3);
float RELERRORSQUARE = RELERROR * RELERROR; float RELERRORSQUARE = RELERROR * RELERROR;
int MAXITERATIONSGJKRAYCAST = 32; int MAXITERATIONSGJKRAYCAST = 32;
@ -32,7 +18,7 @@ namespace ephysics {
* Polytope Algorithm) to compute the correct penetration depth between the * Polytope Algorithm) to compute the correct penetration depth between the
* enlarged objects. * enlarged objects.
*/ */
class GJKAlgorithm : public NarrowPhaseAlgorithm { class GJKAlgorithm extends NarrowPhaseAlgorithm {
private : private :
EPAAlgorithm this.algoEPA; //!< EPA Algorithm EPAAlgorithm this.algoEPA; //!< EPA Algorithm
/// Private copy-ructor /// Private copy-ructor
@ -45,18 +31,16 @@ namespace ephysics {
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// 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. /// compute the correct penetration depth and contact points of the enlarged objects.
void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info, void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
etk::Transform3D transform1, Transform3D transform1,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
etk::Transform3D transform2, Transform3D transform2,
NarrowPhaseCallback* narrowPhaseCallback, NarrowPhaseCallback* narrowPhaseCallback,
vec3 v); Vector3f v);
public : public :
/// Constructor /// Constructor
GJKAlgorithm(); GJKAlgorithm();
/// Destructor
~GJKAlgorithm();
/// Initalize the algorithm /// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection) { void init(CollisionDetection* collisionDetection) {
NarrowPhaseAlgorithm::init(collisionDetection); NarrowPhaseAlgorithm::init(collisionDetection);
this.algoEPA.init(); this.algoEPA.init();
}; };
@ -69,11 +53,11 @@ namespace ephysics {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the /// 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 /// 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. /// the correct penetration depth and contact points between the enlarged objects.
virtual void testCollision( CollisionShapeInfo shape1Info, void testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback); NarrowPhaseCallback* narrowPhaseCallback);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape /// 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 /// 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 /// 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". /// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".

View File

@ -18,7 +18,7 @@ Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) {
/// suppPointA : support point of object A in a direction -v /// suppPointA : support point of object A in a direction -v
/// suppPointB : support point of object B in a direction v /// suppPointB : support point of object B in a direction v
/// point : support point of object (A-B) => point = suppPointA - suppPointB /// 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()); assert(!isFull());
mLastFound = 0; mLastFound = 0;
@ -28,7 +28,7 @@ void Simplex::addPoint( vec3 point, vec3 suppPointA, vec3 suppPointB) {
// the current simplex // the current simplex
while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { while (overlap(mBitsCurrentSimplex, mLastFoundBit)) {
mLastFound++; mLastFound++;
mLastFoundBit <<= 1; mLastFoundBit += 1;
} }
assert(mLastFound < 4); 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 // Return true if the point is in the simplex
boolean Simplex::isPointInSimplex( vec3 point) { boolean Simplex::isPointInSimplex( Vector3f point) {
int i; int i;
Bits bit; Bits bit;
// For each four possible points in the simplex // 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 // 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; return false;
@ -69,7 +69,7 @@ void Simplex::updateCache() {
Bits bit; Bits bit;
// For each of the four possible points of the simplex // 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 the current points is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
@ -86,22 +86,22 @@ void Simplex::updateCache() {
} }
// Return the points of the simplex // Return the points of the simplex
int Simplex::getSimplex(vec3* suppPointsA, vec3* suppPointsB, int Simplex::getSimplex(Vector3f* suppPointsA, Vector3f* suppPointsB,
vec3* points) { Vector3f* points) {
int nbVertices = 0; int nbVertices = 0;
int i; int i;
Bits bit; Bits bit;
// For each four point in the possible simplex // 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 the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
// Store the points // Store the points
suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; suppPointsA[nbVertices] = this.mSuppPointsA[nbVertices];
suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; suppPointsB[nbVertices] = this.mSuppPointsB[nbVertices];
points[nbVertices] = this->mPoints[nbVertices]; points[nbVertices] = this.mPoints[nbVertices];
nbVertices++; nbVertices++;
} }
@ -121,7 +121,7 @@ void Simplex::computeDeterminants() {
Bits bitI; Bits bitI;
// For each possible four points in the simplex set // 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 the current point is in the simplex
if (overlap(mBitsCurrentSimplex, bitI)) { if (overlap(mBitsCurrentSimplex, bitI)) {
@ -134,7 +134,7 @@ void Simplex::computeDeterminants() {
int j; int j;
Bits bitJ; Bits bitJ;
for (j=0, bitJ = 0x1; j<i; j++, bitJ <<= 1) { for (j=0, bitJ = 0x1; j<i; j++, bitJ += 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) { if (overlap(mBitsCurrentSimplex, bitJ)) {
int k; int k;
Bits bit3 = bitJ | bit2; Bits bit3 = bitJ | bit2;
@ -202,8 +202,8 @@ boolean Simplex::isProperSubset(Bits subset) {
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<=1) { for (i=0, bit=0x1; i<4; i++, bit +=1) {
if (overlap(subset, bit) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDet[subset][i] <= 0.0) { if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
return false; return false;
} }
} }
@ -220,7 +220,7 @@ boolean Simplex::isAffinelyDependent() {
Bits bit; Bits bit;
// For each four point of the possible simplex set // For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit += 1) {
if (overlap(mAllBits, bit)) { if (overlap(mAllBits, bit)) {
sum += mDet[mAllBits][i]; sum += mDet[mAllBits][i];
} }
@ -238,7 +238,7 @@ boolean Simplex::isValidSubset(Bits subset) {
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) { for (i=0, bit=0x1; i<4; i++, bit += 1) {
if (overlap(mAllBits, bit)) { if (overlap(mAllBits, bit)) {
// If the current point is in the subset // If the current point is in the subset
if (overlap(subset, bit)) { if (overlap(subset, bit)) {
@ -265,7 +265,7 @@ boolean Simplex::isValidSubset(Bits subset) {
/// pA = sum(lambdai * ai) where "ai" are the support points of object A /// pA = sum(lambdai * ai) where "ai" are the support points of object A
/// pB = sum(lambdai * bi) where "bi" are the support points of object B /// pB = sum(lambdai * bi) where "bi" are the support points of object B
/// with lambdai = deltaXi / deltaX /// with lambdai = deltaXi / deltaX
void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) { void Simplex::computeClosestPointsOfAandB(Vector3f pA, Vector3f pB) {
float deltaX = 0.0; float deltaX = 0.0;
pA.setValue(0.0, 0.0, 0.0); pA.setValue(0.0, 0.0, 0.0);
pB.setValue(0.0, 0.0, 0.0); pB.setValue(0.0, 0.0, 0.0);
@ -273,7 +273,7 @@ void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) {
Bits bit; Bits bit;
// For each four points in the possible simplex set // For each four points in the possible simplex set
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 part of the simplex // If the current point is part of the simplex
if (overlap(mBitsCurrentSimplex, bit)) { if (overlap(mBitsCurrentSimplex, bit)) {
deltaX += mDet[mBitsCurrentSimplex][i]; deltaX += mDet[mBitsCurrentSimplex][i];
@ -292,14 +292,14 @@ void Simplex::computeClosestPointsOfAandB(vec3 pA, vec3 pB) {
/// This method executes the Jonhnson's algorithm for computing the point /// This method executes the Jonhnson's algorithm for computing the point
/// "v" of simplex that is closest to the origin. The method returns true /// "v" of simplex that is closest to the origin. The method returns true
/// if a closest point has been found. /// if a closest point has been found.
boolean Simplex::computeClosestPoint(vec3 v) { boolean Simplex::computeClosestPoint(Vector3f v) {
Bits subset; Bits subset;
// For each possible simplex set // For each possible simplex set
for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) { for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) {
// If the simplex is a subset of the current simplex and is valid for the Johnson's // If the simplex is a subset of the current simplex and is valid for the Johnson's
// algorithm test // algorithm test
if (isSubset(subset, mBitsCurrentSimplex) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isValidSubset(subset | mLastFoundBit)) { if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) {
mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex
v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex
return true; return true;
@ -319,13 +319,13 @@ boolean Simplex::computeClosestPoint(vec3 v) {
} }
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(vec3 v) { void Simplex::backupClosestPointInSimplex(Vector3f v) {
float minDistSquare = FLTMAX; float minDistSquare = FLTMAX;
Bits bit; Bits bit;
for (bit = mAllBits; bit != 0x0; bit--) { for (bit = mAllBits; bit != 0x0; bit--) {
if (isSubset(bit, mAllBits) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isProperSubset(bit)) { if (isSubset(bit, mAllBits) && isProperSubset(bit)) {
vec3 u = computeClosestPointForSubset(bit); Vector3f u = computeClosestPointForSubset(bit);
float distSquare = u.dot(u); float distSquare = u.dot(u);
if (distSquare < minDistSquare) { if (distSquare < minDistSquare) {
minDistSquare = distSquare; minDistSquare = distSquare;
@ -338,15 +338,15 @@ void Simplex::backupClosestPointInSimplex(vec3 v) {
// Return the closest point "v" in the convex hull of the points in the subset // Return the closest point "v" in the convex hull of the points in the subset
// represented by the bits "subset" // represented by the bits "subset"
vec3 Simplex::computeClosestPointForSubset(Bits subset) { Vector3f Simplex::computeClosestPointForSubset(Bits subset) {
vec3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambdai * points[i]) Vector3f v(0.0, 0.0, 0.0); // Closet point v = sum(lambdai * points[i])
mMaxLengthSquare = 0.0; mMaxLengthSquare = 0.0;
float deltaX = 0.0; // deltaX = sum of all det[subset][i] float deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i; int i;
Bits bit; Bits bit;
// For each four point in the possible simplex set // For each four point in the possible simplex set
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 subset // If the current point is in the subset
if (overlap(subset, bit)) { if (overlap(subset, bit)) {
// deltaX = sum of all det[subset][i] // deltaX = sum of all det[subset][i]

View File

@ -1,22 +1,4 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase.GJK;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
#include <etk/Vector.hpp>
/// ReactPhysics3D namespace
namespace ephysics {
// Type definitions
typedef int Bits;
// Class Simplex // Class Simplex
/** /**
@ -34,7 +16,7 @@ class Simplex {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Current points /// Current points
vec3 mPoints[4]; Vector3f mPoints[4];
/// pointsLengthSquare[i] = (points[i].length)^2 /// pointsLengthSquare[i] = (points[i].length)^2
float mPointsLengthSquare[4]; float mPointsLengthSquare[4];
@ -43,13 +25,13 @@ class Simplex {
float mMaxLengthSquare; float mMaxLengthSquare;
/// Support points of object A in local coordinates /// Support points of object A in local coordinates
vec3 mSuppPointsA[4]; Vector3f mSuppPointsA[4];
/// Support points of object B in local coordinates /// Support points of object B in local coordinates
vec3 mSuppPointsB[4]; Vector3f mSuppPointsB[4];
/// diff[i][j] contains points[i] - points[j] /// diff[i][j] contains points[i] - points[j]
vec3 mDiffLength[4][4]; Vector3f mDiffLength[4][4];
/// Cached determinant values /// Cached determinant values
float mDet[16][4]; float mDet[16][4];
@ -59,16 +41,16 @@ class Simplex {
/// 4 bits that identify the current points of the 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 /// 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 /// 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) /// Position of the last found support point (lastFoundBit = 0x1 + lastFound)
Bits mLastFoundBit; int mLastFoundBit;
/// allBits = bitsCurrentSimplex | lastFoundBit; /// allBits = bitsCurrentSimplex | lastFoundBit;
Bits mAllBits; int mAllBits;
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -97,7 +79,7 @@ class Simplex {
void computeDeterminants(); void computeDeterminants();
/// Return the closest point "v" in the convex hull of a subset of points /// Return the closest point "v" in the convex hull of a subset of points
vec3 computeClosestPointForSubset(Bits subset); Vector3f computeClosestPointForSubset(Bits subset);
public: public:
@ -113,29 +95,29 @@ class Simplex {
boolean isEmpty() ; boolean isEmpty() ;
/// Return the points of the simplex /// Return the points of the simplex
int getSimplex(vec3* mSuppPointsA, vec3* mSuppPointsB, int getSimplex(Vector3f* mSuppPointsA, Vector3f* mSuppPointsB,
vec3* mPoints) ; Vector3f* mPoints) ;
/// Return the maximum squared length of a point /// Return the maximum squared length of a point
float getMaxLengthSquareOfAPoint() ; float getMaxLengthSquareOfAPoint() ;
/// Add a new support point of (A-B) into the simplex. /// 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 /// 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 /// Return true if the set is affinely dependent
boolean isAffinelyDependent() ; boolean isAffinelyDependent() ;
/// Backup the closest point /// Backup the closest point
void backupClosestPointInSimplex(vec3 point); void backupClosestPointInSimplex(Vector3f point);
/// Compute the closest points "pA" and "pB" of object A and B. /// 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. /// 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" // Return true if some bits of "a" overlap with bits of "b"

View File

@ -1,21 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.narrowphase;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/Body.hpp>
#include <ephysics/raint/ContactPoint.hpp>
#include <ephysics/engine/OverlappingPair.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp>
namespace ephysics {
class CollisionDetection;
/** /**
* @brief It is the base class for a narrow-phase collision * @brief It is the base class for a narrow-phase collision
@ -23,9 +7,8 @@ namespace ephysics {
*/ */
class NarrowPhaseCallback { class NarrowPhaseCallback {
public: public:
virtual ~NarrowPhaseCallback() = default;
/// Called by a narrow-phase collision algorithm when a new contact has been found /// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, void notifyContact(OverlappingPair* overlappingPair,
ContactPointInfo contactInfo) = 0; ContactPointInfo contactInfo) = 0;
}; };
@ -39,21 +22,15 @@ namespace ephysics {
protected : protected :
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision 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 : public :
/// Constructor /// Constructor
NarrowPhaseAlgorithm(); NarrowPhaseAlgorithm();
/// Destructor
virtual ~NarrowPhaseAlgorithm() = default;
/// Initalize the algorithm /// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection); void init(CollisionDetection* collisionDetection);
/// Set the current overlapping pair of bodies /// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair); void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Compute a contact info if the two bounding volume collide /// Compute a contact info if the two bounding volume collide
virtual void testCollision( CollisionShapeInfo shape1Info, void testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info, CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) = 0; NarrowPhaseCallback* narrowPhaseCallback) = 0;
}; };

View File

@ -21,20 +21,20 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI
ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape); ephysics::SphereShape* sphereShape1 = staticcast< ephysics::SphereShape*>(shape1Info.collisionShape);
ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape); ephysics::SphereShape* sphereShape2 = staticcast< ephysics::SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms // Get the local-space to world-space transforms
etk::Transform3D transform1 = shape1Info.shapeToWorldTransform; Transform3D transform1 = shape1Info.shapeToWorldTransform;
etk::Transform3D transform2 = shape2Info.shapeToWorldTransform; Transform3D transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers // Compute the distance between the centers
vec3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition(); Vector3f vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
float squaredDistanceBetweenCenters = vectorBetweenCenters.length2(); float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
// Compute the sum of the radius // 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 the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) { if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
vec3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition(); Vector3f centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
vec3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition(); Vector3f centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
vec3 intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.safeNormalized(); Vector3f intersectionOnBody1 = sphereShape1.getRadius() * centerSphere2InBody1LocalSpace.safeNormalized();
vec3 intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.safeNormalized(); Vector3f intersectionOnBody2 = sphereShape2.getRadius() * centerSphere1InBody2LocalSpace.safeNormalized();
float penetrationDepth = sumRadius - etk::sqrt(squaredDistanceBetweenCenters); float penetrationDepth = sumRadius - sqrt(squaredDistanceBetweenCenters);
// Create the contact info object // Create the contact info object
ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape, ephysics::ContactPointInfo contactInfo(shape1Info.proxyShape,
@ -46,6 +46,6 @@ void ephysics::SphereVsSphereAlgorithm::testCollision( ephysics::CollisionShapeI
intersectionOnBody1, intersectionOnBody1,
intersectionOnBody2); intersectionOnBody2);
// Notify about the new contact // Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); narrowPhaseCallback.notifyContact(shape1Info.overlappingPair, contactInfo);
} }
} }

View File

@ -1,42 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/Body.hpp>
#include <ephysics/raint/ContactPoint.hpp>
#include <ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp>
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);
};
}

View File

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

View File

@ -20,50 +20,50 @@ AABB::AABB():
} }
AABB::AABB( vec3 minCoordinates, vec3 maxCoordinates): AABB::AABB( Vector3f minCoordinates, Vector3f maxCoordinates):
this.minCoordinates(minCoordinates), this.minCoordinates(minCoordinates),
this.maxCoordinates(maxCoordinates) { this.maxCoordinates(maxCoordinates) {
} }
AABB::AABB( AABB aabb): AABB::AABB( AABB aabb):
this.minCoordinates(aabb.this.minCoordinates), this.minCoordinates(aabb.minCoordinates),
this.maxCoordinates(aabb.this.maxCoordinates) { this.maxCoordinates(aabb.maxCoordinates) {
} }
void AABB::mergeWithAABB( AABB aabb) { void AABB::mergeWithAABB( AABB aabb) {
this.minCoordinates.setX(etk::min(this.minCoordinates.x(), aabb.this.minCoordinates.x())); this.minCoordinates.setX(min(this.minCoordinates.x(), aabb.minCoordinates.x()));
this.minCoordinates.setY(etk::min(this.minCoordinates.y(), aabb.this.minCoordinates.y())); this.minCoordinates.setY(min(this.minCoordinates.y(), aabb.minCoordinates.y()));
this.minCoordinates.setZ(etk::min(this.minCoordinates.z(), aabb.this.minCoordinates.z())); this.minCoordinates.setZ(min(this.minCoordinates.z(), aabb.minCoordinates.z()));
this.maxCoordinates.setX(etk::max(this.maxCoordinates.x(), aabb.this.maxCoordinates.x())); this.maxCoordinates.setX(max(this.maxCoordinates.x(), aabb.maxCoordinates.x()));
this.maxCoordinates.setY(etk::max(this.maxCoordinates.y(), aabb.this.maxCoordinates.y())); this.maxCoordinates.setY(max(this.maxCoordinates.y(), aabb.maxCoordinates.y()));
this.maxCoordinates.setZ(etk::max(this.maxCoordinates.z(), aabb.this.maxCoordinates.z())); this.maxCoordinates.setZ(max(this.maxCoordinates.z(), aabb.maxCoordinates.z()));
} }
void AABB::mergeTwoAABBs( AABB aabb1, AABB aabb2) { void AABB::mergeTwoAABBs( AABB aabb1, AABB aabb2) {
this.minCoordinates.setX(etk::min(aabb1.this.minCoordinates.x(), aabb2.this.minCoordinates.x())); this.minCoordinates.setX(min(aabb1.minCoordinates.x(), aabb2.minCoordinates.x()));
this.minCoordinates.setY(etk::min(aabb1.this.minCoordinates.y(), aabb2.this.minCoordinates.y())); this.minCoordinates.setY(min(aabb1.minCoordinates.y(), aabb2.minCoordinates.y()));
this.minCoordinates.setZ(etk::min(aabb1.this.minCoordinates.z(), aabb2.this.minCoordinates.z())); this.minCoordinates.setZ(min(aabb1.minCoordinates.z(), aabb2.minCoordinates.z()));
this.maxCoordinates.setX(etk::max(aabb1.this.maxCoordinates.x(), aabb2.this.maxCoordinates.x())); this.maxCoordinates.setX(max(aabb1.maxCoordinates.x(), aabb2.maxCoordinates.x()));
this.maxCoordinates.setY(etk::max(aabb1.this.maxCoordinates.y(), aabb2.this.maxCoordinates.y())); this.maxCoordinates.setY(max(aabb1.maxCoordinates.y(), aabb2.maxCoordinates.y()));
this.maxCoordinates.setZ(etk::max(aabb1.this.maxCoordinates.z(), aabb2.this.maxCoordinates.z())); this.maxCoordinates.setZ(max(aabb1.maxCoordinates.z(), aabb2.maxCoordinates.z()));
} }
boolean AABB::contains( AABB aabb) { boolean AABB::contains( AABB aabb) {
boolean isInside = true; boolean isInside = true;
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.x() <= aabb.this.minCoordinates.x(); isInside = isInside && this.minCoordinates.x() <= aabb.minCoordinates.x();
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.y() <= aabb.this.minCoordinates.y(); isInside = isInside && this.minCoordinates.y() <= aabb.minCoordinates.y();
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.minCoordinates.z() <= aabb.this.minCoordinates.z(); isInside = isInside && this.minCoordinates.z() <= aabb.minCoordinates.z();
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.x() >= aabb.this.maxCoordinates.x(); isInside = isInside && this.maxCoordinates.x() >= aabb.maxCoordinates.x();
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.y() >= aabb.this.maxCoordinates.y(); isInside = isInside && this.maxCoordinates.y() >= aabb.maxCoordinates.y();
isInside = isInside hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.maxCoordinates.z() >= aabb.this.maxCoordinates.z(); isInside = isInside && this.maxCoordinates.z() >= aabb.maxCoordinates.z();
return isInside; return isInside;
} }
AABB AABB::createAABBForTriangle( vec3* trianglePoints) { AABB AABB::createAABBForTriangle( Vector3f* trianglePoints) {
vec3 minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z()); Vector3f minCoords(trianglePoints[0].x(), trianglePoints[0].y(), trianglePoints[0].z());
vec3 maxCoords(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()) { if (trianglePoints[1].x() < minCoords.x()) {
minCoords.setX(trianglePoints[1].x()); minCoords.setX(trianglePoints[1].x());
} }
@ -104,21 +104,21 @@ AABB AABB::createAABBForTriangle( vec3* trianglePoints) {
} }
boolean AABB::testRayIntersect( Ray ray) { boolean AABB::testRayIntersect( Ray ray) {
vec3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); Vector3f point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
vec3 e = this.maxCoordinates - this.minCoordinates; Vector3f e = this.maxCoordinates - this.minCoordinates;
vec3 d = point2 - ray.point1; Vector3f d = point2 - ray.point1;
vec3 m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates; Vector3f m = ray.point1 + point2 - this.minCoordinates - this.maxCoordinates;
// Test if the AABB face normals are separating axis // Test if the AABB face normals are separating axis
float adx = etk::abs(d.x()); float adx = abs(d.x());
if (etk::abs(m.x()) > e.x() + adx) { if (abs(m.x()) > e.x() + adx) {
return false; return false;
} }
float ady = etk::abs(d.y()); float ady = abs(d.y());
if (etk::abs(m.y()) > e.y() + ady) { if (abs(m.y()) > e.y() + ady) {
return false; return false;
} }
float adz = etk::abs(d.z()); float adz = abs(d.z());
if (etk::abs(m.z()) > e.z() + adz) { if (abs(m.z()) > e.z() + adz) {
return false; return false;
} }
// Add in an epsilon term to counteract arithmetic errors when segment is // Add in an epsilon term to counteract arithmetic errors when segment is
@ -129,50 +129,50 @@ boolean AABB::testRayIntersect( Ray ray) {
adz += epsilon; adz += epsilon;
// Test if the cross products between face normals and ray direction are // Test if the cross products between face normals and ray direction are
// separating axis // 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; 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; 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; return false;
} }
// No separating axis has been found // No separating axis has been found
return true; return true;
} }
vec3 AABB::getExtent() { Vector3f AABB::getExtent() {
return this.maxCoordinates - this.minCoordinates; return this.maxCoordinates - this.minCoordinates;
} }
void AABB::inflate(float dx, float dy, float dz) { void AABB::inflate(float dx, float dy, float dz) {
this.maxCoordinates += vec3(dx, dy, dz); this.maxCoordinates += Vector3f(dx, dy, dz);
this.minCoordinates -= vec3(dx, dy, dz); this.minCoordinates -= Vector3f(dx, dy, dz);
} }
boolean AABB::testCollision( AABB aabb) { boolean AABB::testCollision( AABB aabb) {
if ( this.maxCoordinates.x() < aabb.this.minCoordinates.x() if ( this.maxCoordinates.x() < aabb.minCoordinates.x()
|| aabb.this.maxCoordinates.x() < this.minCoordinates.x()) { || aabb.maxCoordinates.x() < this.minCoordinates.x()) {
return false; return false;
} }
if ( this.maxCoordinates.y() < aabb.this.minCoordinates.y() if ( this.maxCoordinates.y() < aabb.minCoordinates.y()
|| aabb.this.maxCoordinates.y() < this.minCoordinates.y()) { || aabb.maxCoordinates.y() < this.minCoordinates.y()) {
return false; return false;
} }
if ( this.maxCoordinates.z() < aabb.this.minCoordinates.z() if ( this.maxCoordinates.z() < aabb.minCoordinates.z()
|| aabb.this.maxCoordinates.z() < this.minCoordinates.z()) { || aabb.maxCoordinates.z() < this.minCoordinates.z()) {
return false; return false;
} }
return true; return true;
} }
float AABB::getVolume() { float AABB::getVolume() {
vec3 diff = this.maxCoordinates - this.minCoordinates; Vector3f diff = this.maxCoordinates - this.minCoordinates;
return (diff.x() * diff.y() * diff.z()); 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()) { if (min3(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) {
return false; return false;
} }
@ -194,16 +194,16 @@ boolean AABB::testCollisionTriangleAABB( vec3* trianglePoints) {
return true; return true;
} }
boolean AABB::contains( vec3 point) { boolean AABB::contains( Vector3f point) {
return point.x() >= this.minCoordinates.x() - FLTEPSILON hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.x() <= this.maxCoordinates.x() + FLTEPSILON return point.x() >= this.minCoordinates.x() - FLTEPSILON && point.x() <= this.maxCoordinates.x() + FLTEPSILON
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj point.y() >= this.minCoordinates.y() - FLTEPSILON hjkhjkhjkhkj point.y() <= this.maxCoordinates.y() + FLTEPSILON && 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; && point.z() >= this.minCoordinates.z() - FLTEPSILON hjkhjkhjkhkj point.z() <= this.maxCoordinates.z() + FLTEPSILON;
} }
AABB AABB::operator=( AABB aabb) { AABB AABB::operator=( AABB aabb) {
if (this != aabb) { if (this != aabb) {
this.minCoordinates = aabb.this.minCoordinates; this.minCoordinates = aabb.minCoordinates;
this.maxCoordinates = aabb.this.maxCoordinates; this.maxCoordinates = aabb.maxCoordinates;
} }
return *this; return *this;
} }

View File

@ -1,14 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/** /**
* @brief Represents a bounding volume of type "Axis Aligned * @brief Represents a bounding volume of type "Axis Aligned
* Bounding Box". It's a box where all the edges are always aligned * Bounding Box". It's a box where all the edges are always aligned
@ -18,9 +9,9 @@ namespace ephysics {
class AABB { class AABB {
private : private :
/// Minimum world coordinates of the AABB on the x,y and z axis /// 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 /// Maximum world coordinates of the AABB on the x,y and z axis
vec3 this.maxCoordinates; Vector3f maxCoordinates;
public : public :
/** /**
* @brief default contructor * @brief default contructor
@ -31,7 +22,7 @@ namespace ephysics {
* @param[in] minCoordinates Minimum coordinates * @param[in] minCoordinates Minimum coordinates
* @param[in] maxCoordinates Maximum coordinates * @param[in] maxCoordinates Maximum coordinates
*/ */
AABB( vec3 minCoordinates, vec3 maxCoordinates); AABB( Vector3f minCoordinates, Vector3f maxCoordinates);
/** /**
* @brief Copy-contructor * @brief Copy-contructor
* @param[in] aabb the object to copy * @param[in] aabb the object to copy
@ -41,42 +32,42 @@ namespace ephysics {
* @brief Get the center point of the AABB box * @brief Get the center point of the AABB box
* @return The 3D position of the center * @return The 3D position of the center
*/ */
vec3 getCenter() { Vector3f getCenter() {
return (this.minCoordinates + this.maxCoordinates) * 0.5f; return (this.minCoordinates + this.maxCoordinates) * 0.5f;
} }
/** /**
* @brief Get the minimum coordinates of the AABB * @brief Get the minimum coordinates of the AABB
* @return The 3d minimum coordonates * @return The 3d minimum coordonates
*/ */
vec3 getMin() { Vector3f getMin() {
return this.minCoordinates; return this.minCoordinates;
} }
/** /**
* @brief Set the minimum coordinates of the AABB * @brief Set the minimum coordinates of the AABB
* @param[in] min The 3d minimum coordonates * @param[in] min The 3d minimum coordonates
*/ */
void setMin( vec3 min) { void setMin( Vector3f min) {
this.minCoordinates = min; this.minCoordinates = min;
} }
/** /**
* @brief Return the maximum coordinates of the AABB * @brief Return the maximum coordinates of the AABB
* @return The 3d maximum coordonates * @return The 3d maximum coordonates
*/ */
vec3 getMax() { Vector3f getMax() {
return this.maxCoordinates; return this.maxCoordinates;
} }
/** /**
* @brief Set the maximum coordinates of the AABB * @brief Set the maximum coordinates of the AABB
* @param[in] max The 3d maximum coordonates * @param[in] max The 3d maximum coordonates
*/ */
void setMax( vec3 max) { void setMax( Vector3f max) {
this.maxCoordinates = max; this.maxCoordinates = max;
} }
/** /**
* @brief Get the size of the AABB in the three dimension x, y and z * @brief Get the size of the AABB in the three dimension x, y and z
* @return the AABB 3D size * @return the AABB 3D size
*/ */
vec3 getExtent() ; Vector3f getExtent() ;
/** /**
* @brief Inflate each side of the AABB by a given size * @brief Inflate each side of the AABB by a given size
* @param[in] dx Inflate X size * @param[in] dx Inflate X size
@ -119,13 +110,13 @@ namespace ephysics {
* @param[in] point Point to check. * @param[in] point Point to check.
* @return true The point in contained inside * @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 * @brief check if the AABB of a triangle intersects the AABB
* @param[in] trianglePoints List of 3 point od a triangle * @param[in] trianglePoints List of 3 point od a triangle
* @return true The triangle is contained in the Box * @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 * @brief check if the ray intersects the AABB
* This method use the line vs AABB raycasting technique described in * 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 * @param[in] trianglePoints List of 3 point od a triangle
* @return An AABB box * @return An AABB box
*/ */
static AABB createAABBForTriangle( vec3* trianglePoints); static AABB createAABBForTriangle( Vector3f* trianglePoints);
/** /**
* @brief Assignment operator * @brief Assignment operator
* @param[in] aabb The other box to compare * @param[in] aabb The other box to compare
* @return reference on this * @return reference on this
*/ */
AABB operator=( AABB aabb); AABB operator=( AABB aabb);
friend class DynamicAABBTree;
}; };
} }

View File

@ -14,17 +14,17 @@
using namespace ephysics; using namespace ephysics;
BoxShape::BoxShape( vec3 extent, float margin): BoxShape::BoxShape( Vector3f extent, float margin):
ConvexShape(BOX, margin), ConvexShape(BOX, margin),
this.extent(extent - vec3(margin, margin, margin)) { this.extent(extent - Vector3f(margin, margin, margin)) {
assert(extent.x() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.x() > margin); assert(extent.x() > 0.0f && extent.x() > margin);
assert(extent.y() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.y() > margin); assert(extent.y() > 0.0f && extent.y() > margin);
assert(extent.z() > 0.0f hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj extent.z() > 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; 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 xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y(); float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z(); 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) { 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 tMin = FLTMIN;
float tMax = FLTMAX; float tMax = FLTMAX;
vec3 normalDirection(0,0,0); Vector3f normalDirection(0,0,0);
vec3 currentNormal(0,0,0); Vector3f currentNormal(0,0,0);
// For each of the three slabs // For each of the three slabs
for (int iii=0; iii<3; ++iii) { for (int iii=0; iii<3; ++iii) {
// If ray is parallel to the slab // 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 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]) { if (ray.point1[iii] > this.extent[iii] || ray.point1[iii] < -this.extent[iii]) {
return false; 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 // Swap t1 and t2 if need so that t1 is intersection with near plane and
// t2 with far plane // t2 with far plane
if (t1 > t2) { if (t1 > t2) {
etk::swap(t1, t2); swap(t1, t2);
currentNormal = -currentNormal; currentNormal = -currentNormal;
} }
// Compute the intersection of the of slab intersection interval with previous slabs // 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; tMin = t1;
normalDirection = currentNormal; 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 is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) { if (tMin > ray.maxFraction) {
return false; return false;
@ -82,12 +82,12 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
|| tMin > ray.maxFraction) { || tMin > ray.maxFraction) {
return false; return false;
} }
if (normalDirection == vec3(0,0,0)) { if (normalDirection == Vector3f(0,0,0)) {
return false; return false;
} }
// The ray intersects the three slabs, we compute the hit point // The ray intersects the three slabs, we compute the hit point
vec3 localHitPoint = ray.point1 + tMin * rayDirection; Vector3f localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin; raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
@ -95,39 +95,39 @@ boolean BoxShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxySh
return true; return true;
} }
vec3 BoxShape::getExtent() { Vector3f BoxShape::getExtent() {
return this.extent + vec3(this.margin, this.margin, this.margin); 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; this.extent = (this.extent / this.scaling) * scaling;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
void BoxShape::getLocalBounds(vec3 min, vec3 max) { void BoxShape::getLocalBounds(Vector3f min, Vector3f max) {
// Maximum bounds // Maximum bounds
max = this.extent + vec3(this.margin, this.margin, this.margin); max = this.extent + Vector3f(this.margin, this.margin, this.margin);
// Minimum bounds // Minimum bounds
min = -max; min = -max;
} }
sizet BoxShape::getSizeInBytes() { long BoxShape::getSizeInBytes() {
return sizeof(BoxShape); return sizeof(BoxShape);
} }
vec3 BoxShape::getLocalSupportPointWithoutMargin( vec3 direction, Vector3f BoxShape::getLocalSupportPointWithoutMargin( Vector3f direction,
void** cachedCollisionData) { 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.y() < 0.0 ? -this.extent.y() : this.extent.y(),
direction.z() < 0.0 ? -this.extent.z() : this.extent.z()); direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
} }
boolean BoxShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { boolean BoxShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
return ( localPoint.x() < this.extent[0] return ( localPoint.x() < this.extent[0]
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.x() > -this.extent[0] && localPoint.x() > -this.extent[0]
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.extent[1] && localPoint.y() < this.extent[1]
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.extent[1] && localPoint.y() > -this.extent[1]
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() < this.extent[2] && localPoint.z() < this.extent[2]
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.z() > -this.extent[2]); && localPoint.z() > -this.extent[2]);
} }

View File

@ -1,58 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
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;
};
}

View File

@ -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();
};
}

View File

@ -19,7 +19,7 @@ CapsuleShape::CapsuleShape(float radius, float height):
assert(height > 0.0f); 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 // The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
float height = this.halfHeight + this.halfHeight; float height = this.halfHeight + this.halfHeight;
float radiusSquare = this.margin * this.margin; float radiusSquare = this.margin * this.margin;
@ -37,39 +37,39 @@ void CapsuleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass)
0.0, 0.0, IxxAndzz); 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 diffYCenterSphere1 = localPoint.y() - this.halfHeight;
float diffYCenterSphere2 = localPoint.y() + this.halfHeight; float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
float xSquare = localPoint.x() * localPoint.x(); float xSquare = localPoint.x() * localPoint.x();
float zSquare = localPoint.z() * localPoint.z(); float zSquare = localPoint.z() * localPoint.z();
float squareRadius = this.margin * this.margin; 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 true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj return ((xSquare + zSquare) < squareRadius &&
localPoint.y() < this.halfHeight hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight) || localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius; (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
} }
boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { 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); float epsilon = float(0.01);
vec3 p(float(0), -this.halfHeight, float(0)); Vector3f p(float(0), -this.halfHeight, float(0));
vec3 q(float(0), this.halfHeight, float(0)); Vector3f q(float(0), this.halfHeight, float(0));
vec3 d = q - p; Vector3f d = q - p;
vec3 m = ray.point1 - p; Vector3f m = ray.point1 - p;
float t; float t;
float mDotD = m.dot(d); float mDotD = m.dot(d);
float nDotD = n.dot(d); float nDotD = n.dot(d);
float dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // 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 if ( vec1DotD < 0.0f
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD < float(0.0)) { && vec1DotD + nDotD < float(0.0)) {
return false; return false;
} }
float ddotDExtraCaps = float(2.0) * this.margin * d.y(); float ddotDExtraCaps = float(2.0) * this.margin * d.y();
if ( vec1DotD > dDotD + ddotDExtraCaps if ( vec1DotD > dDotD + ddotDExtraCaps
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj vec1DotD + nDotD > dDotD + ddotDExtraCaps) { && vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
return false; return false;
} }
float nDotN = n.dot(n); 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 k = m.dot(m) - this.margin * this.margin;
float c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis // 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 the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > 0.0f) { if (c > 0.0f) {
return false; 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 the ray intersects with the "p" endcap of the capsule
if (mDotD < 0.0f) { if (mDotD < 0.0f) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check intersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint; Vector3f hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p; Vector3f normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder } 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 // Check intersection between the ray and the "q" sphere endcap of the capsule
vec3 hitLocalPoint; Vector3f hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q; Vector3f normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -125,33 +125,33 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
return false; return false;
} }
// Compute the smallest root (first intersection along the ray) // 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 // If the intersection is outside the finite cylinder of the capsule on "p" endcap side
float value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < 0.0f) { if (value < 0.0f) {
// Check intersection between the ray and the "p" sphere endcap of the capsule // Check intersection between the ray and the "p" sphere endcap of the capsule
vec3 hitLocalPoint; Vector3f hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - p; Vector3f normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
return false; return false;
} else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side } 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 // Check intersection between the ray and the "q" sphere endcap of the capsule
vec3 hitLocalPoint; Vector3f hitLocalPoint;
float hitFraction; float hitFraction;
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) { if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = hitLocalPoint; raycastInfo.worldPoint = hitLocalPoint;
vec3 normalDirection = hitLocalPoint - q; Vector3f normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -164,31 +164,31 @@ boolean CapsuleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pro
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 v = localHitPoint - p; Vector3f v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d; Vector3f w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w)).safeNormalized(); Vector3f normalDirection = (localHitPoint - (p + w)).safeNormalized();
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1, boolean CapsuleShape::raycastWithSphereEndCap( Vector3f point1,
vec3 point2, Vector3f point2,
vec3 sphereCenter, Vector3f sphereCenter,
float maxFraction, float maxFraction,
vec3 hitLocalPoint, Vector3f hitLocalPoint,
float hitFraction) { float hitFraction) {
vec3 m = point1 - sphereCenter; Vector3f m = point1 - sphereCenter;
float c = m.dot(m) - this.margin * this.margin; float c = m.dot(m) - this.margin * this.margin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no intersection
if (c < 0.0f) { if (c < 0.0f) {
return false; return false;
} }
vec3 rayDirection = point2 - point1; Vector3f rayDirection = point2 - point1;
float b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // is pointing away from the sphere, there is no intersection
@ -204,7 +204,7 @@ boolean CapsuleShape::raycastWithSphereEndCap( vec3 point1,
return false; return false;
} }
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
float t = -b - etk::sqrt(discriminant); float t = -b - sqrt(discriminant);
assert(t >= 0.0f); assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) { if (t < maxFraction * raySquareLength) {
@ -225,17 +225,17 @@ float CapsuleShape::getHeight() {
return this.halfHeight + this.halfHeight; 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.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
this.margin = (this.margin / this.scaling.x()) * scaling.x(); this.margin = (this.margin / this.scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
sizet CapsuleShape::getSizeInBytes() { long CapsuleShape::getSizeInBytes() {
return sizeof(CapsuleShape); return sizeof(CapsuleShape);
} }
void CapsuleShape::getLocalBounds(vec3 min, vec3 max) { void CapsuleShape::getLocalBounds(Vector3f min, Vector3f max) {
// Maximum bounds // Maximum bounds
max.setX(this.margin); max.setX(this.margin);
max.setY(this.halfHeight + this.margin); max.setY(this.halfHeight + this.margin);
@ -246,7 +246,7 @@ void CapsuleShape::getLocalBounds(vec3 min, vec3 max) {
min.setZ(min.x()); min.setZ(min.x());
} }
vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction, Vector3f CapsuleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
void** cachedCollisionData) { void** cachedCollisionData) {
// Support point top sphere // Support point top sphere
float dotProductTop = this.halfHeight * direction.y(); float dotProductTop = this.halfHeight * direction.y();
@ -254,7 +254,7 @@ vec3 CapsuleShape::getLocalSupportPointWithoutMargin( vec3 direction,
float dotProductBottom = -this.halfHeight * direction.y(); float dotProductBottom = -this.halfHeight * direction.y();
// Return the point with the maximum dot product // Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) { 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);
} }

View File

@ -1,18 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/** /**
* @brief It represents a capsule collision shape that is defined around the Y axis. * @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. * 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 * and height of the shape. Therefore, no need to specify an object margin for a
* capsule shape. * capsule shape.
*/ */
class CapsuleShape : public ConvexShape { class CapsuleShape extends ConvexShape {
public : public :
/** /**
* @brief Constructor * @brief Constructor
@ -44,23 +31,23 @@ namespace ephysics {
* @return The height of the capsule shape (in meters) * @return The height of the capsule shape (in meters)
*/ */
float getHeight() ; float getHeight() ;
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
protected: protected:
float this.halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres) float halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
/** /**
* @brief Raycasting method between a ray one of the two spheres end cap of the capsule * @brief Raycasting method between a ray one of the two spheres end cap of the capsule
*/ */
boolean raycastWithSphereEndCap( vec3 point1, boolean raycastWithSphereEndCap( Vector3f point1,
vec3 point2, Vector3f point2,
vec3 sphereCenter, Vector3f sphereCenter,
float maxFraction, float maxFraction,
vec3 hitLocalPoint, Vector3f hitLocalPoint,
float hitFraction) ; float hitFraction) ;
sizet getSizeInBytes() override; long getSizeInBytes() ;
}; };
} }

View File

@ -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()"); PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction // Get the local bounds in x,y and z direction
vec3 minBounds(0,0,0); Vector3f minBounds(0,0,0);
vec3 maxBounds(0,0,0); Vector3f maxBounds(0,0,0);
getLocalBounds(minBounds, maxBounds); getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body // Rotate the local bounds according to the orientation of the body
etk::Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsolute(); Matrix3f worldAxis = transform.getOrientation().getMatrix().getAbsolute();
vec3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds), Vector3f worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).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(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds)); worldAxis.getColumn(2).dot(maxBounds));
// Compute the minimum and maximum coordinates of the rotated extents // Compute the minimum and maximum coordinates of the rotated extents
vec3 minCoordinates = transform.getPosition() + worldMinBounds; Vector3f minCoordinates = transform.getPosition() + worldMinBounds;
vec3 maxCoordinates = transform.getPosition() + worldMaxBounds; Vector3f maxCoordinates = transform.getPosition() + worldMaxBounds;
// Update the AABB with the new minimum and maximum coordinates // Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates); aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates); aabb.setMax(maxCoordinates);
@ -44,7 +44,7 @@ void CollisionShape::computeAABB(AABB aabb, etk::Transform3D transform) {
int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1, int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) { CollisionShapeType shapeType2) {
// If both shapes are convex // If both shapes are convex
if (isConvex(shapeType1) hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj isConvex(shapeType2)) { if (isConvex(shapeType1) && isConvex(shapeType2)) {
return NBMAXCONTACTMANIFOLDSCONVEXSHAPE; return NBMAXCONTACTMANIFOLDSCONVEXSHAPE;
} }
// If there is at least one concave shape // If there is at least one concave shape

View File

@ -1,21 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <etk/typeInfo.hpp>
#include <etk/math/Vector3D.hpp>
#include <etk/math/Matrix3x3.hpp>
#include <ephysics/mathematics/Ray.hpp>
#include <ephysics/collision/shapes/AABB.hpp>
#include <ephysics/collision/RaycastInfo.hpp>
namespace ephysics {
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER, enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD}; CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD};
int NBCOLLISIONSHAPETYPES = 9; int NBCOLLISIONSHAPETYPES = 9;
@ -31,12 +15,6 @@ class CollisionShape {
public : public :
/// Constructor /// Constructor
CollisionShape(CollisionShapeType type); 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 * @brief Get the type of the collision shapes
* @return The type of the collision shape (box, sphere, cylinder, ...) * @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 true If the collision shape is convex
* @return false If it is concave * @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. * @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 * This method is used to compute the AABB of the box
* @param min The minimum bounds of the shape in local-space coordinates * @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum 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 /// Return the scaling vector of the collision shape
vec3 getScaling() { Vector3f getScaling() {
return this.scaling; return this.scaling;
} }
/** /**
* @brief Set the scaling vector of the collision shape * @brief Set the scaling vector of the collision shape
*/ */
virtual void setLocalScaling( vec3 scaling) { void setLocalScaling( Vector3f scaling) {
this.scaling = 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[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 * @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 * @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[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 * @brief Check if the shape is convex
* @param[in] shapeType shape type * @param[in] shapeType shape type
@ -87,7 +65,7 @@ class CollisionShape {
*/ */
static boolean isConvex(CollisionShapeType shapeType) { static boolean isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVEMESH return shapeType != CONCAVEMESH
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapeType != HEIGHTFIELD; && shapeType != HEIGHTFIELD;
} }
/** /**
* @brief Get the maximum number of contact * @brief Get the maximum number of contact
@ -95,17 +73,15 @@ class CollisionShape {
*/ */
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1, static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2); CollisionShapeType shapeType2);
friend class ProxyShape;
friend class CollisionWorld;
protected : protected :
CollisionShapeType this.type; //!< Type of the collision shape CollisionShapeType type; //!< Type of the collision shape
vec3 this.scaling; //!< Scaling vector of the collision shape Vector3f scaling; //!< Scaling vector of the collision shape
/// Return true if a point is inside 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 /// 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 /// Return the number of bytes used by the collision shape
virtual sizet getSizeInBytes() = 0; long getSizeInBytes() = 0;
}; };

View File

@ -21,13 +21,13 @@ ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh):
void ConcaveMeshShape::initBVHTree() { void ConcaveMeshShape::initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree // TODO : Try to randomly add the triangles into the tree to obtain a better tree
// For each sub-part of the mesh // For each sub-part of the mesh
for (int subPart=0; subPart<this.triangleMesh->getNbSubparts(); subPart++) { for (int subPart=0; subPart<this.triangleMesh.getNbSubparts(); subPart++) {
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = this.triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
// For each triangle of the concave mesh // For each triangle of the concave mesh
for (sizet iii=0; iii<triangleVertexArray->getNbTriangles(); ++iii) { for (long iii=0; iii<triangleVertexArray.getNbTriangles(); ++iii) {
ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(iii); ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(iii);
vec3 trianglePoints2[3]; Vector3f trianglePoints2[3];
trianglePoints2[0] = trianglePoints[0]; trianglePoints2[0] = trianglePoints[0];
trianglePoints2[1] = trianglePoints[1]; trianglePoints2[1] = trianglePoints[1];
trianglePoints2[2] = trianglePoints[2]; trianglePoints2[2] = trianglePoints[2];
@ -40,14 +40,14 @@ void ConcaveMeshShape::initBVHTree() {
} }
} }
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, vec3* outTriangleVertices) { void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int subPart, int triangleIndex, Vector3f* outTriangleVertices) {
EPHYASSERT(outTriangleVertices != null, "Input check error"); EPHYASSERT(outTriangleVertices != null, "Input check error");
// Get the triangle vertex array of the current sub-part // Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = this.triangleMesh->getSubpart(subPart); TriangleVertexArray* triangleVertexArray = this.triangleMesh.getSubpart(subPart);
if (triangleVertexArray == null) { if (triangleVertexArray == null) {
Log.error("get null ..."); Log.error("get null ...");
} }
ephysics::Triangle trianglePoints = triangleVertexArray->getTriangle(triangleIndex); ephysics::Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
outTriangleVertices[0] = trianglePoints[0] * this.scaling; outTriangleVertices[0] = trianglePoints[0] * this.scaling;
outTriangleVertices[1] = trianglePoints[1] * this.scaling; outTriangleVertices[1] = trianglePoints[1] * this.scaling;
outTriangleVertices[2] = trianglePoints[2] * 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) // Get the node data (triangle index and mesh subpart index)
int* data = this.dynamicAABBTree.getNodeDataInt(nodeId); int* data = this.dynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape // Get the triangle vertices for this node from the concave mesh shape
vec3 trianglePoints[3]; Vector3f trianglePoints[3];
getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints); getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle // Call the callback to test narrow-phase collision with this triangle
callback.testTriangle(trianglePoints); callback.testTriangle(trianglePoints);
@ -86,13 +86,13 @@ float ConcaveMeshRaycastCallback::operator()(int nodeId, Ray ray) {
} }
void ConcaveMeshRaycastCallback::raycastTriangles() { void ConcaveMeshRaycastCallback::raycastTriangles() {
etk::Vector<int>::Iterator it; Vector<int>::Iterator it;
float smallestHitFraction = this.ray.maxFraction; float smallestHitFraction = this.ray.maxFraction;
for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) { for (it = this.hitAABBNodes.begin(); it != this.hitAABBNodes.end(); ++it) {
// Get the node data (triangle index and mesh subpart index) // Get the node data (triangle index and mesh subpart index)
int* data = this.dynamicAABBTree.getNodeDataInt(*it); int* data = this.dynamicAABBTree.getNodeDataInt(*it);
// Get the triangle vertices for this node from the concave mesh shape // 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); this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Create a triangle collision shape // Create a triangle collision shape
float margin = this.concaveMeshShape.getTriangleMargin(); float margin = this.concaveMeshShape.getTriangleMargin();
@ -102,7 +102,7 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
RaycastInfo raycastInfo; RaycastInfo raycastInfo;
boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape); boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
// If the ray hit the collision shape // If the ray hit the collision shape
if (isTriangleHit hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= smallestHitFraction) { if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= 0.0f); assert(raycastInfo.hitFraction >= 0.0f);
this.raycastInfo.body = raycastInfo.body; this.raycastInfo.body = raycastInfo.body;
this.raycastInfo.proxyShape = raycastInfo.proxyShape; this.raycastInfo.proxyShape = raycastInfo.proxyShape;
@ -117,24 +117,24 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
} }
} }
sizet ConcaveMeshShape::getSizeInBytes() { long ConcaveMeshShape::getSizeInBytes() {
return sizeof(ConcaveMeshShape); return sizeof(ConcaveMeshShape);
} }
void ConcaveMeshShape::getLocalBounds(vec3 min, vec3 max) { void ConcaveMeshShape::getLocalBounds(Vector3f min, Vector3f max) {
// Get the AABB of the whole tree // Get the AABB of the whole tree
AABB treeAABB = this.dynamicAABBTree.getRootAABB(); AABB treeAABB = this.dynamicAABBTree.getRootAABB();
min = treeAABB.getMin(); min = treeAABB.getMin();
max = treeAABB.getMax(); max = treeAABB.getMax();
} }
void ConcaveMeshShape::setLocalScaling( vec3 scaling) { void ConcaveMeshShape::setLocalScaling( Vector3f scaling) {
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
this.dynamicAABBTree.reset(); this.dynamicAABBTree.reset();
initBVHTree(); initBVHTree();
} }
void ConcaveMeshShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { void ConcaveMeshShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
// Default inertia tensor // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // 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, // However, in most cases, it will only be used static bodies and therefore,

View File

@ -1,85 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConcaveShape.hpp>
#include <ephysics/collision/broadphase/DynamicAABBTree.hpp>
#include <ephysics/collision/TriangleMesh.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp>
#include <ephysics/engine/Profiler.hpp>
namespace ephysics {
class ConcaveMeshShape;
class ConcaveMeshRaycastCallback {
private:
etk::Vector<int> 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) ;
};
}

View File

@ -0,0 +1,63 @@
package org.atriaSoft.ephysics.collision.shapes;
class ConcaveMeshRaycastCallback {
private:
Vector<int> 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) ;
};
}

View File

@ -29,7 +29,7 @@ boolean ConcaveShape::isConvex() {
return false; return false;
} }
boolean ConcaveShape::testPointInside( vec3 localPoint, ProxyShape* proxyShape) { boolean ConcaveShape::testPointInside( Vector3f localPoint, ProxyShape* proxyShape) {
return false; return false;
} }

View File

@ -1,33 +1,19 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/CollisionShape.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp>
namespace ephysics {
/** /**
* @brief It is used to encapsulate a callback method for * @brief It is used to encapsulate a callback method for
* a single triangle of a ConcaveMesh. * a single triangle of a ConcaveMesh.
*/ */
class TriangleCallback { class TriangleCallback {
public: /// Report a triangle
virtual ~TriangleCallback() = default; public void testTriangle( Vector3f* trianglePoints)=0;
/// Report a triangle
virtual void testTriangle( vec3* trianglePoints)=0;
}; };
/** /**
* @brief This abstract class represents a concave collision shape associated with a * @brief This abstract class represents a concave collision shape associated with a
* body that is used during the narrow-phase collision detection. * body that is used during the narrow-phase collision detection.
*/ */
class ConcaveShape : public CollisionShape { class ConcaveShape extends CollisionShape {
public : public :
/// Constructor /// Constructor
ConcaveShape(CollisionShapeType type); ConcaveShape(CollisionShapeType type);
@ -36,10 +22,10 @@ namespace ephysics {
/// DELETE assignment operator /// DELETE assignment operator
ConcaveShape operator=( ConcaveShape shape) = delete; ConcaveShape operator=( ConcaveShape shape) = delete;
protected : protected :
boolean this.isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
float this.triangleMargin; //!< Margin use for collision detection for each triangle float triangleMargin; //!< Margin use for collision detection for each triangle
TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
public: public:
/// Return the triangle margin /// Return the triangle margin
float getTriangleMargin() ; float getTriangleMargin() ;
@ -51,9 +37,9 @@ namespace ephysics {
*/ */
void setRaycastTestType(TriangleRaycastSide testType); void setRaycastTestType(TriangleRaycastSide testType);
/// Return true if the collision shape is convex, false if it is concave /// 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 /// 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 /// Return true if the smooth mesh collision is enabled
boolean getIsSmoothMeshCollisionEnabled() ; boolean getIsSmoothMeshCollisionEnabled() ;
/** /**

View File

@ -22,49 +22,49 @@ ConeShape::ConeShape(float radius, float height, float margin):
this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height)); this.sinTheta = this.radius / (sqrt(this.radius * this.radius + height * height));
} }
vec3 ConeShape::getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) { Vector3f ConeShape::getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) {
vec3 v = direction; Vector3f v = direction;
float sinThetaTimesLengthV = this.sinTheta * v.length(); float sinThetaTimesLengthV = this.sinTheta * v.length();
vec3 supportPoint; Vector3f supportPoint;
if (v.y() > sinThetaTimesLengthV) { if (v.y() > sinThetaTimesLengthV) {
supportPoint = vec3(0.0, this.halfHeight, 0.0); supportPoint = Vector3f(0.0, this.halfHeight, 0.0);
} else { } else {
float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z()); float projectedLength = sqrt(v.x() * v.x() + v.z() * v.z());
if (projectedLength > FLTEPSILON) { if (projectedLength > FLTEPSILON) {
float d = this.radius / projectedLength; 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 { } else {
supportPoint = vec3(0.0, -this.halfHeight, 0.0); supportPoint = Vector3f(0.0, -this.halfHeight, 0.0);
} }
} }
return supportPoint; return supportPoint;
} }
boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { 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); float epsilon = float(0.00001);
vec3 V(0, this.halfHeight, 0); Vector3f V(0, this.halfHeight, 0);
vec3 centerBase(0, -this.halfHeight, 0); Vector3f centerBase(0, -this.halfHeight, 0);
vec3 axis(0, float(-1.0), 0); Vector3f axis(0, float(-1.0), 0);
float heightSquare = float(4.0) * this.halfHeight * this.halfHeight; float heightSquare = float(4.0) * this.halfHeight * this.halfHeight;
float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius); float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
float factor = 1.0f - cosThetaSquare; 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 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 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 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)}; float tHit[] = {float(-1.0), float(-1.0), float(-1.0)};
vec3 localHitPoint[3]; Vector3f localHitPoint[3];
vec3 localNormal[3]; Vector3f localNormal[3];
// If c2 is different from zero // If c2 is different from zero
if (etk::abs(c2) > FLTEPSILON) { if (abs(c2) > FLTEPSILON) {
float gamma = c1 * c1 - c0 * c2; float gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation // If there is no real roots in the quadratic equation
if (gamma < 0.0f) { if (gamma < 0.0f) {
return false; return false;
} else if (gamma > 0.0f) { // The equation has two real roots } else if (gamma > 0.0f) { // The equation has two real roots
// Compute two intersections // Compute two intersections
float sqrRoot = etk::sqrt(gamma); float sqrRoot = sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2; tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2; tHit[1] = (-c1 + sqrRoot) / c2;
} else { // If the equation has a single real root } else { // If the equation has a single real root
@ -73,7 +73,7 @@ boolean ConeShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyS
} }
} else { } else {
// If c2 == 0 // If c2 == 0
if (etk::abs(c1) > FLTEPSILON) { if (abs(c1) > FLTEPSILON) {
// If c2 = 0 and c1 != 0 // If c2 = 0 and c1 != 0
tHit[0] = -c0 / (float(2.0) * c1); tHit[0] = -c0 / (float(2.0) * c1);
} else { } 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 value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
float rOverH = this.radius / h; float rOverH = this.radius / h;
float value2 = 1.0f + rOverH * rOverH; 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 x = localHitPoint[hitIndex].x() * factor;
float z = localHitPoint[hitIndex].z() * factor; float z = localHitPoint[hitIndex].z() * factor;
localNormal[hitIndex].setX(x); 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); localNormal[hitIndex].setZ(z);
} }
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex]; raycastInfo.worldPoint = localHitPoint[hitIndex];
@ -163,17 +163,17 @@ float ConeShape::getHeight() {
return float(2.0) * this.halfHeight; 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.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
this.radius = (this.radius / this.scaling.x()) * scaling.x(); this.radius = (this.radius / this.scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
sizet ConeShape::getSizeInBytes() { long ConeShape::getSizeInBytes() {
return sizeof(ConeShape); return sizeof(ConeShape);
} }
void ConeShape::getLocalBounds(vec3 min, vec3 max) { void ConeShape::getLocalBounds(Vector3f min, Vector3f max) {
// Maximum bounds // Maximum bounds
max.setX(this.radius + this.margin); max.setX(this.radius + this.margin);
max.setY(this.halfHeight + this.margin); max.setY(this.halfHeight + this.margin);
@ -184,7 +184,7 @@ void ConeShape::getLocalBounds(vec3 min, vec3 max) {
min.setZ(min.x()); 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 rSquare = this.radius * this.radius;
float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight); float diagXZ = float(0.15) * mass * (rSquare + this.halfHeight);
tensor.setValue(diagXZ, 0.0, 0.0, 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); 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 float radiusHeight = this.radius
* (-localPoint.y() + this.halfHeight) * (-localPoint.y() + this.halfHeight)
/ (this.halfHeight * float(2.0)); / (this.halfHeight * float(2.0));
return ( localPoint.y() < this.halfHeight return ( localPoint.y() < this.halfHeight
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight) && localPoint.y() > -this.halfHeight)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight); && (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight *radiusHeight);
} }

View File

@ -1,18 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/** /**
* @brief This class represents a cone collision shape centered at the * @brief This class represents a cone collision shape centered at the
* origin and alligned with the Y axis. The cone is defined * 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 * ructor of the cone shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the ructor. * default margin distance by not using the "margin" parameter in the ructor.
*/ */
class ConeShape : public ConvexShape { class ConeShape extends ConvexShape {
public : public :
/** /**
* @brief Constructor * @brief Constructor
@ -36,32 +23,28 @@ namespace ephysics {
* @param margin Collision margin (in meters) around the collision shape * @param margin Collision margin (in meters) around the collision shape
*/ */
ConeShape(float radius, float height, float margin = OBJECTMARGIN); ConeShape(float radius, float height, float margin = OBJECTMARGIN);
/// DELETE copy-ructor
ConeShape( ConeShape shape) = delete;
/// DELETE assignment operator
ConeShape operator=( ConeShape shape) = delete;
protected : protected :
float this.radius; //!< Radius of the base float radius; //!< Radius of the base
float this.halfHeight; //!< Half height of the cone float halfHeight; //!< Half height of the cone
float this.sinTheta; //!< sine of the semi angle at the apex point float sinTheta; //!< sine of the semi angle at the apex point
virtual vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
sizet getSizeInBytes() override; long getSizeInBytes() ;
public: public:
/** /**
* @brief Return the radius * @brief Return the radius
* @return Radius of the cone (in meters) * @return Radius of the cone (in meters)
*/ */
float getRadius() ; float getRadius();
/** /**
* @brief Return the height * @brief Return the height
* @return Height of the cone (in meters) * @return Height of the cone (in meters)
*/ */
float getHeight() ; float getHeight();
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
}; };
} }

View File

@ -26,7 +26,7 @@ ConvexMeshShape::ConvexMeshShape( float* arrayVertices,
// Copy all the vertices into the internal array // Copy all the vertices into the internal array
for (int iii=0; iii<this.numberVertices; iii++) { for (int iii=0; iii<this.numberVertices; iii++) {
float* newPoint = ( float*) vertexPointer; float* newPoint = ( float*) vertexPointer;
this.vertices.pushBack(vec3(newPoint[0], newPoint[1], newPoint[2])); this.vertices.pushBack(Vector3f(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride; vertexPointer += stride;
} }
// Recalculate the bounds of the mesh // Recalculate the bounds of the mesh
@ -41,17 +41,17 @@ ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray,
this.maxBounds(0, 0, 0), this.maxBounds(0, 0, 0),
this.isEdgesInformationUsed(isEdgesInformationUsed) { this.isEdgesInformationUsed(isEdgesInformationUsed) {
// For each vertex of the mesh // For each vertex of the mesh
for (auto it: triangleVertexArray->getVertices()) { for (auto it: triangleVertexArray.getVertices()) {
this.vertices.pushBack(it*this.scaling); this.vertices.pushBack(it*this.scaling);
} }
// If we need to use the edges information of the mesh // If we need to use the edges information of the mesh
if (this.isEdgesInformationUsed) { if (this.isEdgesInformationUsed) {
// For each triangle of the mesh // For each triangle of the mesh
for (sizet iii=0; iii<triangleVertexArray->getNbTriangles(); iii++) { for (long iii=0; iii<triangleVertexArray.getNbTriangles(); iii++) {
int vertexIndex[3] = {0, 0, 0}; int vertexIndex[3] = {0, 0, 0};
vertexIndex[0] = triangleVertexArray->getIndices()[iii*3]; vertexIndex[0] = triangleVertexArray.getIndices()[iii*3];
vertexIndex[1] = triangleVertexArray->getIndices()[iii*3+1]; vertexIndex[1] = triangleVertexArray.getIndices()[iii*3+1];
vertexIndex[2] = triangleVertexArray->getIndices()[iii*3+2]; vertexIndex[2] = triangleVertexArray.getIndices()[iii*3+2];
// Add information about the edges // Add information about the edges
addEdge(vertexIndex[0], vertexIndex[1]); addEdge(vertexIndex[0], vertexIndex[1]);
addEdge(vertexIndex[0], vertexIndex[2]); 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) { void** cachedCollisionData) {
assert(this.numberVertices == this.vertices.size()); assert(this.numberVertices == this.vertices.size());
assert(cachedCollisionData != null); assert(cachedCollisionData != null);
@ -91,9 +91,9 @@ vec3 ConvexMeshShape::getLocalSupportPointWithoutMargin( vec3 direction,
isOptimal = true; isOptimal = true;
assert(this.edgesAdjacencyList[maxVertex].size() > 0); assert(this.edgesAdjacencyList[maxVertex].size() > 0);
// For all neighbors of the current vertex // For all neighbors of the current vertex
etk::Set<int>::Iterator it; Set<int>::Iterator it;
etk::Set<int>::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin(); Set<int>::Iterator itBegin = this.edgesAdjacencyList[maxVertex].begin();
etk::Set<int>::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end(); Set<int>::Iterator itEnd = this.edgesAdjacencyList[maxVertex].end();
for (it = itBegin; it != itEnd; ++it) { for (it = itBegin; it != itEnd; ++it) {
// Compute the dot product // Compute the dot product
float dotProduct = direction.dot(this.vertices[*it]); float dotProduct = direction.dot(this.vertices[*it]);
@ -160,32 +160,32 @@ void ConvexMeshShape::recalculateBounds() {
this.maxBounds = this.maxBounds * this.scaling; this.maxBounds = this.maxBounds * this.scaling;
this.minBounds = this.minBounds * this.scaling; this.minBounds = this.minBounds * this.scaling;
// Add the object margin to the bounds // Add the object margin to the bounds
this.maxBounds += vec3(this.margin, this.margin, this.margin); this.maxBounds += Vector3f(this.margin, this.margin, this.margin);
this.minBounds -= vec3(this.margin, this.margin, this.margin); this.minBounds -= Vector3f(this.margin, this.margin, this.margin);
} }
boolean ConvexMeshShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { 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); ConvexShape::setLocalScaling(scaling);
recalculateBounds(); recalculateBounds();
} }
sizet ConvexMeshShape::getSizeInBytes() { long ConvexMeshShape::getSizeInBytes() {
return sizeof(ConvexMeshShape); return sizeof(ConvexMeshShape);
} }
void ConvexMeshShape::getLocalBounds(vec3 min, vec3 max) { void ConvexMeshShape::getLocalBounds(Vector3f min, Vector3f max) {
min = this.minBounds; min = this.minBounds;
max = this.maxBounds; 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; float factor = (1.0f / float(3.0)) * mass;
vec3 realExtent = 0.5f * (this.maxBounds - this.minBounds); Vector3f realExtent = 0.5f * (this.maxBounds - this.minBounds);
assert(realExtent.x() > 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0); assert(realExtent.x() > 0 && realExtent.y() > 0 hjkhjkhjkhkj realExtent.z() > 0);
float xSquare = realExtent.x() * realExtent.x(); float xSquare = realExtent.x() * realExtent.x();
float ySquare = realExtent.y() * realExtent.y(); float ySquare = realExtent.y() * realExtent.y();
float zSquare = realExtent.z() * realExtent.z(); 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)); 0.0, 0.0, factor * (xSquare + ySquare));
} }
void ConvexMeshShape::addVertex( vec3 vertex) { void ConvexMeshShape::addVertex( Vector3f vertex) {
// Add the vertex in to vertices array // Add the vertex in to vertices array
this.vertices.pushBack(vertex); this.vertices.pushBack(vertex);
this.numberVertices++; this.numberVertices++;
@ -222,11 +222,11 @@ void ConvexMeshShape::addVertex( vec3 vertex) {
void ConvexMeshShape::addEdge(int v1, int v2) { void ConvexMeshShape::addEdge(int v1, int v2) {
// If the entry for vertex v1 does not exist in the adjacency list // If the entry for vertex v1 does not exist in the adjacency list
if (this.edgesAdjacencyList.count(v1) == 0) { if (this.edgesAdjacencyList.count(v1) == 0) {
this.edgesAdjacencyList.add(v1, etk::Set<int>()); this.edgesAdjacencyList.add(v1, Set<int>());
} }
// If the entry for vertex v2 does not exist in the adjacency list // If the entry for vertex v2 does not exist in the adjacency list
if (this.edgesAdjacencyList.count(v2) == 0) { if (this.edgesAdjacencyList.count(v2) == 0) {
this.edgesAdjacencyList.add(v2, etk::Set<int>()); this.edgesAdjacencyList.add(v2, Set<int>());
} }
// Add the edge in the adjacency list // Add the edge in the adjacency list
this.edgesAdjacencyList[v1].add(v2); this.edgesAdjacencyList[v1].add(v2);
@ -241,8 +241,8 @@ void ConvexMeshShape::setIsEdgesInformationUsed(boolean isEdgesUsed) {
this.isEdgesInformationUsed = isEdgesUsed; this.isEdgesInformationUsed = isEdgesUsed;
} }
boolean ConvexMeshShape::testPointInside( vec3 localPoint, boolean ConvexMeshShape::testPointInside( Vector3f localPoint,
ProxyShape* proxyShape) { ProxyShape* proxyShape) {
// Use the GJK algorithm to test if the point is inside the convex mesh // 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);
} }

View File

@ -1,23 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/engine/CollisionWorld.hpp>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/collision/TriangleMesh.hpp>
#include <ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp>
#include <etk/Vector.hpp>
#include <etk/Map.hpp>
namespace ephysics {
class CollisionWorld;
/** /**
* @brief It represents a convex mesh shape. In order to create a convex mesh shape, you * @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 * 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 * with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection. * in order to use the edges information for collision detection.
*/ */
class ConvexMeshShape : public ConvexShape { class ConvexMeshShape extends ConvexShape {
protected : protected :
etk::Vector<vec3> this.vertices; //!< Array with the vertices of the mesh Vector<Vector3f> vertices; //!< Array with the vertices of the mesh
int this.numberVertices; //!< Number of vertices in the mesh int numberVertices; //!< Number of vertices in the mesh
vec3 this.minBounds; //!< Mesh minimum bounds in the three local x, y and z directions Vector3f 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 Vector3f 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 boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
etk::Map<int, etk::Set<int> > this.edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh Map<int, Set<int> > edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
/// Private copy-ructor
ConvexMeshShape( ConvexMeshShape shape);
/// Private assignment operator
ConvexMeshShape operator=( ConvexMeshShape shape);
/// Recompute the bounds of the mesh /// Recompute the bounds of the mesh
void recalculateBounds(); void recalculateBounds();
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
sizet getSizeInBytes() override; long getSizeInBytes() ;
public : public :
/** /**
* @brief Constructor to initialize with an array of 3D vertices. * @brief Constructor to initialize with an array of 3D vertices.
@ -81,13 +59,13 @@ namespace ephysics {
*/ */
ConvexMeshShape(float margin = OBJECTMARGIN); ConvexMeshShape(float margin = OBJECTMARGIN);
public: public:
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
/** /**
* @brief Add a vertex into the convex mesh * @brief Add a vertex into the convex mesh
* @param vertex Vertex to be added * @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. * @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 * @note that the vertex indices start at zero and need to correspond to the order of

View File

@ -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 // Get the support point without margin
vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData); Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (this.margin != 0.0f) { if (this.margin != 0.0f) {
// Add the margin to the support point // 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) { if (direction.length2() > FLTEPSILON * FLTEPSILON) {
unitVec = direction.safeNormalized(); unitVec = direction.safeNormalized();
} }

View File

@ -1,51 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/CollisionShape.hpp>
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;
};
}

View File

@ -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;
}
};
}

View File

@ -20,11 +20,11 @@ CylinderShape::CylinderShape(float radius,
assert(height > 0.0f); assert(height > 0.0f);
} }
vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction, Vector3f CylinderShape::getLocalSupportPointWithoutMargin( Vector3f direction,
void** cachedCollisionData) { void** cachedCollisionData) {
vec3 supportPoint(0.0, 0.0, 0.0); Vector3f supportPoint(0.0, 0.0, 0.0);
float uDotv = direction.y(); 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()); float lengthW = sqrt(direction.x() * direction.x() + direction.z() * direction.z());
if (lengthW > FLTEPSILON) { if (lengthW > FLTEPSILON) {
if (uDotv < 0.0) { if (uDotv < 0.0) {
@ -44,21 +44,21 @@ vec3 CylinderShape::getLocalSupportPointWithoutMargin( vec3 direction,
} }
boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) { 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); float epsilon = float(0.01);
vec3 p(float(0), -this.halfHeight, float(0)); Vector3f p(float(0), -this.halfHeight, float(0));
vec3 q(float(0), this.halfHeight, float(0)); Vector3f q(float(0), this.halfHeight, float(0));
vec3 d = q - p; Vector3f d = q - p;
vec3 m = ray.point1 - p; Vector3f m = ray.point1 - p;
float t; float t;
float mDotD = m.dot(d); float mDotD = m.dot(d);
float nDotD = n.dot(d); float nDotD = n.dot(d);
float dDotD = d.dot(d); float dDotD = d.dot(d);
// Test if the segment is outside the cylinder // 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; return false;
} }
if (mDotD > dDotD hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj mDotD + nDotD > dDotD) { if (mDotD > dDotD && mDotD + nDotD > dDotD) {
return false; return false;
} }
float nDotN = n.dot(n); 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 k = m.dot(m) - this.radius * this.radius;
float c = dDotD * k - mDotD * mDotD; float c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis // 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 the origin is outside the surface of the cylinder, we return no hit
if (c > 0.0f) { if (c > 0.0f) {
return false; return false;
@ -82,12 +82,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, float(-1), 0); Vector3f normalDirection(0, float(-1), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -100,12 +100,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, 1.0f, 0); Vector3f normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -119,7 +119,7 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the smallest root (first intersection along the ray) // 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 // If the intersection is outside the cylinder on "p" endcap side
float value = mDotD + t * nDotD; float value = mDotD + t * nDotD;
if (value < 0.0f) { if (value < 0.0f) {
@ -139,12 +139,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, float(-1.0), 0); Vector3f normalDirection(0, float(-1.0), 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -166,12 +166,12 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 normalDirection(0, 1.0f, 0); Vector3f normalDirection(0, 1.0f, 0);
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -182,14 +182,14 @@ boolean CylinderShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return false; return false;
} }
// Compute the hit information // Compute the hit information
vec3 localHitPoint = ray.point1 + t * n; Vector3f localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
vec3 v = localHitPoint - p; Vector3f v = localHitPoint - p;
vec3 w = (v.dot(d) / d.length2()) * d; Vector3f w = (v.dot(d) / d.length2()) * d;
vec3 normalDirection = (localHitPoint - (p + w)); Vector3f normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection; raycastInfo.worldNormal = normalDirection;
return true; return true;
} }
@ -202,17 +202,17 @@ float CylinderShape::getHeight() {
return this.halfHeight + this.halfHeight; 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.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
this.radius = (this.radius / this.scaling.x()) * scaling.x(); this.radius = (this.radius / this.scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
sizet CylinderShape::getSizeInBytes() { long CylinderShape::getSizeInBytes() {
return sizeof(CylinderShape); return sizeof(CylinderShape);
} }
void CylinderShape::getLocalBounds(vec3 min, vec3 max) { void CylinderShape::getLocalBounds(Vector3f min, Vector3f max) {
// Maximum bounds // Maximum bounds
max.setX(this.radius + this.margin); max.setX(this.radius + this.margin);
max.setY(this.halfHeight + this.margin); max.setY(this.halfHeight + this.margin);
@ -223,7 +223,7 @@ void CylinderShape::getLocalBounds(vec3 min, vec3 max) {
min.setZ(min.x()); 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 height = float(2.0) * this.halfHeight;
float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height); float diag = (1.0f / float(12.0)) * mass * (3 * this.radius * this.radius + height * height);
tensor.setValue(diag, 0.0, 0.0, 0.0, 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); 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 return ( (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() < this.halfHeight && localPoint.y() < this.halfHeight
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj localPoint.y() > -this.halfHeight); && localPoint.y() > -this.halfHeight);
} }

View File

@ -1,18 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/** /**
* @brief It represents a cylinder collision shape around the Y axis * @brief It represents a cylinder collision shape around the Y axis
* and centered at the origin. The cylinder is defined by its height * and centered at the origin. The cylinder is defined by its height
@ -28,16 +15,12 @@ namespace ephysics {
*/ */
class CylinderShape: public ConvexShape { class CylinderShape: public ConvexShape {
protected: protected:
float this.radius; //!< Radius of the base float radius; //!< Radius of the base
float this.halfHeight; //!< Half height of the cylinder float halfHeight; //!< Half height of the cylinder
/// DELETED copy-ructor Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
CylinderShape( CylinderShape) = delete; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
/// DELETED assignment operator boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
CylinderShape operator=( CylinderShape) = delete; long getSizeInBytes() ;
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;
public: public:
/** /**
* @brief Contructor * @brief Contructor
@ -56,9 +39,9 @@ namespace ephysics {
* @return Height of the cylinder (in meters) * @return Height of the cylinder (in meters)
*/ */
float getHeight() ; float getHeight() ;
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
}; };
} }

View File

@ -40,25 +40,25 @@ HeightFieldShape::HeightFieldShape(int nbGridColumns,
assert(halfHeight >= 0); assert(halfHeight >= 0);
// Compute the local AABB of the height field // Compute the local AABB of the height field
if (this.upAxis == 0) { if (this.upAxis == 0) {
this.AABB.setMin(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(vec3(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) { } else if (this.upAxis == 1) {
this.AABB.setMin(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(vec3(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) { } else if (this.upAxis == 2) {
this.AABB.setMin(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(vec3(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; min = this.AABB.getMin() * this.scaling;
max = this.AABB.getMax() * this.scaling; max = this.AABB.getMax() * this.scaling;
} }
void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAABB) { void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAABB) {
// Compute the non-scaled AABB // 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); AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the integer grid coordinates inside the area we need to test for collision // Compute the integer grid coordinates inside the area we need to test for collision
int minGridCoords[3]; int minGridCoords[3];
@ -89,20 +89,20 @@ void HeightFieldShape::testAllTriangles(TriangleCallback callback, AABB localAA
jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1); jMax = clamp(maxGridCoords[1], 0, this.numberRows - 1);
break; break;
} }
assert(iMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMin < this.numberColumns); assert(iMin >= 0 && iMin < this.numberColumns);
assert(iMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj iMax < this.numberColumns); assert(iMax >= 0 && iMax < this.numberColumns);
assert(jMin >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMin < this.numberRows); assert(jMin >= 0 && jMin < this.numberRows);
assert(jMax >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj jMax < this.numberRows); assert(jMax >= 0 && jMax < this.numberRows);
// For each sub-grid points (except the last ones one each dimension) // For each sub-grid points (except the last ones one each dimension)
for (int i = iMin; i < iMax; i++) { for (int i = iMin; i < iMax; i++) {
for (int j = jMin; j < jMax; j++) { for (int j = jMin; j < jMax; j++) {
// Compute the four point of the current quad // Compute the four point of the current quad
vec3 p1 = getVertexAt(i, j); Vector3f p1 = getVertexAt(i, j);
vec3 p2 = getVertexAt(i, j + 1); Vector3f p2 = getVertexAt(i, j + 1);
vec3 p3 = getVertexAt(i + 1, j); Vector3f p3 = getVertexAt(i + 1, j);
vec3 p4 = getVertexAt(i + 1, j + 1); Vector3f p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle // 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 // Test collision against the first triangle
callback.testTriangle(trianglePoints); callback.testTriangle(trianglePoints);
// Generate the second triangle for the current grid rectangle // 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) { void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, AABB aabbToCollide) {
// Clamp the min/max coords of the AABB to collide inside the height field AABB // Clamp the min/max coords of the AABB to collide inside the height field AABB
vec3 minPoint = etk::max(aabbToCollide.getMin(), this.AABB.getMin()); Vector3f minPoint = max(aabbToCollide.getMin(), this.AABB.getMin());
minPoint = etk::min(minPoint, this.AABB.getMax()); minPoint = min(minPoint, this.AABB.getMax());
vec3 maxPoint = etk::min(aabbToCollide.getMax(), this.AABB.getMax()); Vector3f maxPoint = min(aabbToCollide.getMax(), this.AABB.getMax());
maxPoint = etk::max(maxPoint, this.AABB.getMin()); maxPoint = max(maxPoint, this.AABB.getMin());
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints] // 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 from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... this.width/2]
// and [-this.length/2 ... this.length/2] // and [-this.length/2 ... this.length/2]
vec3 translateVec = this.AABB.getExtent() * 0.5f; Vector3f translateVec = this.AABB.getExtent() * 0.5f;
minPoint += translateVec; minPoint += translateVec;
maxPoint += translateVec; maxPoint += translateVec;
// Convert the floating min/max coords of the AABB into closest integer // 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()"); PROFILE("HeightFieldShape::raycast()");
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this); TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
// Compute the AABB for the ray // Compute the AABB for the ray
vec3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1); Vector3f rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
AABB rayAABB(etk::min(ray.point1, rayEnd), etk::max(ray.point1, rayEnd)); AABB rayAABB(min(ray.point1, rayEnd), max(ray.point1, rayEnd));
testAllTriangles(triangleCallback, rayAABB); testAllTriangles(triangleCallback, rayAABB);
return triangleCallback.getIsHit(); return triangleCallback.getIsHit();
} }
vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) { Vector3f HeightFieldShape::getVertexAt(int xxx, int yyy) {
// Get the height value // Get the height value
float height = getHeightAt(xxx, yyy); float height = getHeightAt(xxx, yyy);
// Height values origin // Height values origin
float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight; float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
vec3 vertex; Vector3f vertex;
switch (this.upAxis) { switch (this.upAxis) {
case 0: 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; break;
case 1: 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; break;
case 2: 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; break;
default: default:
assert(false); assert(false);
@ -173,7 +173,7 @@ vec3 HeightFieldShape::getVertexAt(int xxx, int yyy) {
return vertex * this.scaling; return vertex * this.scaling;
} }
void TriangleOverlapCallback::testTriangle( vec3* trianglePoints) { void TriangleOverlapCallback::testTriangle( Vector3f* trianglePoints) {
// Create a triangle collision shape // Create a triangle collision shape
float margin = this.heightFieldShape.getTriangleMargin(); float margin = this.heightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin); 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); boolean isTriangleHit = triangleShape.raycast(this.ray, raycastInfo, this.proxyShape);
// If the ray hit the collision shape // If the ray hit the collision shape
if ( isTriangleHit if ( isTriangleHit
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj raycastInfo.hitFraction <= this.smallestHitFraction) { && raycastInfo.hitFraction <= this.smallestHitFraction) {
assert(raycastInfo.hitFraction >= 0.0f); assert(raycastInfo.hitFraction >= 0.0f);
this.raycastInfo.body = raycastInfo.body; this.raycastInfo.body = raycastInfo.body;
this.raycastInfo.proxyShape = raycastInfo.proxyShape; this.raycastInfo.proxyShape = raycastInfo.proxyShape;
@ -209,11 +209,11 @@ HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() {
return this.heightDataType; return this.heightDataType;
} }
sizet HeightFieldShape::getSizeInBytes() { long HeightFieldShape::getSizeInBytes() {
return sizeof(HeightFieldShape); return sizeof(HeightFieldShape);
} }
void HeightFieldShape::setLocalScaling( vec3 scaling) { void HeightFieldShape::setLocalScaling( Vector3f scaling) {
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
@ -235,7 +235,7 @@ int HeightFieldShape::computeIntegerGridValue(float value) {
return (value < 0.0f) ? value - 0.5f : value + 0.5f; 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 // Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh. // 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, // However, in most cases, it will only be used static bodies and therefore,

View File

@ -1,30 +1,17 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConcaveShape.hpp>
#include <ephysics/collision/shapes/TriangleShape.hpp>
#include <ephysics/engine/Profiler.hpp>
namespace ephysics {
class HeightFieldShape; class HeightFieldShape;
/** /**
* @brief This class is used for testing AABB and triangle overlap for raycasting * @brief This class is used for testing AABB and triangle overlap for raycasting
*/ */
class TriangleOverlapCallback : public TriangleCallback { class TriangleOverlapCallback extends TriangleCallback {
protected: protected:
Ray this.ray; Ray ray;
ProxyShape* this.proxyShape; ProxyShape* proxyShape;
RaycastInfo this.raycastInfo; RaycastInfo raycastInfo;
boolean this.isHit; boolean isHit;
float this.smallestHitFraction; float smallestHitFraction;
HeightFieldShape this.heightFieldShape; HeightFieldShape heightFieldShape;
public: public:
TriangleOverlapCallback( Ray ray, TriangleOverlapCallback( Ray ray,
ProxyShape* proxyShape, ProxyShape* proxyShape,
@ -41,7 +28,7 @@ namespace ephysics {
return this.isHit; return this.isHit;
} }
/// Raycast test between a ray and a triangle of the heightfield /// 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 * 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. * 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: public:
/** /**
* @brief Data type for the height data of the height field * @brief Data type for the height data of the height field
@ -66,32 +53,28 @@ namespace ephysics {
HEIGHTINTTYPE HEIGHTINTTYPE
}; };
protected: protected:
int this.numberColumns; //!< Number of columns in the grid of the height field int numberColumns; //!< Number of columns in the grid of the height field
int this.numberRows; //!< Number of rows in the grid of the height field int numberRows; //!< Number of rows in the grid of the height field
float this.width; //!< Height field width float width; //!< Height field width
float this.length; //!< Height field length float length; //!< Height field length
float this.minHeight; //!< Minimum height of the height field float minHeight; //!< Minimum height of the height field
float this.maxHeight; //!< Maximum height of the height field float maxHeight; //!< Maximum height of the height field
int this.upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z) int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
float this.integerHeightScale; //!< Height values scale for height field with integer height values float integerHeightScale; //!< Height values scale for height field with integer height values
HeightDataType this.heightDataType; //!< Data type of the height values HeightDataType heightDataType; //!< Data type of the height values
void* this.heightFieldData; //!< Array of data with all the height values of the height field void* heightFieldData; //!< Array of data with all the height values of the height field
AABB this.AABB; //!< Local AABB of the height field (without scaling) AABB AABB; //!< Local AABB of the height field (without scaling)
/// DELETED copy-ructor boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
HeightFieldShape( HeightFieldShape) = delete; long getSizeInBytes() ;
/// DELETED assignment operator
HeightFieldShape operator=( HeightFieldShape) = delete;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
sizet getSizeInBytes() override;
/// Insert all the triangles into the dynamic AABB tree /// Insert all the triangles into the dynamic AABB tree
void initBVHTree(); void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle /// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle. /// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int subPart, void getTriangleVerticesWithIndexPointer(int subPart,
int triangleIndex, int triangleIndex,
vec3* outTriangleVertices) ; Vector3f* outTriangleVertices) ;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position /// 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 /// Return the height of a given (x,y) point in the height field
float getHeightAt(int x, int y) ; float getHeightAt(int x, int y) ;
/// Return the closest inside integer grid value of a given floating grid value /// Return the closest inside integer grid value of a given floating grid value
@ -123,10 +106,10 @@ namespace ephysics {
int getNbColumns() ; int getNbColumns() ;
/// Return the type of height value in the height field /// Return the type of height value in the height field
HeightDataType getHeightDataType() ; HeightDataType getHeightDataType() ;
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
virtual void testAllTriangles(TriangleCallback callback, AABB localAABB) override; void testAllTriangles(TriangleCallback callback, AABB localAABB) ;
friend class ConvexTriangleAABBOverlapCallback; friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback; friend class ConcaveMeshRaycastCallback;
}; };

View File

@ -18,27 +18,27 @@ SphereShape::SphereShape(float radius):
assert(radius > 0.0f); assert(radius > 0.0f);
} }
void SphereShape::setLocalScaling( vec3 scaling) { void SphereShape::setLocalScaling( Vector3f scaling) {
this.margin = (this.margin / this.scaling.x()) * scaling.x(); this.margin = (this.margin / this.scaling.x()) * scaling.x();
CollisionShape::setLocalScaling(scaling); 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; float diag = 0.4f * mass * this.margin * this.margin;
tensor.setValue(diag, 0.0f, 0.0f, tensor.setValue(diag, 0.0f, 0.0f,
0.0f, diag, 0.0f, 0.0f, diag, 0.0f,
0.0f, 0.0f, diag); 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 // 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 // Update the AABB with the new minimum and maximum coordinates
aabb.setMin(transform.getPosition() - extents); aabb.setMin(transform.getPosition() - extents);
aabb.setMax(transform.getPosition() + extents); aabb.setMax(transform.getPosition() + extents);
} }
void SphereShape::getLocalBounds(vec3 min, vec3 max) { void SphereShape::getLocalBounds(Vector3f min, Vector3f max) {
// Maximum bounds // Maximum bounds
max.setX(this.margin); max.setX(this.margin);
max.setY(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) { 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; float c = m.dot(m) - this.margin * this.margin;
// If the origin of the ray is inside the sphere, we return no intersection // If the origin of the ray is inside the sphere, we return no intersection
if (c < 0.0f) { if (c < 0.0f) {
return false; return false;
} }
vec3 rayDirection = ray.point2 - ray.point1; Vector3f rayDirection = ray.point2 - ray.point1;
float b = m.dot(rayDirection); float b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray // If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection // 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; return false;
} }
// Compute the solution "t" closest to the origin // Compute the solution "t" closest to the origin
float t = -b - etk::sqrt(discriminant); float t = -b - sqrt(discriminant);
assert(t >= 0.0f); assert(t >= 0.0f);
// If the hit point is withing the segment ray fraction // If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) { if (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information // Compute the intersection information
t /= raySquareLength; t /= raySquareLength;
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t; raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection; raycastInfo.worldPoint = ray.point1 + t * rayDirection;

View File

@ -1,57 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/collision/shapes/ConvexShape.hpp>
#include <ephysics/body/CollisionBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
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;
};
}

View File

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

View File

@ -14,7 +14,7 @@
// TODO: REMOVE this... // TODO: REMOVE this...
using namespace ephysics; 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) { : ConvexShape(TRIANGLE, margin) {
this.points[0] = point1; this.points[0] = point1;
this.points[1] = point2; 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) { boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) {
PROFILE("TriangleShape::raycast()"); PROFILE("TriangleShape::raycast()");
vec3 pq = ray.point2 - ray.point1; Vector3f pq = ray.point2 - ray.point1;
vec3 pa = this.points[0] - ray.point1; Vector3f pa = this.points[0] - ray.point1;
vec3 pb = this.points[1] - ray.point1; Vector3f pb = this.points[1] - ray.point1;
vec3 pc = this.points[2] - 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 // Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test. // product for this test.
vec3 m = pq.cross(pc); Vector3f m = pq.cross(pc);
float u = pb.dot(m); float u = pb.dot(m);
if (this.raycastTestType == FRONT) { if (this.raycastTestType == FRONT) {
if (u < 0.0f) { 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 the line PQ is in the triangle plane (case where u=v=w=0)
if ( approxEqual(u, 0) if ( approxEqual(u, 0)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(v, 0) && approxEqual(v, 0)
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj approxEqual(w, 0)) { && approxEqual(w, 0)) {
return false; return false;
} }
// Compute the barycentric coordinates (u, v, w) to determine the // 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; v *= denom;
w *= denom; w *= denom;
// Compute the local hit point using the barycentric coordinates // 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(); float hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if ( hitFraction < 0.0f if ( hitFraction < 0.0f
|| hitFraction > ray.maxFraction) { || hitFraction > ray.maxFraction) {
return false; 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) { if (localHitNormal.dot(pq) > 0.0f) {
localHitNormal = -localHitNormal; localHitNormal = -localHitNormal;
} }
raycastInfo.body = proxyShape->getBody(); raycastInfo.body = proxyShape.getBody();
raycastInfo.proxyShape = proxyShape; raycastInfo.proxyShape = proxyShape;
raycastInfo.worldPoint = localHitPoint; raycastInfo.worldPoint = localHitPoint;
raycastInfo.hitFraction = hitFraction; raycastInfo.hitFraction = hitFraction;
@ -100,49 +100,49 @@ boolean TriangleShape::raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* pr
return true; return true;
} }
sizet TriangleShape::getSizeInBytes() { long TriangleShape::getSizeInBytes() {
return sizeof(TriangleShape); return sizeof(TriangleShape);
} }
vec3 TriangleShape::getLocalSupportPointWithoutMargin( vec3 direction, Vector3f TriangleShape::getLocalSupportPointWithoutMargin( Vector3f direction,
void** cachedCollisionData) { 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()]; return this.points[dotProducts.getMaxAxis()];
} }
void TriangleShape::getLocalBounds(vec3 min, vec3 max) { void TriangleShape::getLocalBounds(Vector3f min, Vector3f max) {
vec3 xAxis(this.points[0].x(), this.points[1].x(), this.points[2].x()); Vector3f 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()); Vector3f 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()); Vector3f zAxis(this.points[0].z(), this.points[1].z(), this.points[2].z());
min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()); min.setValue(xAxis.getMin(), yAxis.getMin(), zAxis.getMin());
max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()); max.setValue(xAxis.getMax(), yAxis.getMax(), zAxis.getMax());
min -= vec3(this.margin, this.margin, this.margin); min -= Vector3f(this.margin, this.margin, this.margin);
max += vec3(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[0] = (this.points[0] / this.scaling) * scaling;
this.points[1] = (this.points[1] / this.scaling) * scaling; this.points[1] = (this.points[1] / this.scaling) * scaling;
this.points[2] = (this.points[2] / this.scaling) * scaling; this.points[2] = (this.points[2] / this.scaling) * scaling;
CollisionShape::setLocalScaling(scaling); CollisionShape::setLocalScaling(scaling);
} }
void TriangleShape::computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) { void TriangleShape::computeLocalInertiaTensor(Matrix3f tensor, float mass) {
tensor.setZero(); tensor.setZero();
} }
void TriangleShape::computeAABB(AABB aabb, etk::Transform3D transform) { void TriangleShape::computeAABB(AABB aabb, Transform3D transform) {
vec3 worldPoint1 = transform * this.points[0]; Vector3f worldPoint1 = transform * this.points[0];
vec3 worldPoint2 = transform * this.points[1]; Vector3f worldPoint2 = transform * this.points[1];
vec3 worldPoint3 = transform * this.points[2]; Vector3f worldPoint3 = transform * this.points[2];
vec3 xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x()); Vector3f xAxis(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
vec3 yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y()); Vector3f yAxis(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
vec3 zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z()); Vector3f zAxis(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
aabb.setMin(vec3(xAxis.getMin(), yAxis.getMin(), zAxis.getMin())); aabb.setMin(Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
aabb.setMax(vec3(xAxis.getMax(), yAxis.getMax(), zAxis.getMax())); 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; return false;
} }
@ -155,9 +155,9 @@ void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
this.raycastTestType = testType; this.raycastTestType = testType;
} }
vec3 TriangleShape::getVertex(int index) { Vector3f TriangleShape::getVertex(int index) {
assert( index >= 0 assert( index >= 0
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3); && index < 3);
return this.points[index]; return this.points[index];
} }

View File

@ -1,17 +1,5 @@
/** @file package org.atriaSoft.ephysics.collision.shapes;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
#include <ephysics/collision/shapes/ConvexShape.hpp>
namespace ephysics {
/** /**
* @brief Raycast test side for the triangle * @brief Raycast test side for the triangle
*/ */
@ -27,16 +15,16 @@ namespace ephysics {
*/ */
class TriangleShape: public ConvexShape { class TriangleShape: public ConvexShape {
protected: protected:
vec3 this.points[3]; //!< Three points of the triangle Vector3f this.points[3]; //!< Three points of the triangle
TriangleRaycastSide this.raycastTestType; //!< Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
/// Private copy-ructor /// Private copy-ructor
TriangleShape( TriangleShape shape); TriangleShape( TriangleShape shape);
/// Private assignment operator /// Private assignment operator
TriangleShape operator=( TriangleShape shape); TriangleShape operator=( TriangleShape shape);
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override; Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override; boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override; boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
sizet getSizeInBytes() override; long getSizeInBytes() ;
public: public:
/** /**
* @brief Constructor * @brief Constructor
@ -45,14 +33,14 @@ namespace ephysics {
* @param point3 Third point of the triangle * @param point3 Third point of the triangle
* @param margin The collision margin (in meters) around the collision shape * @param margin The collision margin (in meters) around the collision shape
*/ */
TriangleShape( vec3 point1, TriangleShape( Vector3f point1,
vec3 point2, Vector3f point2,
vec3 point3, Vector3f point3,
float margin = OBJECTMARGIN); float margin = OBJECTMARGIN);
void getLocalBounds(vec3 min, vec3 max) override; void getLocalBounds(Vector3f min, Vector3f max) ;
void setLocalScaling( vec3 scaling) override; void setLocalScaling( Vector3f scaling) ;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override; void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
void computeAABB(AABB aabb, etk::Transform3D transform) override; void computeAABB(AABB aabb, Transform3D transform) ;
/// Return the raycast test type (front, back, front-back) /// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() ; TriangleRaycastSide getRaycastTestType() ;
/** /**
@ -64,7 +52,7 @@ namespace ephysics {
* @brief Return the coordinates of a given vertex of the triangle * @brief Return the coordinates of a given vertex of the triangle
* @param[in] index Index (0 to 2) of a 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 ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback; friend class TriangleOverlapCallback;
}; };

View File

@ -1,99 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <etk/types.hpp>
#include <etk/Pair.hpp>
/// Namespace ephysics
namespace ephysics {
// ------------------- Type definitions ------------------- //
typedef long long;
typedef etk::Pair<long, long> 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;
}

View File

@ -17,41 +17,41 @@ using namespace ephysics;
// Constructor // Constructor
BallAndSocketJoint::BallAndSocketJoint( BallAndSocketJointInfo jointInfo) 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 // Compute the local-space anchor point for each body
this.localAnchorPointBody1 = this.body1->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody1 = this.body1.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
this.localAnchorPointBody2 = this.body2->getTransform().getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody2 = this.body2.getTransform().getInverse() * jointInfo.anchorPointWorldSpace;
} }
// Initialize before solving the raint // Initialize before solving the raint
void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
// Initialize the bodies index in the velocity array // Initialize the bodies index in the velocity array
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
// Get the bodies center of mass and orientations // Get the bodies center of mass and orientations
vec3 x1 = this.body1->this.centerOfMassWorld; Vector3f x1 = this.body1.this.centerOfMassWorld;
vec3 x2 = this.body2->this.centerOfMassWorld; Vector3f x2 = this.body2.this.centerOfMassWorld;
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = orientationBody1 * this.localAnchorPointBody1; this.r1World = orientationBody1 * this.localAnchorPointBody1;
this.r2World = orientationBody2 * this.localAnchorPointBody2; this.r2World = orientationBody2 * this.localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) // Compute the matrix K=JM^-1J^t (3x3 matrix)
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
@ -59,7 +59,7 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData)
// Compute the inverse mass matrix K^-1 // Compute the inverse mass matrix K^-1
this.inverseMassMatrix.setZero(); 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(); this.inverseMassMatrix = massMatrix.getInverse();
} }
@ -82,24 +82,24 @@ void BallAndSocketJoint::initBeforeSolve( ConstraintSolverData raintSolverData)
void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) { void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
vec3 linearImpulseBody1 = -this.impulse; Vector3f linearImpulseBody1 = -this.impulse;
vec3 angularImpulseBody1 = this.impulse.cross(this.r1World); Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += this.body1->this.massInverse * linearImpulseBody1; v1 += this.body1.this.massInverse * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // 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 // 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; w2 += this.i2 * angularImpulseBody2;
} }
@ -107,31 +107,31 @@ void BallAndSocketJoint::warmstart( ConstraintSolverData raintSolverData) {
void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { void BallAndSocketJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Compute J*v // 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 // Compute the Lagrange multiplier lambda
vec3 deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector); Vector3f deltaLambda = this.inverseMassMatrix * (-Jv - this.biasVector);
this.impulse += deltaLambda; this.impulse += deltaLambda;
// Compute the impulse P=J^T * lambda for the body 1 // Compute the impulse P=J^T * lambda for the body 1
vec3 linearImpulseBody1 = -deltaLambda; Vector3f linearImpulseBody1 = -deltaLambda;
vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World); Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += this.body1->this.massInverse * linearImpulseBody1; v1 += this.body1.this.massInverse * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the body 2 // 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 // Apply the impulse to the body 2
v2 += this.body2->this.massInverse * deltaLambda; v2 += this.body2.this.massInverse * deltaLambda;
w2 += this.i2 * angularImpulseBody2; w2 += this.i2 * angularImpulseBody2;
} }
@ -143,70 +143,70 @@ void BallAndSocketJoint::solvePositionConstraint( ConstraintSolverData raintSolv
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
// Get the bodies center of mass and orientations // Get the bodies center of mass and orientations
vec3 x1 = raintSolverData.positions[this.indexBody1]; Vector3f x1 = raintSolverData.positions[this.indexBody1];
vec3 x2 = raintSolverData.positions[this.indexBody2]; Vector3f x2 = raintSolverData.positions[this.indexBody2];
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; Quaternion q1 = raintSolverData.orientations[this.indexBody1];
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; Quaternion q2 = raintSolverData.orientations[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = q1 * this.localAnchorPointBody1; this.r1World = q1 * this.localAnchorPointBody1;
this.r2World = q2 * this.localAnchorPointBody2; this.r2World = q2 * this.localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints // Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
float inverseMassBodies = inverseMassBody1 + inverseMassBody2; float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) + 0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
this.inverseMassMatrix.setZero(); 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(); this.inverseMassMatrix = massMatrix.getInverse();
} }
// Compute the raint error (value of the C(x) function) // 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 // Compute the Lagrange multiplier lambda
// TODO : Do not solve the system by computing the inverse each time and multiplying with the // 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. // 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 // Compute the impulse of body 1
vec3 linearImpulseBody1 = -lambda; Vector3f linearImpulseBody1 = -lambda;
vec3 angularImpulseBody1 = lambda.cross(this.r1World); Vector3f angularImpulseBody1 = lambda.cross(this.r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body center of mass and orientation of body 1 // Update the body center of mass and orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // 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 // Compute the pseudo velocity of body 2
vec3 v2 = inverseMassBody2 * lambda; Vector3f v2 = inverseMassBody2 * lambda;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
} }

View File

@ -1,68 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/raint/Joint.hpp>
#include <ephysics/mathematics/mathematics.hpp>
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);
};
}

View File

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

View File

@ -14,22 +14,22 @@ using namespace std;
// Constructor // Constructor
ContactPoint::ContactPoint( ContactPointInfo contactInfo): ContactPoint::ContactPoint( ContactPointInfo contactInfo):
this.body1(contactInfo.shape1->getBody()), this.body1(contactInfo.shape1.getBody()),
this.body2(contactInfo.shape2->getBody()), this.body2(contactInfo.shape2.getBody()),
this.normal(contactInfo.normal), this.normal(contactInfo.normal),
this.penetrationDepth(contactInfo.penetrationDepth), this.penetrationDepth(contactInfo.penetrationDepth),
this.localPointOnBody1(contactInfo.localPoint1), this.localPointOnBody1(contactInfo.localPoint1),
this.localPointOnBody2(contactInfo.localPoint2), this.localPointOnBody2(contactInfo.localPoint2),
this.worldPointOnBody1(contactInfo.shape1->getBody()->getTransform() * this.worldPointOnBody1(contactInfo.shape1.getBody().getTransform() *
contactInfo.shape1->getLocalToBodyTransform() * contactInfo.shape1.getLocalToBodyTransform() *
contactInfo.localPoint1), contactInfo.localPoint1),
this.worldPointOnBody2(contactInfo.shape2->getBody()->getTransform() * this.worldPointOnBody2(contactInfo.shape2.getBody().getTransform() *
contactInfo.shape2->getLocalToBodyTransform() * contactInfo.shape2.getLocalToBodyTransform() *
contactInfo.localPoint2), contactInfo.localPoint2),
this.isRestingContact(false) { this.isRestingContact(false) {
this.frictionVectors[0] = vec3(0, 0, 0); this.frictionVectors[0] = Vector3f(0, 0, 0);
this.frictionVectors[1] = vec3(0, 0, 0); this.frictionVectors[1] = Vector3f(0, 0, 0);
assert(this.penetrationDepth >= 0.0); assert(this.penetrationDepth >= 0.0);
@ -51,32 +51,32 @@ CollisionBody* ContactPoint::getBody2() {
} }
// Return the normal vector of the contact // Return the normal vector of the contact
vec3 ContactPoint::getNormal() { Vector3f ContactPoint::getNormal() {
return this.normal; return this.normal;
} }
// Set the penetration depth of the contact // Set the penetration depth of the contact
void ContactPoint::setPenetrationDepth(float penetrationDepth) { void ContactPoint::setPenetrationDepth(float penetrationDepth) {
this->this.penetrationDepth = penetrationDepth; this.this.penetrationDepth = penetrationDepth;
} }
// Return the contact point on body 1 // Return the contact point on body 1
vec3 ContactPoint::getLocalPointOnBody1() { Vector3f ContactPoint::getLocalPointOnBody1() {
return this.localPointOnBody1; return this.localPointOnBody1;
} }
// Return the contact point on body 2 // Return the contact point on body 2
vec3 ContactPoint::getLocalPointOnBody2() { Vector3f ContactPoint::getLocalPointOnBody2() {
return this.localPointOnBody2; return this.localPointOnBody2;
} }
// Return the contact world point on body 1 // Return the contact world point on body 1
vec3 ContactPoint::getWorldPointOnBody1() { Vector3f ContactPoint::getWorldPointOnBody1() {
return this.worldPointOnBody1; return this.worldPointOnBody1;
} }
// Return the contact world point on body 2 // Return the contact world point on body 2
vec3 ContactPoint::getWorldPointOnBody2() { Vector3f ContactPoint::getWorldPointOnBody2() {
return this.worldPointOnBody2; return this.worldPointOnBody2;
} }
@ -96,7 +96,7 @@ float ContactPoint::getFrictionImpulse2() {
} }
// Return the cached rolling resistance impulse // Return the cached rolling resistance impulse
vec3 ContactPoint::getRollingResistanceImpulse() { Vector3f ContactPoint::getRollingResistanceImpulse() {
return this.rollingResistanceImpulse; return this.rollingResistanceImpulse;
} }
@ -116,17 +116,17 @@ void ContactPoint::setFrictionImpulse2(float impulse) {
} }
// Set the cached rolling resistance impulse // Set the cached rolling resistance impulse
void ContactPoint::setRollingResistanceImpulse( vec3 impulse) { void ContactPoint::setRollingResistanceImpulse( Vector3f impulse) {
this.rollingResistanceImpulse = impulse; this.rollingResistanceImpulse = impulse;
} }
// Set the contact world point on body 1 // Set the contact world point on body 1
void ContactPoint::setWorldPointOnBody1( vec3 worldPoint) { void ContactPoint::setWorldPointOnBody1( Vector3f worldPoint) {
this.worldPointOnBody1 = worldPoint; this.worldPointOnBody1 = worldPoint;
} }
// Set the contact world point on body 2 // Set the contact world point on body 2
void ContactPoint::setWorldPointOnBody2( vec3 worldPoint) { void ContactPoint::setWorldPointOnBody2( Vector3f worldPoint) {
this.worldPointOnBody2 = worldPoint; this.worldPointOnBody2 = worldPoint;
} }
@ -141,22 +141,22 @@ void ContactPoint::setIsRestingContact(boolean isRestingContact) {
} }
// Get the first friction vector // Get the first friction vector
vec3 ContactPoint::getFrictionVector1() { Vector3f ContactPoint::getFrictionVector1() {
return this.frictionVectors[0]; return this.frictionVectors[0];
} }
// Set the first friction vector // Set the first friction vector
void ContactPoint::setFrictionVector1( vec3 frictionVector1) { void ContactPoint::setFrictionVector1( Vector3f frictionVector1) {
this.frictionVectors[0] = frictionVector1; this.frictionVectors[0] = frictionVector1;
} }
// Get the second friction vector // Get the second friction vector
vec3 ContactPoint::getFrictionvec2() { Vector3f ContactPoint::getFrictionvec2() {
return this.frictionVectors[1]; return this.frictionVectors[1];
} }
// Set the second friction vector // Set the second friction vector
void ContactPoint::setFrictionvec2( vec3 frictionvec2) { void ContactPoint::setFrictionvec2( Vector3f frictionvec2) {
this.frictionVectors[1] = frictionvec2; this.frictionVectors[1] = frictionvec2;
} }
@ -166,6 +166,6 @@ float ContactPoint::getPenetrationDepth() {
} }
// Return the number of bytes used by the contact point // Return the number of bytes used by the contact point
sizet ContactPoint::getSizeInBytes() { long ContactPoint::getSizeInBytes() {
return sizeof(ContactPoint); return sizeof(ContactPoint);
} }

View File

@ -1,20 +1,4 @@
/** @file package org.atriaSoft.ephysics.constraint;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/body/CollisionBody.hpp>
#include <ephysics/collision/CollisionShapeInfo.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/mathematics/mathematics.hpp>
#include <ephysics/configuration.hpp>
namespace ephysics {
/** /**
* @brief This structure contains informations about a collision contact * @brief This structure contains informations about a collision contact
@ -28,18 +12,18 @@ namespace ephysics {
ProxyShape* shape2; //!< Second proxy shape of the contact ProxyShape* shape2; //!< Second proxy shape of the contact
CollisionShape* collisionShape1; //!< First collision shape CollisionShape* collisionShape1; //!< First collision shape
CollisionShape* collisionShape2; //!< Second 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 float penetrationDepth; //!< Penetration depth of the contact
vec3 localPoint1; //!< Contact point of body 1 in local space of body 1 Vector3f 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 localPoint2; //!< Contact point of body 2 in local space of body 2
ContactPointInfo(ProxyShape* proxyShape1, ContactPointInfo(ProxyShape* proxyShape1,
ProxyShape* proxyShape2, ProxyShape* proxyShape2,
CollisionShape* collShape1, CollisionShape* collShape1,
CollisionShape* collShape2, CollisionShape* collShape2,
vec3 normal, Vector3f normal,
float penetrationDepth, float penetrationDepth,
vec3 localPoint1, Vector3f localPoint1,
vec3 localPoint2): Vector3f localPoint2):
shape1(proxyShape1), shape1(proxyShape1),
shape2(proxyShape2), shape2(proxyShape2),
collisionShape1(collShape1), collisionShape1(collShape1),
@ -55,7 +39,7 @@ namespace ephysics {
shape2(null), shape2(null),
collisionShape1(null), collisionShape1(null),
collisionShape2(null) { collisionShape2(null) {
// TODO: add it for etk::Vector // TODO: add it for Vector
} }
}; };
@ -65,45 +49,39 @@ namespace ephysics {
*/ */
class ContactPoint { class ContactPoint {
private : private :
CollisionBody* this.body1; //!< First rigid body of the contact CollisionBody* body1; //!< First rigid body of the contact
CollisionBody* this.body2; //!< Second rigid body of the contact CollisionBody* body2; //!< Second rigid body of the contact
vec3 this.normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
float this.penetrationDepth; //!< Penetration depth float penetrationDepth; //!< Penetration depth
vec3 this.localPointOnBody1; //!< Contact point on body 1 in local space of body 1 Vector3f 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 Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2
vec3 this.worldPointOnBody1; //!< Contact point on body 1 in world space Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
vec3 this.worldPointOnBody2; //!< Contact point on body 2 in world space Vector3f 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) boolean 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 Vector3f frictionVectors[2]; //!< Two orthogonal vectors that span the tangential friction plane
float this.penetrationImpulse; //!< Cached penetration impulse float penetrationImpulse; //!< Cached penetration impulse
float this.frictionImpulse1; //!< Cached first friction impulse float frictionImpulse1; //!< Cached first friction impulse
float this.frictionImpulse2; //!< Cached second friction impulse float frictionImpulse2; //!< Cached second friction impulse
vec3 this.rollingResistanceImpulse; //!< Cached rolling resistance impulse Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse
/// Private copy-ructor
ContactPoint( ContactPoint contact) = delete;
/// Private assignment operator
ContactPoint operator=( ContactPoint contact) = delete;
public : public :
/// Constructor /// Constructor
ContactPoint( ContactPointInfo contactInfo); ContactPoint( ContactPointInfo contactInfo);
/// Destructor
~ContactPoint();
/// Return the reference to the body 1 /// Return the reference to the body 1
CollisionBody* getBody1() ; CollisionBody* getBody1() ;
/// Return the reference to the body 2 /// Return the reference to the body 2
CollisionBody* getBody2() ; CollisionBody* getBody2() ;
/// Return the normal vector of the contact /// Return the normal vector of the contact
vec3 getNormal() ; Vector3f getNormal() ;
/// Set the penetration depth of the contact /// Set the penetration depth of the contact
void setPenetrationDepth(float penetrationDepth); void setPenetrationDepth(float penetrationDepth);
/// Return the contact local point on body 1 /// Return the contact local point on body 1
vec3 getLocalPointOnBody1() ; Vector3f getLocalPointOnBody1() ;
/// Return the contact local point on body 2 /// Return the contact local point on body 2
vec3 getLocalPointOnBody2() ; Vector3f getLocalPointOnBody2() ;
/// Return the contact world point on body 1 /// Return the contact world point on body 1
vec3 getWorldPointOnBody1() ; Vector3f getWorldPointOnBody1() ;
/// Return the contact world point on body 2 /// Return the contact world point on body 2
vec3 getWorldPointOnBody2() ; Vector3f getWorldPointOnBody2() ;
/// Return the cached penetration impulse /// Return the cached penetration impulse
float getPenetrationImpulse() ; float getPenetrationImpulse() ;
/// Return the cached first friction impulse /// Return the cached first friction impulse
@ -111,7 +89,7 @@ namespace ephysics {
/// Return the cached second friction impulse /// Return the cached second friction impulse
float getFrictionImpulse2() ; float getFrictionImpulse2() ;
/// Return the cached rolling resistance impulse /// Return the cached rolling resistance impulse
vec3 getRollingResistanceImpulse() ; Vector3f getRollingResistanceImpulse() ;
/// Set the cached penetration impulse /// Set the cached penetration impulse
void setPenetrationImpulse(float impulse); void setPenetrationImpulse(float impulse);
/// Set the first cached friction impulse /// Set the first cached friction impulse
@ -119,27 +97,27 @@ namespace ephysics {
/// Set the second cached friction impulse /// Set the second cached friction impulse
void setFrictionImpulse2(float impulse); void setFrictionImpulse2(float impulse);
/// Set the cached rolling resistance impulse /// Set the cached rolling resistance impulse
void setRollingResistanceImpulse( vec3 impulse); void setRollingResistanceImpulse( Vector3f impulse);
/// Set the contact world point on body 1 /// Set the contact world point on body 1
void setWorldPointOnBody1( vec3 worldPoint); void setWorldPointOnBody1( Vector3f worldPoint);
/// Set the contact world point on body 2 /// 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 /// Return true if the contact is a resting contact
boolean getIsRestingContact() ; boolean getIsRestingContact() ;
/// Set the this.isRestingContact variable /// Set the this.isRestingContact variable
void setIsRestingContact(boolean isRestingContact); void setIsRestingContact(boolean isRestingContact);
/// Get the first friction vector /// Get the first friction vector
vec3 getFrictionVector1() ; Vector3f getFrictionVector1() ;
/// Set the first friction vector /// Set the first friction vector
void setFrictionVector1( vec3 frictionVector1); void setFrictionVector1( Vector3f frictionVector1);
/// Get the second friction vector /// Get the second friction vector
vec3 getFrictionvec2() ; Vector3f getFrictionvec2() ;
/// Set the second friction vector /// Set the second friction vector
void setFrictionvec2( vec3 frictionvec2); void setFrictionvec2( Vector3f frictionvec2);
/// Return the penetration depth /// Return the penetration depth
float getPenetrationDepth() ; float getPenetrationDepth() ;
/// Return the number of bytes used by the contact point /// Return the number of bytes used by the contact point
sizet getSizeInBytes() ; long getSizeInBytes() ;
}; };
} }

View File

@ -19,10 +19,10 @@ FixedJoint::FixedJoint( FixedJointInfo jointInfo)
: Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) { : Joint(jointInfo), this.impulseTranslation(0, 0, 0), this.impulseRotation(0, 0, 0) {
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
etk::Transform3D transform1 = this.body1->getTransform(); Transform3D transform1 = this.body1.getTransform();
etk::Transform3D transform2 = this.body2->getTransform(); Transform3D transform2 = this.body2.getTransform();
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
this.initOrientationDifferenceInv = transform2.getOrientation() * this.initOrientationDifferenceInv = transform2.getOrientation() *
@ -40,30 +40,30 @@ FixedJoint::~FixedJoint() {
void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
// Initialize the bodies index in the velocity array // Initialize the bodies index in the velocity array
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = this.body1->this.centerOfMassWorld; Vector3f x1 = this.body1.this.centerOfMassWorld;
vec3 x2 = this.body2->this.centerOfMassWorld; Vector3f x2 = this.body2.this.centerOfMassWorld;
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = orientationBody1 * this.localAnchorPointBody1; this.r1World = orientationBody1 * this.localAnchorPointBody1;
this.r2World = orientationBody2 * this.localAnchorPointBody2; this.r2World = orientationBody2 * this.localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) 0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + 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 // Compute the inverse mass matrix K^-1 for the 3 translation raints
this.inverseMassMatrixTranslation.setZero(); 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(); 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 // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
this.inverseMassMatrixRotation = this.i1 + this.i2; 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(); this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
} }
// Compute the bias "b" for the 3 rotation raints // Compute the bias "b" for the 3 rotation raints
this.biasRotation.setZero(); this.biasRotation.setZero();
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) { if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
this.biasRotation = biasFactor * float(2.0) * qError.getVectorV(); this.biasRotation = biasFactor * float(2.0) * qError.getVectorV();
} }
@ -111,18 +111,18 @@ void FixedJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
void FixedJoint::warmstart( ConstraintSolverData raintSolverData) { void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1 // Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
vec3 linearImpulseBody1 = -this.impulseTranslation; Vector3f linearImpulseBody1 = -this.impulseTranslation;
vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
angularImpulseBody1 += -this.impulseRotation; angularImpulseBody1 += -this.impulseRotation;
@ -132,7 +132,7 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2 // 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 // Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
angularImpulseBody2 += this.impulseRotation; angularImpulseBody2 += this.impulseRotation;
@ -146,35 +146,35 @@ void FixedJoint::warmstart( ConstraintSolverData raintSolverData) {
void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass of the bodies // Get the inverse mass of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation raints // 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 // Compute the Lagrange multiplier lambda
vec3 deltaLambda = this.inverseMassMatrixTranslation * Vector3f deltaLambda = this.inverseMassMatrixTranslation *
(-JvTranslation - this.biasTranslation); (-JvTranslation - this.biasTranslation);
this.impulseTranslation += deltaLambda; this.impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for body 1 // Compute the impulse P=J^T * lambda for body 1
vec3 linearImpulseBody1 = -deltaLambda; Vector3f linearImpulseBody1 = -deltaLambda;
vec3 angularImpulseBody1 = deltaLambda.cross(this.r1World); Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2 // 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 // Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda; v2 += inverseMassBody2 * deltaLambda;
@ -183,10 +183,10 @@ void FixedJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation raints // 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 // 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; this.impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1 // 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; if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = raintSolverData.positions[this.indexBody1]; Vector3f x1 = raintSolverData.positions[this.indexBody1];
vec3 x2 = raintSolverData.positions[this.indexBody2]; Vector3f x2 = raintSolverData.positions[this.indexBody2];
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; Quaternion q1 = raintSolverData.orientations[this.indexBody1];
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; Quaternion q2 = raintSolverData.orientations[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = q1 * this.localAnchorPointBody1; this.r1World = q1 * this.localAnchorPointBody1;
this.r2World = q2 * this.localAnchorPointBody2; this.r2World = q2 * this.localAnchorPointBody2;
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) 0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
this.inverseMassMatrixTranslation.setZero(); 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(); this.inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute position error for the 3 translation raints // 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 // Compute the Lagrange multiplier lambda
vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1 // Compute the impulse of body 1
vec3 linearImpulseBody1 = -lambdaTranslation; Vector3f linearImpulseBody1 = -lambdaTranslation;
vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World); Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // 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 // Compute the pseudo velocity of body 2
vec3 v2 = inverseMassBody2 * lambdaTranslation; Vector3f v2 = inverseMassBody2 * lambdaTranslation;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- 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 // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
this.inverseMassMatrixRotation = this.i1 + this.i2; 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(); this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.getInverse();
} }
// Compute the position error for the 3 rotation raints // Compute the position error for the 3 rotation raints
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
vec3 errorRotation = float(2.0) * qError.getVectorV(); Vector3f errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation raints // 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 // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
angularImpulseBody1 = -lambdaRotation; angularImpulseBody1 = -lambdaRotation;
@ -298,14 +298,14 @@ void FixedJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
w1 = this.i1 * angularImpulseBody1; w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // 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(); q1.normalize();
// Compute the pseudo velocity of body 2 // Compute the pseudo velocity of body 2
w2 = this.i2 * lambdaRotation; w2 = this.i2 * lambdaRotation;
// Update the body position/orientation of body 2 // 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(); q2.normalize();
} }

View File

@ -1,78 +0,0 @@
/** @file
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/raint/Joint.hpp>
#include <ephysics/mathematics/mathematics.hpp>
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();
};
}

View File

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

View File

@ -23,14 +23,14 @@ HingeJoint::HingeJoint( HingeJointInfo jointInfo)
this.isLowerLimitViolated(false), this.isUpperLimitViolated(false), this.isLowerLimitViolated(false), this.isUpperLimitViolated(false),
this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) { this.motorSpeed(jointInfo.motorSpeed), this.maxMotorTorque(jointInfo.maxMotorTorque) {
assert(this.lowerLimit <= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lowerLimit >= -2.0 * PI); assert(this.lowerLimit <= 0 && this.lowerLimit >= -2.0 * PI);
assert(this.upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.upperLimit <= 2.0 * PI); assert(this.upperLimit >= 0 && this.upperLimit <= 2.0 * PI);
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
etk::Transform3D transform1 = this.body1->getTransform(); Transform3D transform1 = this.body1.getTransform();
etk::Transform3D transform2 = this.body2->getTransform(); Transform3D transform2 = this.body2.getTransform();
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the local-space hinge axis // Compute the local-space hinge axis
this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld; this.hingeLocalAxisBody1 = transform1.getOrientation().getInverse() * jointInfo.rotationAxisWorld;
@ -54,18 +54,18 @@ HingeJoint::~HingeJoint() {
void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
// Initialize the bodies index in the velocity array // Initialize the bodies index in the velocity array
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = this.body1->this.centerOfMassWorld; Vector3f x1 = this.body1.this.centerOfMassWorld;
vec3 x2 = this.body2->this.centerOfMassWorld; Vector3f x2 = this.body2.this.centerOfMassWorld;
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = orientationBody1 * this.localAnchorPointBody1; this.r1World = orientationBody1 * this.localAnchorPointBody1;
@ -90,27 +90,27 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
mA1 = orientationBody1 * this.hingeLocalAxisBody1; mA1 = orientationBody1 * this.hingeLocalAxisBody1;
vec3 a2 = orientationBody2 * this.hingeLocalAxisBody2; Vector3f a2 = orientationBody2 * this.hingeLocalAxisBody2;
mA1.normalize(); mA1.normalize();
a2.normalize(); a2.normalize();
vec3 b2 = a2.getOrthoVector(); Vector3f b2 = a2.getOrthoVector();
vec3 c2 = a2.cross(b2); Vector3f c2 = a2.cross(b2);
this.b2CrossA1 = b2.cross(mA1); this.b2CrossA1 = b2.cross(mA1);
this.c2CrossA1 = c2.cross(mA1); this.c2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix) // 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; float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) 0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
this.inverseMassMatrixTranslation.setZero(); 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(); 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) // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1; Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1; Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1; Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1; Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
float el11 = this.b2CrossA1.dot(I1B2CrossA1) + float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
this.b2CrossA1.dot(I2B2CrossA1); this.b2CrossA1.dot(I2B2CrossA1);
float el12 = this.b2CrossA1.dot(I1C2CrossA1) + float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
@ -134,9 +134,9 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
this.c2CrossA1.dot(I2B2CrossA1); this.c2CrossA1.dot(I2B2CrossA1);
float el22 = this.c2CrossA1.dot(I1C2CrossA1) + float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
this.c2CrossA1.dot(I2C2CrossA1); this.c2CrossA1.dot(I2C2CrossA1);
etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); Matrix2x2 matrixKRotation(el11, el12, el21, el22);
this.inverseMassMatrixRotation.setZero(); 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(); this.inverseMassMatrixRotation = matrixKRotation.getInverse();
} }
@ -158,7 +158,7 @@ void HingeJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
} }
// If the motor or limits are enabled // 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) // 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); 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) { void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Compute the impulse P=J^T * lambda for the 2 rotation raints // 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 // 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 // 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 // Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
vec3 linearImpulseBody1 = -this.impulseTranslation; Vector3f linearImpulseBody1 = -this.impulseTranslation;
vec3 angularImpulseBody1 = this.impulseTranslation.cross(this.r1World); Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1 // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
angularImpulseBody1 += rotationImpulse; angularImpulseBody1 += rotationImpulse;
@ -222,7 +222,7 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2 // 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 // Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
angularImpulseBody2 += -rotationImpulse; angularImpulseBody2 += -rotationImpulse;
@ -242,35 +242,35 @@ void HingeJoint::warmstart( ConstraintSolverData raintSolverData) {
void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) { void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute J*v // 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 // Compute the Lagrange multiplier lambda
vec3 deltaLambdaTranslation = this.inverseMassMatrixTranslation * Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation *
(-JvTranslation - this.bTranslation); (-JvTranslation - this.bTranslation);
this.impulseTranslation += deltaLambdaTranslation; this.impulseTranslation += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda of body 1 // Compute the impulse P=J^T * lambda of body 1
vec3 linearImpulseBody1 = -deltaLambdaTranslation; Vector3f linearImpulseBody1 = -deltaLambdaTranslation;
vec3 angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World); Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda of body 2 // 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 // Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambdaTranslation; v2 += inverseMassBody2 * deltaLambdaTranslation;
@ -313,17 +313,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
// Compute the Lagrange multiplier lambda for the lower limit raint // Compute the Lagrange multiplier lambda for the lower limit raint
float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit); float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit -this.bLowerLimit);
float lambdaTemp = this.impulseLowerLimit; 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; deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1 // 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 // Apply the impulse to the body 1
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2 // 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 // Apply the impulse to the body 2
w2 += this.i2 * angularImpulseBody2; w2 += this.i2 * angularImpulseBody2;
@ -338,17 +338,17 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
// Compute the Lagrange multiplier lambda for the upper limit raint // Compute the Lagrange multiplier lambda for the upper limit raint
float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit); float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit -this.bUpperLimit);
float lambdaTemp = this.impulseUpperLimit; 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; deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1 // 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 // Apply the impulse to the body 1
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2 // 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 // Apply the impulse to the body 2
w2 += this.i2 * angularImpulseBody2; w2 += this.i2 * angularImpulseBody2;
@ -371,13 +371,13 @@ void HingeJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
deltaLambdaMotor = this.impulseMotor - lambdaTemp; deltaLambdaMotor = this.impulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1 // 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 // Apply the impulse to the body 1
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2 // 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 // Apply the impulse to the body 2
w2 += this.i2 * angularImpulseBody2; w2 += this.i2 * angularImpulseBody2;
@ -392,18 +392,18 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = raintSolverData.positions[this.indexBody1]; Vector3f x1 = raintSolverData.positions[this.indexBody1];
vec3 x2 = raintSolverData.positions[this.indexBody2]; Vector3f x2 = raintSolverData.positions[this.indexBody2];
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; Quaternion q1 = raintSolverData.orientations[this.indexBody1];
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; Quaternion q2 = raintSolverData.orientations[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Compute the vector from body center to the anchor point in world-space // Compute the vector from body center to the anchor point in world-space
this.r1World = q1 * this.localAnchorPointBody1; this.r1World = q1 * this.localAnchorPointBody1;
@ -420,70 +420,70 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
// Compute vectors needed in the Jacobian // Compute vectors needed in the Jacobian
mA1 = q1 * this.hingeLocalAxisBody1; mA1 = q1 * this.hingeLocalAxisBody1;
vec3 a2 = q2 * this.hingeLocalAxisBody2; Vector3f a2 = q2 * this.hingeLocalAxisBody2;
mA1.normalize(); mA1.normalize();
a2.normalize(); a2.normalize();
vec3 b2 = a2.getOrthoVector(); Vector3f b2 = a2.getOrthoVector();
vec3 c2 = a2.cross(b2); Vector3f c2 = a2.cross(b2);
this.b2CrossA1 = b2.cross(mA1); this.b2CrossA1 = b2.cross(mA1);
this.c2CrossA1 = c2.cross(mA1); this.c2CrossA1 = c2.cross(mA1);
// Compute the corresponding skew-symmetric matrices // Compute the corresponding skew-symmetric matrices
etk::Matrix3x3 skewSymmetricMatrixU1= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r1World); Matrix3f skewSymmetricMatrixU1= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r1World);
etk::Matrix3x3 skewSymmetricMatrixU2= etk::Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(this.r2World); Matrix3f skewSymmetricMatrixU2= Matrix3f::computeSkewSymmetricMatrixForCrossProduct(this.r2World);
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
float inverseMassBodies = this.body1->this.massInverse + this.body2->this.massInverse; float inverseMassBodies = this.body1.this.massInverse + this.body2.this.massInverse;
etk::Matrix3x3 massMatrix = etk::Matrix3x3(inverseMassBodies, 0, 0, Matrix3f massMatrix = Matrix3f(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0, 0, inverseMassBodies, 0,
0, 0, inverseMassBodies) 0, 0, inverseMassBodies)
+ skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose() + skewSymmetricMatrixU1 * this.i1 * skewSymmetricMatrixU1.getTranspose()
+ skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose(); + skewSymmetricMatrixU2 * this.i2 * skewSymmetricMatrixU2.getTranspose();
this.inverseMassMatrixTranslation.setZero(); 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(); this.inverseMassMatrixTranslation = massMatrix.getInverse();
} }
// Compute position error for the 3 translation raints // 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 // Compute the Lagrange multiplier lambda
vec3 lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation); Vector3f lambdaTranslation = this.inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1 // Compute the impulse of body 1
vec3 linearImpulseBody1 = -lambdaTranslation; Vector3f linearImpulseBody1 = -lambdaTranslation;
vec3 angularImpulseBody1 = lambdaTranslation.cross(this.r1World); Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
// Compute the pseudo velocity of body 1 // Compute the pseudo velocity of body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse of body 2 // 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 // Compute the pseudo velocity of body 2
vec3 v2 = inverseMassBody2 * lambdaTranslation; Vector3f v2 = inverseMassBody2 * lambdaTranslation;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix) // Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
vec3 I1B2CrossA1 = this.i1 * this.b2CrossA1; Vector3f I1B2CrossA1 = this.i1 * this.b2CrossA1;
vec3 I1C2CrossA1 = this.i1 * this.c2CrossA1; Vector3f I1C2CrossA1 = this.i1 * this.c2CrossA1;
vec3 I2B2CrossA1 = this.i2 * this.b2CrossA1; Vector3f I2B2CrossA1 = this.i2 * this.b2CrossA1;
vec3 I2C2CrossA1 = this.i2 * this.c2CrossA1; Vector3f I2C2CrossA1 = this.i2 * this.c2CrossA1;
float el11 = this.b2CrossA1.dot(I1B2CrossA1) + float el11 = this.b2CrossA1.dot(I1B2CrossA1) +
this.b2CrossA1.dot(I2B2CrossA1); this.b2CrossA1.dot(I2B2CrossA1);
float el12 = this.b2CrossA1.dot(I1C2CrossA1) + float el12 = this.b2CrossA1.dot(I1C2CrossA1) +
@ -492,9 +492,9 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
this.c2CrossA1.dot(I2B2CrossA1); this.c2CrossA1.dot(I2B2CrossA1);
float el22 = this.c2CrossA1.dot(I1C2CrossA1) + float el22 = this.c2CrossA1.dot(I1C2CrossA1) +
this.c2CrossA1.dot(I2C2CrossA1); this.c2CrossA1.dot(I2C2CrossA1);
etk::Matrix2x2 matrixKRotation(el11, el12, el21, el22); Matrix2x2 matrixKRotation(el11, el12, el21, el22);
this.inverseMassMatrixRotation.setZero(); 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(); this.inverseMassMatrixRotation = matrixKRotation.getInverse();
} }
@ -511,7 +511,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
w1 = this.i1 * angularImpulseBody1; w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // 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(); q1.normalize();
// Compute the impulse of body 2 // Compute the impulse of body 2
@ -521,7 +521,7 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
w2 = this.i2 * angularImpulseBody2; w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // 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(); q2.normalize();
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -543,23 +543,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError ); float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1 // 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 // 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 // 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(); q1.normalize();
// Compute the impulse P=J^T * lambda of body 2 // 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 // 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 // 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(); q2.normalize();
} }
@ -570,23 +570,23 @@ void HingeJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError); float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1 // 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 // 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 // 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(); q1.normalize();
// Compute the impulse P=J^T * lambda of body 2 // 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 // 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 // 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(); q2.normalize();
} }
} }
@ -620,8 +620,8 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) {
this.impulseMotor = 0.0; this.impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
// Set the minimum angle limit // Set the minimum angle limit
@ -630,7 +630,7 @@ void HingeJoint::enableMotor(boolean isMotorEnabled) {
*/ */
void HingeJoint::setMinAngleLimit(float lowerLimit) { 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) { if (lowerLimit != this.lowerLimit) {
@ -647,7 +647,7 @@ void HingeJoint::setMinAngleLimit(float lowerLimit) {
*/ */
void HingeJoint::setMaxAngleLimit(float upperLimit) { void HingeJoint::setMaxAngleLimit(float upperLimit) {
assert(upperLimit >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj upperLimit <= 2.0 * PI); assert(upperLimit >= 0 && upperLimit <= 2.0 * PI);
if (upperLimit != this.upperLimit) { if (upperLimit != this.upperLimit) {
@ -666,8 +666,8 @@ void HingeJoint::resetLimits() {
this.impulseUpperLimit = 0.0; this.impulseUpperLimit = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
// Set the motor speed // Set the motor speed
@ -678,8 +678,8 @@ void HingeJoint::setMotorSpeed(float motorSpeed) {
this.motorSpeed = motorSpeed; this.motorSpeed = motorSpeed;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
} }
@ -695,8 +695,8 @@ void HingeJoint::setMaxMotorTorque(float maxMotorTorque) {
this.maxMotorTorque = maxMotorTorque; this.maxMotorTorque = maxMotorTorque;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->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 // Compute the current angle around the hinge axis
float HingeJoint::computeCurrentHingeAngle( etk::Quaternion orientationBody1, float HingeJoint::computeCurrentHingeAngle( Quaternion orientationBody1,
etk::Quaternion orientationBody2) { Quaternion orientationBody2) {
float hingeAngle; float hingeAngle;
// Compute the current orientation difference between the two bodies // Compute the current orientation difference between the two bodies
etk::Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
currentOrientationDiff.normalize(); currentOrientationDiff.normalize();
// Compute the relative rotation considering the initial orientation difference // Compute the relative rotation considering the initial orientation difference
etk::Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv; Quaternion relativeRotation = currentOrientationDiff * this.initOrientationDifferenceInv;
relativeRotation.normalize(); relativeRotation.normalize();
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit // 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 the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= 0.0f) { if (dotProduct >= 0.0f) {
hingeAngle = float(2.0) * etk::atan2(sinHalfAngleAbs, cosHalfAngle); hingeAngle = float(2.0) * atan2(sinHalfAngleAbs, cosHalfAngle);
} }
else { 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] // 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 // Return the number of bytes used by the joint
sizet HingeJoint::getSizeInBytes() { long HingeJoint::getSizeInBytes() {
return sizeof(HingeJoint); return sizeof(HingeJoint);
} }

View File

@ -1,25 +1,13 @@
/** @file package org.atriaSoft.ephysics.constraint;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/raint/Joint.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/** /**
* @brief It is used to gather the information needed to create a hinge joint. * @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. * This structure will be used to create the actual hinge joint.
*/ */
struct HingeJointInfo : public JointInfo { struct HingeJointInfo extends JointInfo {
public : public :
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
vec3 rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates) Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
boolean isLimitEnabled; //!< True if the hinge joint limits are enabled boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
boolean isMotorEnabled; //!< True if the hinge joint motor is 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] 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, HingeJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initRotationAxisWorld): Vector3f initRotationAxisWorld):
JointInfo(rigidBody1, rigidBody2, HINGEJOINT), JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
this.anchorPointWorldSpace(initAnchorPointWorldSpace), this.anchorPointWorldSpace(initAnchorPointWorldSpace),
rotationAxisWorld(initRotationAxisWorld), rotationAxisWorld(initRotationAxisWorld),
@ -59,8 +47,8 @@ namespace ephysics {
*/ */
HingeJointInfo(RigidBody* rigidBody1, HingeJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initRotationAxisWorld, Vector3f initRotationAxisWorld,
float initMinAngleLimit, float initMinAngleLimit,
float initMaxAngleLimit): float initMaxAngleLimit):
JointInfo(rigidBody1, rigidBody2, HINGEJOINT), JointInfo(rigidBody1, rigidBody2, HINGEJOINT),
@ -87,8 +75,8 @@ namespace ephysics {
*/ */
HingeJointInfo(RigidBody* rigidBody1, HingeJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initRotationAxisWorld, Vector3f initRotationAxisWorld,
float initMinAngleLimit, float initMinAngleLimit,
float initMaxAngleLimit, float initMaxAngleLimit,
float initMotorSpeed, float initMotorSpeed,
@ -111,46 +99,42 @@ namespace ephysics {
* between two bodies around a single axis. This joint has one degree of freedom. It * between two bodies around a single axis. This joint has one degree of freedom. It
* can be useful to simulate doors or pendulumns. * can be useful to simulate doors or pendulumns.
*/ */
class HingeJoint : public Joint { class HingeJoint extends Joint {
private : private :
static float BETA; //!< Beta value for the bias factor of position correction 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) Vector3f 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) Vector3f 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) Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
vec3 this.hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2) Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
vec3 mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1 Vector3f 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 Vector3f 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 Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
vec3 this.b2CrossA1; //!< Cross product of vector b2 and a1 Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
vec3 this.c2CrossA1; //!< Cross product of vector c2 and a1; Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
vec3 this.impulseTranslation; //!< Impulse for the 3 translation raints Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
vec2 this.impulseRotation; //!< Impulse for the 2 rotation raints vec2 impulseRotation; //!< Impulse for the 2 rotation raints
float this.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
float this.impulseMotor; //!< Accumulated impulse for the motor raint; float impulseMotor; //!< Accumulated impulse for the motor raint;
etk::Matrix3x3 this.inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints Matrix3f 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 Matrix2x2 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 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 float 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 Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
vec2 this.bRotation; //!< Bias vector for the error correction for the rotation raints vec2 bRotation; //!< Bias vector for the error correction for the rotation raints
float this.bLowerLimit; //!< Bias of the lower limit raint float bLowerLimit; //!< Bias of the lower limit raint
float this.bUpperLimit; //!< Bias of the upper limit raint float bUpperLimit; //!< Bias of the upper limit raint
etk::Quaternion this.initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
boolean this.isLimitEnabled; //!< True if the joint limits are enabled boolean isLimitEnabled; //!< True if the joint limits are enabled
boolean this.isMotorEnabled; //!< True if the motor of the joint in enabled boolean isMotorEnabled; //!< True if the motor of the joint in enabled
float this.lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian) float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
float this.upperLimit; //!< Upper limit (maximum translation distance) float upperLimit; //!< Upper limit (maximum translation distance)
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated boolean isLowerLimitViolated; //!< True if the lower limit is violated
boolean this.isUpperLimitViolated; //!< True if the upper limit is violated boolean isUpperLimitViolated; //!< True if the upper limit is violated
float this.motorSpeed; //!< Motor speed (in rad/s) float motorSpeed; //!< Motor speed (in rad/s)
float this.maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed float 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);
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Given an angle in radian, this method returns the corresponding /// Given an angle in radian, this method returns the corresponding
@ -163,17 +147,15 @@ namespace ephysics {
float lowerLimitAngle, float lowerLimitAngle,
float upperLimitAngle) ; float upperLimitAngle) ;
/// Compute the current angle around the hinge axis /// Compute the current angle around the hinge axis
float computeCurrentHingeAngle( etk::Quaternion orientationBody1, etk::Quaternion orientationBody2); float computeCurrentHingeAngle( Quaternion orientationBody1, Quaternion orientationBody2);
sizet getSizeInBytes() override; long getSizeInBytes() ;
void initBeforeSolve( ConstraintSolverData raintSolverData) override; void initBeforeSolve( ConstraintSolverData raintSolverData) ;
void warmstart( ConstraintSolverData raintSolverData) override; void warmstart( ConstraintSolverData raintSolverData) ;
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
void solvePositionConstraint( ConstraintSolverData raintSolverData) override; void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
public : public :
/// Constructor /// Constructor
HingeJoint( HingeJointInfo jointInfo); HingeJoint( HingeJointInfo jointInfo);
/// Destructor
virtual ~HingeJoint();
/// Return true if the limits or the joint are enabled /// Return true if the limits or the joint are enabled
boolean isLimitEnabled() ; boolean isLimitEnabled() ;
/// Return true if the motor of the joint is enabled /// Return true if the motor of the joint is enabled

View File

@ -44,7 +44,7 @@ RigidBody* Joint::getBody2() {
* @return True if the joint is active * @return True if the joint is active
*/ */
boolean Joint::isActive() { boolean Joint::isActive() {
return (this.body1->isActive() hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.body2->isActive()); return (this.body1.isActive() && this.body2.isActive());
} }
// Return the type of the joint // Return the type of the joint

View File

@ -1,18 +1,4 @@
/** @file package org.atriaSoft.ephysics.constraint;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/configuration.hpp>
#include <ephysics/body/RigidBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
/// Enumeration for the type of a raint /// Enumeration for the type of a raint
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT}; enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
@ -69,7 +55,7 @@ namespace ephysics {
} }
/// Destructor /// Destructor
virtual ~JointInfo() = default; ~JointInfo() = default;
}; };
/** /**
@ -77,14 +63,14 @@ namespace ephysics {
*/ */
class Joint { class Joint {
protected : protected :
RigidBody* this.body1; //!< Pointer to the first body of the joint RigidBody* body1; //!< Pointer to the first body of the joint
RigidBody* this.body2; //!< Pointer to the second body of the joint RigidBody* body2; //!< Pointer to the second body of the joint
JointType this.type; //!< Type of the joint JointType type; //!< Type of the joint
int this.indexBody1; //!< Body 1 index in the velocity array to solve the raint int 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 int 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) JointsPositionCorrectionTechnique 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 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 boolean isAlreadyInIsland; //!< True if the joint has already been added into an island
/// Private copy-ructor /// Private copy-ructor
Joint( Joint raint); Joint( Joint raint);
/// Private assignment operator /// Private assignment operator
@ -92,20 +78,20 @@ namespace ephysics {
/// Return true if the joint has already been added into an island /// Return true if the joint has already been added into an island
boolean isAlreadyInIsland() ; boolean isAlreadyInIsland() ;
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual sizet getSizeInBytes() = 0; long getSizeInBytes() = 0;
/// Initialize before solving the joint /// 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) /// 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 /// Solve the velocity raint
virtual void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0; void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
/// Solve the position raint /// Solve the position raint
virtual void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0; void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
public : public :
/// Constructor /// Constructor
Joint( JointInfo jointInfo); Joint( JointInfo jointInfo);
/// Destructor /// Destructor
virtual ~Joint(); ~Joint();
/// Return the reference to the body 1 /// Return the reference to the body 1
RigidBody* getBody1() ; RigidBody* getBody1() ;
/// Return the reference to the body 2 /// Return the reference to the body 2

View File

@ -28,10 +28,10 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo)
assert(this.maxMotorForce >= 0.0); assert(this.maxMotorForce >= 0.0);
// Compute the local-space anchor point for each body // Compute the local-space anchor point for each body
etk::Transform3D transform1 = this.body1->getTransform(); Transform3D transform1 = this.body1.getTransform();
etk::Transform3D transform2 = this.body2->getTransform(); Transform3D transform2 = this.body2.getTransform();
this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace;
this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.this.anchorPointWorldSpace; this.localAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace;
// Compute the inverse of the initial orientation difference between the two bodies // Compute the inverse of the initial orientation difference between the two bodies
this.initOrientationDifferenceInv = transform2.getOrientation() * this.initOrientationDifferenceInv = transform2.getOrientation() *
@ -40,7 +40,7 @@ SliderJoint::SliderJoint( SliderJointInfo jointInfo)
this.initOrientationDifferenceInv.inverse(); this.initOrientationDifferenceInv.inverse();
// Compute the slider axis in local-space of body 1 // 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; jointInfo.sliderAxisWorldSpace;
this.sliderAxisBody1.normalize(); this.sliderAxisBody1.normalize();
} }
@ -54,25 +54,25 @@ SliderJoint::~SliderJoint() {
void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) { void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
// Initialize the bodies index in the veloc ity array // Initialize the bodies index in the veloc ity array
this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1)->second; this.indexBody1 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body1).second;
this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2)->second; this.indexBody2 = raintSolverData.mapBodyToConstrainedVelocityIndex.find(this.body2).second;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = this.body1->this.centerOfMassWorld; Vector3f x1 = this.body1.this.centerOfMassWorld;
vec3 x2 = this.body2->this.centerOfMassWorld; Vector3f x2 = this.body2.this.centerOfMassWorld;
etk::Quaternion orientationBody1 = this.body1->getTransform().getOrientation(); Quaternion orientationBody1 = this.body1.getTransform().getOrientation();
etk::Quaternion orientationBody2 = this.body2->getTransform().getOrientation(); Quaternion orientationBody2 = this.body2.getTransform().getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
this.R1 = orientationBody1 * this.localAnchorPointBody1; this.R1 = orientationBody1 * this.localAnchorPointBody1;
this.R2 = orientationBody2 * this.localAnchorPointBody2; this.R2 = orientationBody2 * this.localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // 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 // Compute the two orthogonal vectors to the slider axis in world-space
this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1; this.sliderAxisWorld = orientationBody1 * this.sliderAxisBody1;
@ -99,18 +99,18 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
this.R2CrossN1 = this.R2.cross(this.N1); this.R2CrossN1 = this.R2.cross(this.N1);
this.R2CrossN2 = this.R2.cross(this.N2); this.R2CrossN2 = this.R2.cross(this.N2);
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
vec3 r1PlusU = this.R1 + u; Vector3f r1PlusU = this.R1 + u;
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1); this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2); this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld); this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// raints (2x2 matrix) // raints (2x2 matrix)
float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse; float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1; Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2; Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
this.R2CrossN1.dot(I2R2CrossN1); this.R2CrossN1.dot(I2R2CrossN1);
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -119,9 +119,9 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
this.R2CrossN2.dot(I2R2CrossN1); this.R2CrossN2.dot(I2R2CrossN1);
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
this.R2CrossN2.dot(I2R2CrossN2); this.R2CrossN2.dot(I2R2CrossN2);
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
this.inverseMassMatrixTranslationConstraint.setZero(); 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(); 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 // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2; 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(); this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
} }
// Compute the bias "b" of the rotation raint // Compute the bias "b" of the rotation raint
this.bRotation.setZero(); this.bRotation.setZero();
if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) { if (this.positionCorrectionTechnique == BAUMGARTEJOINTS) {
etk::Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
this.bRotation = biasFactor * float(2.0) * qError.getVectorV(); this.bRotation = biasFactor * float(2.0) * qError.getVectorV();
} }
// If the limits are enabled // 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) // 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.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis); this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ? this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
@ -177,7 +177,7 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
if (this.isMotorEnabled) { if (this.isMotorEnabled) {
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix) // 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) ? this.inverseMassMatrixMotor = (this.inverseMassMatrixMotor > 0.0) ?
1.0f / this.inverseMassMatrixMotor : 0.0f; 1.0f / this.inverseMassMatrixMotor : 0.0f;
} }
@ -198,25 +198,25 @@ void SliderJoint::initBeforeSolve( ConstraintSolverData raintSolverData) {
void SliderJoint::warmstart( ConstraintSolverData raintSolverData) { void SliderJoint::warmstart( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->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 // Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit; 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 // 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 // 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(); Vector3f linearImpulseBody1 = -this.N1 * this.impulseTranslation.x() - this.N2 * this.impulseTranslation.y();
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() - Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * this.impulseTranslation.x() -
this.R1PlusUCrossN2 * this.impulseTranslation.y(); this.R1PlusUCrossN2 * this.impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 // 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; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 // 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(); Vector3f linearImpulseBody2 = this.N1 * this.impulseTranslation.x() + this.N2 * this.impulseTranslation.y();
vec3 angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() + Vector3f angularImpulseBody2 = this.R2CrossN1 * this.impulseTranslation.x() +
this.R2CrossN2 * this.impulseTranslation.y(); this.R2CrossN2 * this.impulseTranslation.y();
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 // 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) { void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData) {
// Get the velocities // Get the velocities
vec3 v1 = raintSolverData.linearVelocities[this.indexBody1]; Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
vec3 v2 = raintSolverData.linearVelocities[this.indexBody2]; Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
vec3 w1 = raintSolverData.angularVelocities[this.indexBody1]; Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
vec3 w2 = raintSolverData.angularVelocities[this.indexBody2]; Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// --------------- Translation Constraints --------------- // // --------------- Translation Constraints --------------- //
@ -280,8 +280,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
this.impulseTranslation += deltaLambda; this.impulseTranslation += deltaLambda;
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 // 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(); Vector3f linearImpulseBody1 = -this.N1 * deltaLambda.x() - this.N2 * deltaLambda.y();
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() - Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * deltaLambda.x() -
this.R1PlusUCrossN2 * deltaLambda.y(); this.R1PlusUCrossN2 * deltaLambda.y();
// Apply the impulse to the body 1 // Apply the impulse to the body 1
@ -289,8 +289,8 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 // 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(); Vector3f linearImpulseBody2 = this.N1 * deltaLambda.x() + this.N2 * deltaLambda.y();
vec3 angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y(); Vector3f angularImpulseBody2 = this.R2CrossN1 * deltaLambda.x() + this.R2CrossN2 * deltaLambda.y();
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -299,10 +299,10 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
// --------------- Rotation Constraints --------------- // // --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation raints // 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 // 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; this.impulseRotation += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1 // 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 // Compute the Lagrange multiplier lambda for the lower limit raint
float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit); float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit -this.bLowerLimit);
float lambdaTemp = this.impulseLowerLimit; 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; deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1 // Compute the impulse P=J^T * lambda for the lower limit raint of body 1
vec3 linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld; Vector3f linearImpulseBody1 = -deltaLambdaLower * this.sliderAxisWorld;
vec3 angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis; Vector3f angularImpulseBody1 = -deltaLambdaLower * this.R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2 // Compute the impulse P=J^T * lambda for the lower limit raint of body 2
vec3 linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld; Vector3f linearImpulseBody2 = deltaLambdaLower * this.sliderAxisWorld;
vec3 angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis; Vector3f angularImpulseBody2 = deltaLambdaLower * this.R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -361,20 +361,20 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
// Compute the Lagrange multiplier lambda for the upper limit raint // Compute the Lagrange multiplier lambda for the upper limit raint
float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit); float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit -this.bUpperLimit);
float lambdaTemp = this.impulseUpperLimit; 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; deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1 // Compute the impulse P=J^T * lambda for the upper limit raint of body 1
vec3 linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld; Vector3f linearImpulseBody1 = deltaLambdaUpper * this.sliderAxisWorld;
vec3 angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis; Vector3f angularImpulseBody1 = deltaLambdaUpper * this.R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
w1 += this.i1 * angularImpulseBody1; w1 += this.i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2 // Compute the impulse P=J^T * lambda for the upper limit raint of body 2
vec3 linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld; Vector3f linearImpulseBody2 = -deltaLambdaUpper * this.sliderAxisWorld;
vec3 angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis; Vector3f angularImpulseBody2 = -deltaLambdaUpper * this.R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -397,13 +397,13 @@ void SliderJoint::solveVelocityConstraint( ConstraintSolverData raintSolverData)
deltaLambdaMotor = this.impulseMotor - lambdaTemp; deltaLambdaMotor = this.impulseMotor - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1 // 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 // Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1; v1 += inverseMassBody1 * linearImpulseBody1;
// Compute the impulse P=J^T * lambda for the motor of body 2 // 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 // Apply the impulse to the body 2
v2 += inverseMassBody2 * linearImpulseBody2; v2 += inverseMassBody2 * linearImpulseBody2;
@ -418,25 +418,25 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return; if (this.positionCorrectionTechnique != NONLINEARGAUSSSEIDEL) return;
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = raintSolverData.positions[this.indexBody1]; Vector3f x1 = raintSolverData.positions[this.indexBody1];
vec3 x2 = raintSolverData.positions[this.indexBody2]; Vector3f x2 = raintSolverData.positions[this.indexBody2];
etk::Quaternion q1 = raintSolverData.orientations[this.indexBody1]; Quaternion q1 = raintSolverData.orientations[this.indexBody1];
etk::Quaternion q2 = raintSolverData.orientations[this.indexBody2]; Quaternion q2 = raintSolverData.orientations[this.indexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies // Get the inverse mass and inverse inertia tensors of the bodies
float inverseMassBody1 = this.body1->this.massInverse; float inverseMassBody1 = this.body1.this.massInverse;
float inverseMassBody2 = this.body2->this.massInverse; float inverseMassBody2 = this.body2.this.massInverse;
// Recompute the inertia tensor of bodies // Recompute the inertia tensor of bodies
this.i1 = this.body1->getInertiaTensorInverseWorld(); this.i1 = this.body1.getInertiaTensorInverseWorld();
this.i2 = this.body2->getInertiaTensorInverseWorld(); this.i2 = this.body2.getInertiaTensorInverseWorld();
// Vector from body center to the anchor point // Vector from body center to the anchor point
this.R1 = q1 * this.localAnchorPointBody1; this.R1 = q1 * this.localAnchorPointBody1;
this.R2 = q2 * this.localAnchorPointBody2; this.R2 = q2 * this.localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // 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 // Compute the two orthogonal vectors to the slider axis in world-space
this.sliderAxisWorld = q1 * this.sliderAxisBody1; this.sliderAxisWorld = q1 * this.sliderAxisBody1;
@ -455,7 +455,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
this.R2CrossN1 = this.R2.cross(this.N1); this.R2CrossN1 = this.R2.cross(this.N1);
this.R2CrossN2 = this.R2.cross(this.N2); this.R2CrossN2 = this.R2.cross(this.N2);
this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld); this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
vec3 r1PlusU = this.R1 + u; Vector3f r1PlusU = this.R1 + u;
this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1); this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2); this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld); 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 // Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
// raints (2x2 matrix) // raints (2x2 matrix)
float sumInverseMass = this.body1->this.massInverse + this.body2->this.massInverse; float sumInverseMass = this.body1.this.massInverse + this.body2.this.massInverse;
vec3 I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1; Vector3f I1R1PlusUCrossN1 = this.i1 * this.R1PlusUCrossN1;
vec3 I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2; Vector3f I1R1PlusUCrossN2 = this.i1 * this.R1PlusUCrossN2;
vec3 I2R2CrossN1 = this.i2 * this.R2CrossN1; Vector3f I2R2CrossN1 = this.i2 * this.R2CrossN1;
vec3 I2R2CrossN2 = this.i2 * this.R2CrossN2; Vector3f I2R2CrossN2 = this.i2 * this.R2CrossN2;
float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
this.R2CrossN1.dot(I2R2CrossN1); this.R2CrossN1.dot(I2R2CrossN1);
float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
@ -477,9 +477,9 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
this.R2CrossN2.dot(I2R2CrossN1); this.R2CrossN2.dot(I2R2CrossN1);
float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
this.R2CrossN2.dot(I2R2CrossN2); this.R2CrossN2.dot(I2R2CrossN2);
etk::Matrix2x2 matrixKTranslation(el11, el12, el21, el22); Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
this.inverseMassMatrixTranslationConstraint.setZero(); 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(); this.inverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse();
} }
@ -490,31 +490,31 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError); vec2 lambdaTranslation = this.inverseMassMatrixTranslationConstraint * (-translationError);
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1 // 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(); Vector3f linearImpulseBody1 = -this.N1 * lambdaTranslation.x() - this.N2 * lambdaTranslation.y();
vec3 angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() - Vector3f angularImpulseBody1 = -this.R1PlusUCrossN1 * lambdaTranslation.x() -
this.R1PlusUCrossN2 * lambdaTranslation.y(); this.R1PlusUCrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 1 // Apply the impulse to the body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2 // 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(); Vector3f linearImpulseBody2 = this.N1 * lambdaTranslation.x() + this.N2 * lambdaTranslation.y();
vec3 angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() + Vector3f angularImpulseBody2 = this.R2CrossN1 * lambdaTranslation.x() +
this.R2CrossN2 * lambdaTranslation.y(); this.R2CrossN2 * lambdaTranslation.y();
// Apply the impulse to the body 2 // Apply the impulse to the body 2
vec3 v2 = inverseMassBody2 * linearImpulseBody2; Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
// --------------- Rotation Constraints --------------- // // --------------- 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 // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix) // contraints (3x3 matrix)
this.inverseMassMatrixRotationConstraint = this.i1 + this.i2; 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(); this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.getInverse();
} }
// Compute the position error for the 3 rotation raints // Compute the position error for the 3 rotation raints
etk::Quaternion currentOrientationDifference = q2 * q1.getInverse(); Quaternion currentOrientationDifference = q2 * q1.getInverse();
currentOrientationDifference.normalize(); currentOrientationDifference.normalize();
etk::Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv; Quaternion qError = currentOrientationDifference * this.initOrientationDifferenceInv;
vec3 errorRotation = float(2.0) * qError.getVectorV(); Vector3f errorRotation = float(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation raints // 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 // Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
angularImpulseBody1 = -lambdaRotation; angularImpulseBody1 = -lambdaRotation;
@ -542,7 +542,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
w1 = this.i1 * angularImpulseBody1; w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // 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(); q1.normalize();
// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2 // 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; w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // 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(); q2.normalize();
// --------------- Limits Constraints --------------- // // --------------- Limits Constraints --------------- //
@ -562,7 +562,7 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
if (this.isLowerLimitViolated || this.isUpperLimitViolated) { if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix) // 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.R1PlusUCrossSliderAxis.dot(this.i1 * this.R1PlusUCrossSliderAxis) +
this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis); this.R2CrossSliderAxis.dot(this.i2 * this.R2CrossSliderAxis);
this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ? this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0) ?
@ -576,29 +576,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError); float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
// Compute the impulse P=J^T * lambda for the lower limit raint of body 1 // Compute the impulse P=J^T * lambda for the lower limit raint of body 1
vec3 linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld; Vector3f linearImpulseBody1 = -lambdaLowerLimit * this.sliderAxisWorld;
vec3 angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis; Vector3f angularImpulseBody1 = -lambdaLowerLimit * this.R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the lower limit raint of body 2 // Compute the impulse P=J^T * lambda for the lower limit raint of body 2
vec3 linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld; Vector3f linearImpulseBody2 = lambdaLowerLimit * this.sliderAxisWorld;
vec3 angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis; Vector3f angularImpulseBody2 = lambdaLowerLimit * this.R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
vec3 v2 = inverseMassBody2 * linearImpulseBody2; Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
} }
@ -609,29 +609,29 @@ void SliderJoint::solvePositionConstraint( ConstraintSolverData raintSolverData)
float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError); float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
// Compute the impulse P=J^T * lambda for the upper limit raint of body 1 // Compute the impulse P=J^T * lambda for the upper limit raint of body 1
vec3 linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld; Vector3f linearImpulseBody1 = lambdaUpperLimit * this.sliderAxisWorld;
vec3 angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis; Vector3f angularImpulseBody1 = lambdaUpperLimit * this.R1PlusUCrossSliderAxis;
// Apply the impulse to the body 1 // Apply the impulse to the body 1
vec3 v1 = inverseMassBody1 * linearImpulseBody1; Vector3f v1 = inverseMassBody1 * linearImpulseBody1;
vec3 w1 = this.i1 * angularImpulseBody1; Vector3f w1 = this.i1 * angularImpulseBody1;
// Update the body position/orientation of body 1 // Update the body position/orientation of body 1
x1 += v1; x1 += v1;
q1 += etk::Quaternion(0, w1) * q1 * 0.5f; q1 += Quaternion(0, w1) * q1 * 0.5f;
q1.normalize(); q1.normalize();
// Compute the impulse P=J^T * lambda for the upper limit raint of body 2 // Compute the impulse P=J^T * lambda for the upper limit raint of body 2
vec3 linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld; Vector3f linearImpulseBody2 = -lambdaUpperLimit * this.sliderAxisWorld;
vec3 angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis; Vector3f angularImpulseBody2 = -lambdaUpperLimit * this.R2CrossSliderAxis;
// Apply the impulse to the body 2 // Apply the impulse to the body 2
vec3 v2 = inverseMassBody2 * linearImpulseBody2; Vector3f v2 = inverseMassBody2 * linearImpulseBody2;
vec3 w2 = this.i2 * angularImpulseBody2; Vector3f w2 = this.i2 * angularImpulseBody2;
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
x2 += v2; x2 += v2;
q2 += etk::Quaternion(0, w2) * q2 * 0.5f; q2 += Quaternion(0, w2) * q2 * 0.5f;
q2.normalize(); q2.normalize();
} }
} }
@ -664,8 +664,8 @@ void SliderJoint::enableMotor(boolean isMotorEnabled) {
this.impulseMotor = 0.0; this.impulseMotor = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
// Return the current translation value of the joint // 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 // TODO : Check if we need to compare rigid body position or center of mass here
// Get the bodies positions and orientations // Get the bodies positions and orientations
vec3 x1 = this.body1->getTransform().getPosition(); Vector3f x1 = this.body1.getTransform().getPosition();
vec3 x2 = this.body2->getTransform().getPosition(); Vector3f x2 = this.body2.getTransform().getPosition();
etk::Quaternion q1 = this.body1->getTransform().getOrientation(); Quaternion q1 = this.body1.getTransform().getOrientation();
etk::Quaternion q2 = this.body2->getTransform().getOrientation(); Quaternion q2 = this.body2.getTransform().getOrientation();
// Compute the two anchor points in world-space coordinates // Compute the two anchor points in world-space coordinates
vec3 anchorBody1 = x1 + q1 * this.localAnchorPointBody1; Vector3f anchorBody1 = x1 + q1 * this.localAnchorPointBody1;
vec3 anchorBody2 = x2 + q2 * this.localAnchorPointBody2; Vector3f anchorBody2 = x2 + q2 * this.localAnchorPointBody2;
// Compute the vector u (difference between anchor points) // Compute the vector u (difference between anchor points)
vec3 u = anchorBody2 - anchorBody1; Vector3f u = anchorBody2 - anchorBody1;
// Compute the slider axis in world-space // Compute the slider axis in world-space
vec3 sliderAxisWorld = q1 * this.sliderAxisBody1; Vector3f sliderAxisWorld = q1 * this.sliderAxisBody1;
sliderAxisWorld.normalize(); sliderAxisWorld.normalize();
// Compute and return the translation value // Compute and return the translation value
@ -739,8 +739,8 @@ void SliderJoint::resetLimits() {
this.impulseUpperLimit = 0.0; this.impulseUpperLimit = 0.0;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
// Set the motor speed // Set the motor speed
@ -754,8 +754,8 @@ void SliderJoint::setMotorSpeed(float motorSpeed) {
this.motorSpeed = motorSpeed; this.motorSpeed = motorSpeed;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
} }
@ -771,8 +771,8 @@ void SliderJoint::setMaxMotorForce(float maxMotorForce) {
this.maxMotorForce = maxMotorForce; this.maxMotorForce = maxMotorForce;
// Wake up the two bodies of the joint // Wake up the two bodies of the joint
this.body1->setIsSleeping(false); this.body1.setIsSleeping(false);
this.body2->setIsSleeping(false); this.body2.setIsSleeping(false);
} }
} }
@ -834,7 +834,7 @@ float SliderJoint::getMotorForce(float timeStep) {
} }
// Return the number of bytes used by the joint // Return the number of bytes used by the joint
sizet SliderJoint::getSizeInBytes() { long SliderJoint::getSizeInBytes() {
return sizeof(SliderJoint); return sizeof(SliderJoint);
} }

View File

@ -1,26 +1,14 @@
/** @file package org.atriaSoft.ephysics.constraint;
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> 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 <ephysics/mathematics/mathematics.hpp>
#include <ephysics/engine/ConstraintSolver.hpp>
namespace ephysics {
/** /**
* This structure is used to gather the information needed to create a slider * 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. * joint. This structure will be used to create the actual slider joint.
*/ */
struct SliderJointInfo : public JointInfo { struct SliderJointInfo extends JointInfo {
public : public :
vec3 this.anchorPointWorldSpace; //!< Anchor point (in world-space coordinates) Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
vec3 sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates) Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
boolean isLimitEnabled; //!< True if the slider limits are enabled boolean isLimitEnabled; //!< True if the slider limits are enabled
boolean isMotorEnabled; //!< True if the slider motor is enabled boolean isMotorEnabled; //!< True if the slider motor is enabled
float minTranslationLimit; //!< Mininum allowed translation if limits are enabled float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
@ -36,8 +24,8 @@ namespace ephysics {
*/ */
SliderJointInfo(RigidBody* rigidBody1, SliderJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initSliderAxisWorldSpace): Vector3f initSliderAxisWorldSpace):
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
this.anchorPointWorldSpace(initAnchorPointWorldSpace), this.anchorPointWorldSpace(initAnchorPointWorldSpace),
sliderAxisWorldSpace(initSliderAxisWorldSpace), sliderAxisWorldSpace(initSliderAxisWorldSpace),
@ -60,8 +48,8 @@ namespace ephysics {
*/ */
SliderJointInfo(RigidBody* rigidBody1, SliderJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initSliderAxisWorldSpace, Vector3f initSliderAxisWorldSpace,
float initMinTranslationLimit, float initMinTranslationLimit,
float initMaxTranslationLimit): float initMaxTranslationLimit):
JointInfo(rigidBody1, rigidBody2, SLIDERJOINT), JointInfo(rigidBody1, rigidBody2, SLIDERJOINT),
@ -88,8 +76,8 @@ namespace ephysics {
*/ */
SliderJointInfo(RigidBody* rigidBody1, SliderJointInfo(RigidBody* rigidBody1,
RigidBody* rigidBody2, RigidBody* rigidBody2,
vec3 initAnchorPointWorldSpace, Vector3f initAnchorPointWorldSpace,
vec3 initSliderAxisWorldSpace, Vector3f initSliderAxisWorldSpace,
float initMinTranslationLimit, float initMinTranslationLimit,
float initMaxTranslationLimit, float initMaxTranslationLimit,
float initMotorSpeed, float initMotorSpeed,
@ -115,38 +103,38 @@ namespace ephysics {
class SliderJoint: public Joint { class SliderJoint: public Joint {
private: private:
static float BETA; //!< Beta value for the position correction bias factor 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) Vector3f 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) Vector3f 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) Vector3f this.sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
etk::Matrix3x3 this.i1; //!< Inertia tensor of body 1 (in world-space coordinates) Matrix3f this.i1; //!< Inertia tensor of body 1 (in world-space coordinates)
etk::Matrix3x3 this.i2; //!< Inertia tensor of body 2 (in world-space coordinates) Matrix3f 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 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 Vector3f 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 Vector3f 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 Vector3f this.R1; //!< Vector r1 in world-space coordinates
vec3 this.R2; //!< Vector r2 in world-space coordinates Vector3f this.R2; //!< Vector r2 in world-space coordinates
vec3 this.R2CrossN1; //!< Cross product of r2 and n1 Vector3f this.R2CrossN1; //!< Cross product of r2 and n1
vec3 this.R2CrossN2; //!< Cross product of r2 and n2 Vector3f this.R2CrossN2; //!< Cross product of r2 and n2
vec3 this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis Vector3f this.R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
vec3 this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1 Vector3f this.R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
vec3 this.R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2 Vector3f 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.R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
vec2 this.bTranslation; //!< Bias of the 2 translation raints 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.bLowerLimit; //!< Bias of the lower limit raint
float this.bUpperLimit; //!< Bias of the upper 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) 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) 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.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 float this.inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
vec2 this.impulseTranslation; //!< Accumulated impulse for the 2 translation raints 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.impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint float this.impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
float this.impulseMotor; //!< Accumulated impulse for the motor float this.impulseMotor; //!< Accumulated impulse for the motor
boolean this.isLimitEnabled; //!< True if the slider limits are enabled boolean this.isLimitEnabled; //!< True if the slider limits are enabled
boolean this.isMotorEnabled; //!< True if the motor of the joint in 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.lowerLimit; //!< Lower limit (minimum translation distance)
float this.upperLimit; //!< Upper limit (maximum translation distance) float this.upperLimit; //!< Upper limit (maximum translation distance)
boolean this.isLowerLimitViolated; //!< True if the lower limit is violated boolean this.isLowerLimitViolated; //!< True if the lower limit is violated
@ -159,16 +147,16 @@ namespace ephysics {
SliderJoint operator=( SliderJoint raint); SliderJoint operator=( SliderJoint raint);
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
sizet getSizeInBytes() override; long getSizeInBytes() ;
void initBeforeSolve( ConstraintSolverData raintSolverData) override; void initBeforeSolve( ConstraintSolverData raintSolverData) ;
void warmstart( ConstraintSolverData raintSolverData) override; void warmstart( ConstraintSolverData raintSolverData) ;
void solveVelocityConstraint( ConstraintSolverData raintSolverData) override; void solveVelocityConstraint( ConstraintSolverData raintSolverData) ;
void solvePositionConstraint( ConstraintSolverData raintSolverData) override; void solvePositionConstraint( ConstraintSolverData raintSolverData) ;
public : public :
/// Constructor /// Constructor
SliderJoint( SliderJointInfo jointInfo); SliderJoint( SliderJointInfo jointInfo);
/// Destructor /// Destructor
virtual ~SliderJoint(); ~SliderJoint();
/// Return true if the limits or the joint are enabled /// Return true if the limits or the joint are enabled
boolean isLimitEnabled() ; boolean isLimitEnabled() ;
/// Return true if the motor of the joint is enabled /// Return true if the motor of the joint is enabled

View File

@ -1,12 +0,0 @@
/** @file
* @author Edouard DUPIN
* @copyright 2011, Edouard DUPIN, all right reserved
* @license MPL v2.0 (see license file)
*/
#include <ephysics/debug.hpp>
int ephysic::getLogId() {
static int gval = elog::registerInstance("ephysic");
return gval;
}

View File

@ -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 <elog/log.hpp>
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)

View File

@ -26,7 +26,7 @@ CollisionWorld::~CollisionWorld() {
} }
CollisionBody* CollisionWorld::createCollisionBody( etk::Transform3D transform) { CollisionBody* CollisionWorld::createCollisionBody( Transform3D transform) {
// Get the next available body ID // Get the next available body ID
long bodyID = computeNextAvailableBodyID(); long bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index) // 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) { void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Remove all the collision shapes of the body // Remove all the collision shapes of the body
collisionBody->removeAllCollisionShapes(); collisionBody.removeAllCollisionShapes();
// Add the body ID to the list of free IDs // 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 // Remove the collision body from the list of bodies
this.bodies.erase(this.bodies.find(collisionBody)); this.bodies.erase(this.bodies.find(collisionBody));
ETKDELETE(CollisionBody, collisionBody); ETKDELETE(CollisionBody, collisionBody);
@ -66,21 +66,21 @@ long CollisionWorld::computeNextAvailableBodyID() {
void CollisionWorld::resetContactManifoldListsOfBodies() { void CollisionWorld::resetContactManifoldListsOfBodies() {
// For each rigid body of the world // For each rigid body of the world
for (etk::Set<CollisionBody*>::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) { for (Set<CollisionBody*>::Iterator it = this.bodies.begin(); it != this.bodies.end(); ++it) {
// Reset the contact manifold list of the body // Reset the contact manifold list of the body
(*it)->resetContactManifoldsList(); (*it).resetContactManifoldsList();
} }
} }
boolean CollisionWorld::testAABBOverlap( CollisionBody* body1, CollisionBody* body2) { boolean CollisionWorld::testAABBOverlap( CollisionBody* body1, CollisionBody* body2) {
// If one of the body is not active, we return no overlap // If one of the body is not active, we return no overlap
if ( !body1->isActive() if ( !body1.isActive()
|| !body2->isActive()) { || !body2.isActive()) {
return false; return false;
} }
// Compute the AABBs of both bodies // Compute the AABBs of both bodies
AABB body1AABB = body1->getAABB(); AABB body1AABB = body1.getAABB();
AABB body2AABB = body2->getAABB(); AABB body2AABB = body2.getAABB();
// Return true if the two AABBs overlap // Return true if the two AABBs overlap
return body1AABB.testCollision(body2AABB); return body1AABB.testCollision(body2AABB);
} }
@ -89,9 +89,9 @@ void CollisionWorld::testCollision( ProxyShape* shape, CollisionCallback* callba
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
etk::Set<int> shapes; Set<int> shapes;
shapes.add(shape->this.broadPhaseID); shapes.add(shape.this.broadPhaseID);
etk::Set<int> emptySet; Set<int> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet); 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 // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
etk::Set<int> shapes1; Set<int> shapes1;
shapes1.add(shape1->this.broadPhaseID); shapes1.add(shape1.this.broadPhaseID);
etk::Set<int> shapes2; Set<int> shapes2;
shapes2.add(shape2->this.broadPhaseID); shapes2.add(shape2.this.broadPhaseID);
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); 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 // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
etk::Set<int> shapes1; Set<int> shapes1;
// For each shape of the body // For each shape of the body
for ( ProxyShape* shape = body->getProxyShapesList(); for ( ProxyShape* shape = body.getProxyShapesList();
shape != null; shape != null;
shape = shape->getNext()) { shape = shape.getNext()) {
shapes1.add(shape->this.broadPhaseID); shapes1.add(shape.this.broadPhaseID);
} }
etk::Set<int> emptySet; Set<int> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet); 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 // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
// Create the sets of shapes // Create the sets of shapes
etk::Set<int> shapes1; Set<int> shapes1;
for ( ProxyShape* shape = body1->getProxyShapesList(); for ( ProxyShape* shape = body1.getProxyShapesList();
shape != null; shape != null;
shape = shape->getNext()) { shape = shape.getNext()) {
shapes1.add(shape->this.broadPhaseID); shapes1.add(shape.this.broadPhaseID);
} }
etk::Set<int> shapes2; Set<int> shapes2;
for ( ProxyShape* shape = body2->getProxyShapesList(); for ( ProxyShape* shape = body2.getProxyShapesList();
shape != null; shape != null;
shape = shape->getNext()) { shape = shape.getNext()) {
shapes2.add(shape->this.broadPhaseID); shapes2.add(shape.this.broadPhaseID);
} }
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2); this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
@ -147,7 +147,7 @@ void CollisionWorld::testCollision( CollisionBody* body1, CollisionBody* body2,
void CollisionWorld::testCollision(CollisionCallback* callback) { void CollisionWorld::testCollision(CollisionCallback* callback) {
// Reset all the contact manifolds lists of each body // Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies(); resetContactManifoldListsOfBodies();
etk::Set<int> emptySet; Set<int> emptySet;
// Perform the collision detection and report contacts // Perform the collision detection and report contacts
this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet); this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
} }

Some files were not shown because too many files have changed in this diff Show More