HeeroYui/jege_proto
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

[DEV] port some element of ephysiscs

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/org/atriaSoft/ephysics/ContactsPositionCorrectionTechnique.java Normal file
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,
}

9
src/org/atriaSoft/ephysics/JointsPositionCorrectionTechnique.java Normal file
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
}

29
src/org/atriaSoft/ephysics/Log.java Normal file
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);
}
}

70
src/org/atriaSoft/ephysics/Property.java Normal file
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;
}

8
src/org/atriaSoft/ephysics/body/Body.java
View File

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

7
src/org/atriaSoft/ephysics/body/BodyType.java Normal file
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.
}

238
src/org/atriaSoft/ephysics/body/CollisionBody.cpp
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;
}

209
src/org/atriaSoft/ephysics/body/CollisionBody.hpp
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;
};
}

345
src/org/atriaSoft/ephysics/body/CollisionBody.java Normal file
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;
}
}

316
src/org/atriaSoft/ephysics/body/RigidBody.cpp
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;
}

294
src/org/atriaSoft/ephysics/body/RigidBody.hpp
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;
};
}

514
src/org/atriaSoft/ephysics/body/RigidBody.java Normal file
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);
}
}

256
src/org/atriaSoft/ephysics/collision/CollisionDetection.cpp
View File

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

77
src/org/atriaSoft/ephysics/collision/CollisionDetection.hpp → src/org/atriaSoft/ephysics/collision/CollisionDetection.java
View File

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

5
src/org/atriaSoft/ephysics/collision/CollisionShapeInfo.hpp
View File

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

57
src/org/atriaSoft/ephysics/collision/ContactManifold.hpp
View File

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

86
src/org/atriaSoft/ephysics/collision/ContactManifold.cpp → src/org/atriaSoft/ephysics/collision/ContactManifold.java
View File

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

50
src/org/atriaSoft/ephysics/collision/ContactManifoldSet.cpp
View File

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

29
src/org/atriaSoft/ephysics/collision/ContactManifoldSet.hpp → src/org/atriaSoft/ephysics/collision/ContactManifoldSet.java
View File

@ -1,16 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision;
#include <ephysics/collision/ContactManifold.hpp>
namespace ephysics {
int MAXMANIFOLDSINCONTACTMANIFOLDSET = 3; // Maximum number of contact manifolds in the set
int CONTACTCUBEMAPFACENBSUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
/**
@ -21,13 +10,13 @@ namespace ephysics {
*/
class ContactManifoldSet {
private:
int this.nbMaxManifolds; //!< Maximum number of contact manifolds in the set
int this.nbManifolds; //!< Current number of contact manifolds in the set
ProxyShape* this.shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* this.shape2; //!< Pointer to the second proxy shape of the contact
ContactManifold* this.manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
int nbMaxManifolds; //!< Maximum number of contact manifolds in the set
int nbManifolds; //!< Current number of contact manifolds in the set
ProxyShape* shape1; //!< Pointer to the first proxy shape of the contact
ProxyShape* shape2; //!< Pointer to the second proxy shape of the contact
ContactManifold* manifolds[MAXMANIFOLDSINCONTACTMANIFOLDSET]; //!< Contact manifolds of the set
/// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId);
void createManifold(int normalDirectionId);
/// Remove a contact manifold from the set
void removeManifold(int index);
// Return the index of the contact manifold with a similar average normal.
@ -35,14 +24,12 @@ namespace ephysics {
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
int16t computeCubemapNormalId( vec3 normal) ;
int16t computeCubemapNormalId( Vector3f normal) ;
public:
/// Constructor
ContactManifoldSet(ProxyShape* shape1,
ProxyShape* shape2,
int nbMaxManifolds);
/// Destructor
~ContactManifoldSet();
/// Return the first proxy shape
ProxyShape* getShape1() ;
/// Return the second proxy shape

52
src/org/atriaSoft/ephysics/collision/ProxyShape.cpp
View File

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

66
src/org/atriaSoft/ephysics/collision/ProxyShape.hpp → src/org/atriaSoft/ephysics/collision/ProxyShape.java
View File

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

4
src/org/atriaSoft/ephysics/collision/RaycastInfo.cpp
View File

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

28
src/org/atriaSoft/ephysics/collision/RaycastInfo.hpp → src/org/atriaSoft/ephysics/collision/RaycastInfo.java
View File

@ -1,20 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision;
#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.
*/
@ -25,8 +10,8 @@ namespace ephysics {
/// Private assignment operator
RaycastInfo operator=( RaycastInfo) = delete;
public:
vec3 worldPoint; //!< Hit point in world-space coordinates
vec3 worldNormal; //!< Surface normal at hit point in world-space coordinates
Vector3f worldPoint; //!< Hit point in world-space coordinates
Vector3f worldNormal; //!< Surface normal at hit point in world-space coordinates
float hitFraction; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
int meshSubpart; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
int triangleIndex; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)
@ -40,9 +25,6 @@ namespace ephysics {
proxyShape(null) {
}
/// Destructor
virtual ~RaycastInfo() = default;
};
/**
@ -53,8 +35,6 @@ namespace ephysics {
*/
class RaycastCallback {
public:
/// Destructor
virtual ~RaycastCallback() = default;
/**
* @brief This method will be called for each ProxyShape that is hit by the
* ray. You cannot make any assumptions about the order of the
@ -70,7 +50,7 @@ namespace ephysics {
* @param[in] raycastInfo Information about the raycast hit
* @return Value that controls the continuation of the ray after a hit
*/
virtual float notifyRaycastHit( RaycastInfo raycastInfo)=0;
float notifyRaycastHit( RaycastInfo raycastInfo)=0;
};
struct RaycastTest {

19
src/org/atriaSoft/ephysics/collision/TriangleMesh.hpp → src/org/atriaSoft/ephysics/collision/TriangleMesh.java
View File

@ -1,16 +1,5 @@
/** @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 <etk/Vector.hpp>
#include <ephysics/collision/TriangleVertexArray.hpp>
package org.atriaSoft.ephysics.collision;
namespace ephysics {
/**
* @brief Represents a mesh made of triangles. A TriangleMesh contains
* one or several parts. Each part is a set of triangles represented in a
@ -20,16 +9,12 @@ namespace ephysics {
*/
class TriangleMesh {
protected:
etk::Vector<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:
/**
* @brief Constructor
*/
TriangleMesh();
/**
* @brief Virtualisation of Destructor
*/
virtual ~TriangleMesh() = default;
/**
* @brief Add a subpart of the mesh
*/

10
src/org/atriaSoft/ephysics/collision/TriangleVertexArray.cpp
View File

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

34
src/org/atriaSoft/ephysics/collision/TriangleVertexArray.hpp → src/org/atriaSoft/ephysics/collision/TriangleVertexArray.java
View File

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

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

34
src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.hpp → src/org/atriaSoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java
View File

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

252
src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.cpp
View File

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

24
src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.hpp → src/org/atriaSoft/ephysics/collision/broadphase/DynamicAABBTree.java
View File

@ -1,19 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.broadphase;
#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>
/**
* @brief It represents a node of the dynamic AABB tree.
@ -85,7 +71,7 @@ namespace ephysics {
/// Constructor
DynamicAABBTree(float extraAABBGap = 0.0f);
/// Destructor
virtual ~DynamicAABBTree();
~DynamicAABBTree();
/// Add an object into the tree (where node data are two integers)
int addObject( AABB aabb, int data1, int data2);
/// Add an object into the tree (where node data is a pointer)
@ -93,7 +79,7 @@ namespace ephysics {
/// Remove an object from the tree
void removeObject(int nodeID);
/// Update the dynamic tree after an object has moved.
boolean updateObject(int nodeID, AABB newAABB, vec3 displacement, bool forceReinsert = false);
boolean updateObject(int nodeID, AABB newAABB, Vector3f displacement, bool forceReinsert = false);
/// Return the fat AABB corresponding to a given node ID
AABB getFatAABB(int nodeID) ;
/// Return the pointer to the data array of a given leaf node of the tree
@ -101,9 +87,9 @@ namespace ephysics {
/// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) ;
/// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWithAABB( AABB aabb, etk::Function<void(int nodeId)> callback) ;
void reportAllShapesOverlappingWithAABB( AABB aabb, Function<void(int nodeId)> callback) ;
/// 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
int computeHeight();
/// Return the root AABB of the tree

37
src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.hpp
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;
};
}

21
src/org/atriaSoft/ephysics/collision/narrowphase/CollisionDispatch.java Normal file
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;
};
}

114
src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

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

51
src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java
View File

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

8
src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

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

37
src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.hpp
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;
};
}

23
src/org/atriaSoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java Normal file
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) ;
};
}

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

40
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java
View File

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

28
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.cpp
View File

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

28
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java
View File

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

26
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.cpp
View File

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

89
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java
View File

@ -1,16 +1,5 @@
/** @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/mathematics/mathematics.hpp>
#include <ephysics/configuration.hpp>
#include <ephysics/collision/narrowphase/EPA/EdgeEPA.hpp>
namespace ephysics {
package org.atriaSoft.ephysics.collision.narrowphase.EPA;
boolean link( EdgeEPA edge0, EdgeEPA edge1);
void halfLink( EdgeEPA edge0, EdgeEPA edge1);
/**
@ -23,40 +12,40 @@ namespace ephysics {
EdgeEPA this.adjacentEdges[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
boolean this.isObsolete; //!< True if the triangle face is visible from the new support point
float this.determinant; //!< Determinant
vec3 this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
Vector3f this.closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
float this.lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
float this.lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
float this.distSquare; //!< Square distance of the point closest point v to the origin
public:
/// Private copy-ructor
TriangleEPA( TriangleEPA triangle) {
this.indicesVertices[0] = triangle.this.indicesVertices[0];
this.indicesVertices[1] = triangle.this.indicesVertices[1];
this.indicesVertices[2] = triangle.this.indicesVertices[2];
this.adjacentEdges[0] = triangle.this.adjacentEdges[0];
this.adjacentEdges[1] = triangle.this.adjacentEdges[1];
this.adjacentEdges[2] = triangle.this.adjacentEdges[2];
this.isObsolete = triangle.this.isObsolete;
this.determinant = triangle.this.determinant;
this.closestPoint = triangle.this.closestPoint;
this.lambda1 = triangle.this.lambda1;
this.lambda2 = triangle.this.lambda2;
this.distSquare = triangle.this.distSquare;
this.indicesVertices[0] = triangle.indicesVertices[0];
this.indicesVertices[1] = triangle.indicesVertices[1];
this.indicesVertices[2] = triangle.indicesVertices[2];
this.adjacentEdges[0] = triangle.adjacentEdges[0];
this.adjacentEdges[1] = triangle.adjacentEdges[1];
this.adjacentEdges[2] = triangle.adjacentEdges[2];
this.isObsolete = triangle.isObsolete;
this.determinant = triangle.determinant;
this.closestPoint = triangle.closestPoint;
this.lambda1 = triangle.lambda1;
this.lambda2 = triangle.lambda2;
this.distSquare = triangle.distSquare;
}
/// Private assignment operator
TriangleEPA operator=( TriangleEPA triangle) {
this.indicesVertices[0] = triangle.this.indicesVertices[0];
this.indicesVertices[1] = triangle.this.indicesVertices[1];
this.indicesVertices[2] = triangle.this.indicesVertices[2];
this.adjacentEdges[0] = triangle.this.adjacentEdges[0];
this.adjacentEdges[1] = triangle.this.adjacentEdges[1];
this.adjacentEdges[2] = triangle.this.adjacentEdges[2];
this.isObsolete = triangle.this.isObsolete;
this.determinant = triangle.this.determinant;
this.closestPoint = triangle.this.closestPoint;
this.lambda1 = triangle.this.lambda1;
this.lambda2 = triangle.this.lambda2;
this.distSquare = triangle.this.distSquare;
this.indicesVertices[0] = triangle.indicesVertices[0];
this.indicesVertices[1] = triangle.indicesVertices[1];
this.indicesVertices[2] = triangle.indicesVertices[2];
this.adjacentEdges[0] = triangle.adjacentEdges[0];
this.adjacentEdges[1] = triangle.adjacentEdges[1];
this.adjacentEdges[2] = triangle.adjacentEdges[2];
this.isObsolete = triangle.isObsolete;
this.determinant = triangle.determinant;
this.closestPoint = triangle.closestPoint;
this.lambda1 = triangle.lambda1;
this.lambda2 = triangle.lambda2;
this.distSquare = triangle.distSquare;
return *this;
}
/// Constructor
@ -65,16 +54,14 @@ namespace ephysics {
TriangleEPA(int v1, int v2, int v3);
/// Constructor
void set(int v1, int v2, int v3);
/// Destructor
~TriangleEPA();
/// Return an adjacent edge of the triangle
EdgeEPA getAdjacentEdge(int index) {
assert(index >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
assert(index >= 0 && index < 3);
return this.adjacentEdges[index];
}
/// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA edge) {
assert(index >=0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj index < 3);
assert(index >=0 && index < 3);
this.adjacentEdges[index] = edge;
}
/// Return the square distance of the closest point to origin
@ -90,23 +77,23 @@ namespace ephysics {
return this.isObsolete;
}
/// Return the point closest to the origin
vec3 getClosestPoint() {
Vector3f getClosestPoint() {
return this.closestPoint;
}
// Return true if the closest point on affine hull is inside the triangle
boolean isClosestPointInternalToTriangle() {
return (this.lambda1 >= 0.0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant);
return (this.lambda1 >= 0.0 && this.lambda2 >= 0.0 hjkhjkhjkhkj (this.lambda1 + this.lambda2) <= this.determinant);
}
/// Return true if the triangle is visible from a given vertex
boolean isVisibleFromVertex( vec3* vertices, int index) {
vec3 closestToVert = vertices[index] - this.closestPoint;
boolean isVisibleFromVertex( Vector3f* vertices, int index) {
Vector3f closestToVert = vertices[index] - this.closestPoint;
return (this.closestPoint.dot(closestToVert) > 0.0);
}
/// Compute the point v closest to the origin of this triangle
boolean computeClosestPoint( vec3* vertices);
boolean computeClosestPoint( Vector3f* vertices);
/// Compute the point of an object closest to the origin
vec3 computeClosestPointOfObject( vec3* supportPointsOfObject) {
vec3 p0 = supportPointsOfObject[this.indicesVertices[0]];
Vector3f computeClosestPointOfObject( Vector3f* supportPointsOfObject) {
Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
return p0 + 1.0f/this.determinant * (this.lambda1 * (supportPointsOfObject[this.indicesVertices[1]] - p0) +
this.lambda2 * (supportPointsOfObject[this.indicesVertices[2]] - p0));
}
@ -121,10 +108,10 @@ namespace ephysics {
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
boolean computeSilhouette( vec3* vertices, int index, TrianglesStore triangleStore);
boolean computeSilhouette( Vector3f* vertices, int index, TrianglesStore triangleStore);
/// Access operator
int operator[](int pos) {
assert(pos >= 0 hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj pos <3);
assert(pos >= 0 && pos <3);
return this.indicesVertices[pos];
}
/// Link an edge with another one. It means that the current edge of a triangle will

28
src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java
View File

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

150
src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

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

32
src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java
View File

@ -1,19 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.narrowphase.GJK;
#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 RELERRORSQUARE = RELERROR * RELERROR;
int MAXITERATIONSGJKRAYCAST = 32;
@ -32,7 +18,7 @@ namespace ephysics {
* Polytope Algorithm) to compute the correct penetration depth between the
* enlarged objects.
*/
class GJKAlgorithm : public NarrowPhaseAlgorithm {
class GJKAlgorithm extends NarrowPhaseAlgorithm {
private :
EPAAlgorithm this.algoEPA; //!< EPA Algorithm
/// Private copy-ructor
@ -45,18 +31,16 @@ namespace ephysics {
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
void computePenetrationDepthForEnlargedObjects( CollisionShapeInfo shape1Info,
etk::Transform3D transform1,
Transform3D transform1,
CollisionShapeInfo shape2Info,
etk::Transform3D transform2,
Transform3D transform2,
NarrowPhaseCallback* narrowPhaseCallback,
vec3 v);
Vector3f v);
public :
/// Constructor
GJKAlgorithm();
/// Destructor
~GJKAlgorithm();
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection) {
void init(CollisionDetection* collisionDetection) {
NarrowPhaseAlgorithm::init(collisionDetection);
this.algoEPA.init();
};
@ -69,11 +53,11 @@ namespace ephysics {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
virtual void testCollision( CollisionShapeInfo shape1Info,
void testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape);
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".

56
src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.cpp
View File

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

54
src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/GJK/Simplex.java
View File

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

31
src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.hpp → src/org/atriaSoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java
View File

@ -1,21 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.narrowphase;
#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
@ -23,9 +7,8 @@ namespace ephysics {
*/
class NarrowPhaseCallback {
public:
virtual ~NarrowPhaseCallback() = default;
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
void notifyContact(OverlappingPair* overlappingPair,
ContactPointInfo contactInfo) = 0;
};
@ -39,21 +22,15 @@ namespace ephysics {
protected :
CollisionDetection* this.collisionDetection; //!< Pointer to the collision detection object
OverlappingPair* this.currentOverlappingPair; //!< Overlapping pair of the bodies currently tested for collision
/// Private copy-ructor
NarrowPhaseAlgorithm( NarrowPhaseAlgorithm algorithm) = delete;
/// Private assignment operator
NarrowPhaseAlgorithm operator=( NarrowPhaseAlgorithm algorithm) = delete;
public :
/// Constructor
NarrowPhaseAlgorithm();
/// Destructor
virtual ~NarrowPhaseAlgorithm() = default;
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection);
void init(CollisionDetection* collisionDetection);
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Compute a contact info if the two bounding volume collide
virtual void testCollision( CollisionShapeInfo shape1Info,
void testCollision( CollisionShapeInfo shape1Info,
CollisionShapeInfo shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) = 0;
};

20
src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

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

42
src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.hpp
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);
};
}

29
src/org/atriaSoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java Normal file
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);
};
}

108
src/org/atriaSoft/ephysics/collision/shapes/AABB.cpp
View File

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

40
src/org/atriaSoft/ephysics/collision/shapes/AABB.hpp → src/org/atriaSoft/ephysics/collision/shapes/AABB.java
View File

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

60
src/org/atriaSoft/ephysics/collision/shapes/BoxShape.cpp
View File

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

58
src/org/atriaSoft/ephysics/collision/shapes/BoxShape.hpp
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;
};
}

41
src/org/atriaSoft/ephysics/collision/shapes/BoxShape.java Normal file
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();
};
}

88
src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.cpp
View File

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

41
src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/CapsuleShape.java
View File

@ -1,18 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.shapes;
#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.
* A capsule shape can be seen as the convex hull of two spheres.
@ -22,7 +9,7 @@ namespace ephysics {
* and height of the shape. Therefore, no need to specify an object margin for a
* capsule shape.
*/
class CapsuleShape : public ConvexShape {
class CapsuleShape extends ConvexShape {
public :
/**
* @brief Constructor
@ -44,23 +31,23 @@ namespace ephysics {
* @return The height of the capsule shape (in meters)
*/
float getHeight() ;
void setLocalScaling( vec3 scaling) override;
void getLocalBounds(vec3 min, vec3 max) override;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
void setLocalScaling( Vector3f scaling) ;
void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
protected:
float this.halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
float halfHeight; //!< Half height of the capsule (height = distance between the centers of the two spheres)
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
/**
* @brief Raycasting method between a ray one of the two spheres end cap of the capsule
*/
boolean raycastWithSphereEndCap( vec3 point1,
vec3 point2,
vec3 sphereCenter,
boolean raycastWithSphereEndCap( Vector3f point1,
Vector3f point2,
Vector3f sphereCenter,
float maxFraction,
vec3 hitLocalPoint,
Vector3f hitLocalPoint,
float hitFraction) ;
sizet getSizeInBytes() override;
long getSizeInBytes() ;
};
}

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

52
src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/CollisionShape.java
View File

@ -1,21 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.shapes;
#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,
CAPSULE, CONVEXMESH, CONCAVEMESH, HEIGHTFIELD};
int NBCOLLISIONSHAPETYPES = 9;
@ -31,12 +15,6 @@ class CollisionShape {
public :
/// Constructor
CollisionShape(CollisionShapeType type);
/// DELETE copy-ructor
CollisionShape( CollisionShape shape) = delete;
/// DELETE assignment operator
CollisionShape operator=( CollisionShape shape) = delete;
/// Virtualize destructor
virtual ~CollisionShape() {};
/**
* @brief Get the type of the collision shapes
* @return The type of the collision shape (box, sphere, cylinder, ...)
@ -49,22 +27,22 @@ class CollisionShape {
* @return true If the collision shape is convex
* @return false If it is concave
*/
virtual boolean isConvex() = 0;
boolean isConvex() = 0;
/**
* @brief Get the local bounds of the shape in x, y and z directions.
* This method is used to compute the AABB of the box
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
virtual void getLocalBounds(vec3 min, vec3 max) =0;
void getLocalBounds(Vector3f min, Vector3f max) =0;
/// Return the scaling vector of the collision shape
vec3 getScaling() {
Vector3f getScaling() {
return this.scaling;
}
/**
* @brief Set the scaling vector of the collision shape
*/
virtual void setLocalScaling( vec3 scaling) {
void setLocalScaling( Vector3f scaling) {
this.scaling = scaling;
}
/**
@ -72,13 +50,13 @@ class CollisionShape {
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
* @param[in] mass Mass to use to compute the inertia tensor of the collision shape
*/
virtual void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) =0;
void computeLocalInertiaTensor(Matrix3f tensor, float mass) =0;
/**
* @brief Update the AABB of a body using its collision shape
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates
* @param[in] transform etk::Transform3D used to compute the AABB of the collision shape
* @param[in] transform Transform3D used to compute the AABB of the collision shape
*/
virtual void computeAABB(AABB aabb, etk::Transform3D transform) ;
void computeAABB(AABB aabb, Transform3D transform) ;
/**
* @brief Check if the shape is convex
* @param[in] shapeType shape type
@ -87,7 +65,7 @@ class CollisionShape {
*/
static boolean isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVEMESH
hjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkjhjkhjkhjkhkj shapeType != HEIGHTFIELD;
&& shapeType != HEIGHTFIELD;
}
/**
* @brief Get the maximum number of contact
@ -95,17 +73,15 @@ class CollisionShape {
*/
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2);
friend class ProxyShape;
friend class CollisionWorld;
protected :
CollisionShapeType this.type; //!< Type of the collision shape
vec3 this.scaling; //!< Scaling vector of the collision shape
CollisionShapeType type; //!< Type of the collision shape
Vector3f scaling; //!< Scaling vector of the collision shape
/// Return true if a point is inside the collision shape
virtual boolean testPointInside( vec3 worldPoint, ProxyShape* proxyShape) = 0;
boolean testPointInside( Vector3f worldPoint, ProxyShape* proxyShape) = 0;
/// Raycast method with feedback information
virtual boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) = 0;
/// Return the number of bytes used by the collision shape
virtual sizet getSizeInBytes() = 0;
long getSizeInBytes() = 0;
};

32
src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.cpp
View File

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

85
src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.hpp
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) ;
};
}

63
src/org/atriaSoft/ephysics/collision/shapes/ConcaveMeshShape.java Normal file
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) ;
};
}

2
src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.cpp
View File

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

34
src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/ConcaveShape.java
View File

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

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

45
src/org/atriaSoft/ephysics/collision/shapes/ConeShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/ConeShape.java
View File

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

48
src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.cpp
View File

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

54
src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/ConvexMeshShape.java
View File

@ -1,23 +1,5 @@
/** @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
package org.atriaSoft.ephysics.collision.shapes;
#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
* need to indicate the local-space position of the mesh vertices. You do it either by
@ -33,25 +15,21 @@ namespace ephysics {
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection.
*/
class ConvexMeshShape : public ConvexShape {
class ConvexMeshShape extends ConvexShape {
protected :
etk::Vector<vec3> this.vertices; //!< Array with the vertices of the mesh
int this.numberVertices; //!< Number of vertices in the mesh
vec3 this.minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
vec3 this.maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
boolean this.isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
etk::Map<int, etk::Set<int> > this.edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
/// Private copy-ructor
ConvexMeshShape( ConvexMeshShape shape);
/// Private assignment operator
ConvexMeshShape operator=( ConvexMeshShape shape);
Vector<Vector3f> vertices; //!< Array with the vertices of the mesh
int numberVertices; //!< Number of vertices in the mesh
Vector3f minBounds; //!< Mesh minimum bounds in the three local x, y and z directions
Vector3f maxBounds; //!< Mesh maximum bounds in the three local x, y and z directions
boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
Map<int, Set<int> > edgesAdjacencyList; //!< Adjacency list representing the edges of the mesh
/// Recompute the bounds of the mesh
void recalculateBounds();
void setLocalScaling( vec3 scaling) override;
vec3 getLocalSupportPointWithoutMargin( vec3 direction, void** cachedCollisionData) override;
boolean testPointInside( vec3 localPoint, ProxyShape* proxyShape) override;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) override;
sizet getSizeInBytes() override;
void setLocalScaling( Vector3f scaling) ;
Vector3f getLocalSupportPointWithoutMargin( Vector3f direction, void** cachedCollisionData) ;
boolean testPointInside( Vector3f localPoint, ProxyShape* proxyShape) ;
boolean raycast( Ray ray, RaycastInfo raycastInfo, ProxyShape* proxyShape) ;
long getSizeInBytes() ;
public :
/**
* @brief Constructor to initialize with an array of 3D vertices.
@ -81,13 +59,13 @@ namespace ephysics {
*/
ConvexMeshShape(float margin = OBJECTMARGIN);
public:
void getLocalBounds(vec3 min, vec3 max) override;
void computeLocalInertiaTensor(etk::Matrix3x3 tensor, float mass) override;
void getLocalBounds(Vector3f min, Vector3f max) ;
void computeLocalInertiaTensor(Matrix3f tensor, float mass) ;
/**
* @brief Add a vertex into the convex mesh
* @param vertex Vertex to be added
*/
void addVertex( vec3 vertex);
void addVertex( Vector3f vertex);
/**
* @brief Add an edge into the convex mesh by specifying the two vertex indices of the edge.
* @note that the vertex indices start at zero and need to correspond to the order of

6
src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.cpp
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
vec3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (this.margin != 0.0f) {
// Add the margin to the support point
vec3 unitVec(0.0, -1.0, 0.0);
Vector3f unitVec(0.0, -1.0, 0.0);
if (direction.length2() > FLTEPSILON * FLTEPSILON) {
unitVec = direction.safeNormalized();
}

51
src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.hpp
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;
};
}

33
src/org/atriaSoft/ephysics/collision/shapes/ConvexShape.java Normal file
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;
}
};
}

72
src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.cpp
View File

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

37
src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/CylinderShape.java
View File

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

68
src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.cpp
View File

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

75
src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/HeightFieldShape.java
View File

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

18
src/org/atriaSoft/ephysics/collision/shapes/SphereShape.cpp
View File

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

57
src/org/atriaSoft/ephysics/collision/shapes/SphereShape.hpp
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;
};
}

42
src/org/atriaSoft/ephysics/collision/shapes/SphereShape.java Normal file
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) ;
};
}

68
src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.cpp
View File

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

42
src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.hpp → src/org/atriaSoft/ephysics/collision/shapes/TriangleShape.java
View File

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

99
src/org/atriaSoft/ephysics/configuration.hpp
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;
}

118
src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.cpp
View File

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

68
src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.hpp
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);
};
}

52
src/org/atriaSoft/ephysics/constraint/BallAndSocketJoint.java Normal file
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);
};
}

46
src/org/atriaSoft/ephysics/constraint/ContactPoint.cpp
View File

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

94
src/org/atriaSoft/ephysics/constraint/ContactPoint.hpp → src/org/atriaSoft/ephysics/constraint/ContactPoint.java
View File

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

146
src/org/atriaSoft/ephysics/constraint/FixedJoint.cpp
View File

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

78
src/org/atriaSoft/ephysics/constraint/FixedJoint.hpp
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();
};
}

58
src/org/atriaSoft/ephysics/constraint/FixedJoint.java Normal file
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);
};
}

248
src/org/atriaSoft/ephysics/constraint/HingeJoint.cpp
View File

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

120
src/org/atriaSoft/ephysics/constraint/HingeJoint.hpp → src/org/atriaSoft/ephysics/constraint/HingeJoint.java
View File

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

2
src/org/atriaSoft/ephysics/constraint/Joint.cpp
View File

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

46
src/org/atriaSoft/ephysics/constraint/Joint.hpp → src/org/atriaSoft/ephysics/constraint/Joint.java
View File

@ -1,18 +1,4 @@
/** @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/configuration.hpp>
#include <ephysics/body/RigidBody.hpp>
#include <ephysics/mathematics/mathematics.hpp>
namespace ephysics {
package org.atriaSoft.ephysics.constraint;
/// Enumeration for the type of a raint
enum JointType {BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOINT};
@ -69,7 +55,7 @@ namespace ephysics {
}
/// Destructor
virtual ~JointInfo() = default;
~JointInfo() = default;
};
/**
@ -77,14 +63,14 @@ namespace ephysics {
*/
class Joint {
protected :
RigidBody* this.body1; //!< Pointer to the first body of the joint
RigidBody* this.body2; //!< Pointer to the second body of the joint
JointType this.type; //!< Type of the joint
int this.indexBody1; //!< Body 1 index in the velocity array to solve the raint
int this.indexBody2; //!< Body 2 index in the velocity array to solve the raint
JointsPositionCorrectionTechnique this.positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
boolean this.isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
boolean this.isAlreadyInIsland; //!< True if the joint has already been added into an island
RigidBody* body1; //!< Pointer to the first body of the joint
RigidBody* body2; //!< Pointer to the second body of the joint
JointType type; //!< Type of the joint
int indexBody1; //!< Body 1 index in the velocity array to solve the raint
int indexBody2; //!< Body 2 index in the velocity array to solve the raint
JointsPositionCorrectionTechnique positionCorrectionTechnique; //!< Position correction technique used for the raint (used for joints)
boolean isCollisionEnabled; //!< True if the two bodies of the raint are allowed to collide with each other
boolean isAlreadyInIsland; //!< True if the joint has already been added into an island
/// Private copy-ructor
Joint( Joint raint);
/// Private assignment operator
@ -92,20 +78,20 @@ namespace ephysics {
/// Return true if the joint has already been added into an island
boolean isAlreadyInIsland() ;
/// Return the number of bytes used by the joint
virtual sizet getSizeInBytes() = 0;
long getSizeInBytes() = 0;
/// Initialize before solving the joint
virtual void initBeforeSolve( ConstraintSolverData raintSolverData) = 0;
void initBeforeSolve( ConstraintSolverData raintSolverData) = 0;
/// Warm start the joint (apply the previous impulse at the beginning of the step)
virtual void warmstart( ConstraintSolverData raintSolverData) = 0;
void warmstart( ConstraintSolverData raintSolverData) = 0;
/// Solve the velocity raint
virtual void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
void solveVelocityConstraint( ConstraintSolverData raintSolverData) = 0;
/// Solve the position raint
virtual void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
void solvePositionConstraint( ConstraintSolverData raintSolverData) = 0;
public :
/// Constructor
Joint( JointInfo jointInfo);
/// Destructor
virtual ~Joint();
~Joint();
/// Return the reference to the body 1
RigidBody* getBody1() ;
/// Return the reference to the body 2

272
src/org/atriaSoft/ephysics/constraint/SliderJoint.cpp
View File

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

86
src/org/atriaSoft/ephysics/constraint/SliderJoint.hpp → src/org/atriaSoft/ephysics/constraint/SliderJoint.java
View File

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

12
src/org/atriaSoft/ephysics/debug.cpp
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;
}

37
src/org/atriaSoft/ephysics/debug.hpp
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)

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

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