mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add error correction using first world order projection (not active by default), change the Shape class into Collider class, add the new decimal type in order to easily switch between single and double precision

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@460 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2012-01-18 23:06:33 +00:00
parent 7efd553f37
commit c1eabd3e2b
56 changed files with 1092 additions and 834 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/body/Body.cpp
View File

@ -25,16 +25,16 @@
// Libraries
#include "Body.h"
#include "../shapes/Shape.h"
#include "../shapes/Collider.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned int id)
: shape(shape), mass(mass), transform(transform), id(id) {
Body::Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id)
: collider(collider), mass(mass), transform(transform), id(id) {
assert(mass > 0.0);
assert(shape);
assert(collider);
isMotionEnabled = true;
isCollisionEnabled = true;
@ -44,7 +44,7 @@ Body::Body(const Transform& transform, Shape* shape, double mass, long unsigned
oldTransform = transform;
// Create the AABB for broad-phase collision detection
aabb = new AABB(transform, shape->getLocalExtents(OBJECT_MARGIN));
aabb = new AABB(transform, collider->getLocalExtents(OBJECT_MARGIN));
}
// Destructor

70
src/body/Body.h
View File

@ -31,8 +31,8 @@
#include <cassert>
#include "../mathematics/Transform.h"
#include "../shapes/AABB.h"
#include "../shapes/Shape.h"
#include "../constants.h"
#include "../shapes/Collider.h"
#include "../configuration.h"
// Namespace reactphysics3d
namespace reactphysics3d {
@ -46,36 +46,36 @@ namespace reactphysics3d {
*/
class Body {
protected :
Shape* shape; // Collision shape of the body
double mass; // Mass of the body
Collider* collider; // Collider of the body
decimal mass; // Mass of the body
Transform transform; // Position and orientation of the body
Transform oldTransform; // Last position and orientation of the body
double interpolationFactor; // Interpolation factor used for the state interpolation
decimal interpolationFactor; // Interpolation factor used for the state interpolation
bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies
AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection
luint id; // ID of the body
public :
Body(const Transform& transform, Shape* shape, double mass, long unsigned int id); // Constructor
virtual ~Body(); // Destructor
Body(const Transform& transform, Collider* collider, decimal mass, long unsigned int id); // Constructor
virtual ~Body(); // Destructor
luint getID() const; // Return the id of the body
Shape* getShape() const; // Return the collision shape
void setShape(Shape* shape); // Set the collision shape
double getMass() const; // Return the mass of the body
void setMass(double mass); // Set the mass of the body
const Transform& getTransform() const; // Return the current position and orientation
void setTransform(const Transform& transform); // Set the current position and orientation
const AABB* getAABB() const; // Return the AAABB of the body
Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering
void setInterpolationFactor(double factor); // Set the interpolation factor of the body
bool getIsMotionEnabled() const; // Return if the rigid body can move
void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move
bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
void updateOldTransform(); // Update the old transform with the current one
void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates
luint getID() const; // Return the id of the body
Collider* getCollider() const; // Return the collision collider
void setCollider(Collider* collider); // Set the collision collider
decimal getMass() const; // Return the mass of the body
void setMass(decimal mass); // Set the mass of the body
const Transform& getTransform() const; // Return the current position and orientation
void setTransform(const Transform& transform); // Set the current position and orientation
const AABB* getAABB() const; // Return the AAABB of the body
Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering
void setInterpolationFactor(decimal factor); // Set the interpolation factor of the body
bool getIsMotionEnabled() const; // Return if the rigid body can move
void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move
bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
void updateOldTransform(); // Update the old transform with the current one
void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates
};
// Return the id of the body
@ -83,20 +83,20 @@ inline luint Body::getID() const {
return id;
}
// Return the collision shape
inline Shape* Body::getShape() const {
assert(shape);
return shape;
// Return the collider
inline Collider* Body::getCollider() const {
assert(collider);
return collider;
}
// Set the collision shape
inline void Body::setShape(Shape* shape) {
assert(shape);
this->shape = shape;
// Set the collider
inline void Body::setCollider(Collider* collider) {
assert(collider);
this->collider = collider;
}
// Method that return the mass of the body
inline double Body::getMass() const {
inline decimal Body::getMass() const {
return mass;
};
@ -106,7 +106,7 @@ inline Transform Body::getInterpolatedTransform() const {
}
// Set the interpolation factor of the body
inline void Body::setInterpolationFactor(double factor) {
inline void Body::setInterpolationFactor(decimal factor) {
// Set the factor
interpolationFactor = factor;
}
@ -122,7 +122,7 @@ inline void Body::setIsMotionEnabled(bool isMotionEnabled) {
}
// Method that set the mass of the body
inline void Body::setMass(double mass) {
inline void Body::setMass(decimal mass) {
this->mass = mass;
}
@ -160,7 +160,7 @@ inline void Body::updateOldTransform() {
// Update the rigid body in order to reflect a change in the body state
inline void Body::updateAABB() {
// Update the AABB
aabb->update(transform, shape->getLocalExtents(OBJECT_MARGIN));
aabb->update(transform, collider->getLocalExtents(OBJECT_MARGIN));
}

12
src/body/RigidBody.cpp
View File

@ -25,23 +25,23 @@
// Libraries
#include "RigidBody.h"
#include "../shapes/Shape.h"
#include "../shapes/Collider.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape, long unsigned id)
: Body(transform, shape, mass, id), inertiaTensorLocal(inertiaTensorLocal),
RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider, long unsigned id)
: Body(transform, collider, mass, id), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) {
restitution = 1.0;
// Set the body pointer of the AABB and the shape
// Set the body pointer of the AABB and the collider
aabb->setBodyPointer(this);
shape->setBodyPointer(this);
collider->setBodyPointer(this);
assert(shape);
assert(collider);
assert(aabb);
}

36
src/body/RigidBody.h
View File

@ -29,7 +29,7 @@
// Libraries
#include <cassert>
#include "Body.h"
#include "../shapes/Shape.h"
#include "../shapes/Collider.h"
#include "../mathematics/mathematics.h"
// Namespace reactphysics3d
@ -50,32 +50,32 @@ class RigidBody : public Body {
Vector3 externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates)
double massInverse; // Inverse of the mass of the body
double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
decimal massInverse; // Inverse of the mass of the body
decimal restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
public :
RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal,
Shape* shape, long unsigned int id); // Constructor // Copy-constructor
RigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal,
Collider* collider, long unsigned int id); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
Vector3 getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body
Vector3 getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3& angularVelocity); // Set the angular velocity
void setMassInverse(double massInverse); // Set the inverse of the mass
void setMassInverse(decimal massInverse); // Set the inverse of the mass
Vector3 getExternalForce() const; // Return the current external force of the body
void setExternalForce(const Vector3& force); // Set the current external force on the body
Vector3 getExternalTorque() const; // Return the current external torque of the body
void setExternalTorque(const Vector3& torque); // Set the current external torque of the body
double getMassInverse() const; // Return the inverse of the mass of the body
Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates)
Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor
Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates
Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates
decimal getMassInverse() const; // Return the inverse of the mass of the body
Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates)
Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor
Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates
Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates
double getRestitution() const; // Get the restitution coefficient
void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient
decimal getRestitution() const; // Get the restitution coefficient
void setRestitution(decimal restitution) throw(std::invalid_argument); // Set the restitution coefficient
};
// Return the linear velocity
@ -93,7 +93,7 @@ inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
}
// Set the inverse of the mass
inline void RigidBody::setMassInverse(double massInverse) {
inline void RigidBody::setMassInverse(decimal massInverse) {
this->massInverse = massInverse;
}
@ -123,7 +123,7 @@ inline void RigidBody::setExternalTorque(const Vector3& torque) {
}
// Return the inverse of the mass of the body
inline double RigidBody::getMassInverse() const {
inline decimal RigidBody::getMassInverse() const {
return massInverse;
}
@ -165,12 +165,12 @@ inline void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
}
// Get the restitution coeffficient of the rigid body
inline double RigidBody::getRestitution() const {
inline decimal RigidBody::getRestitution() const {
return restitution;
}
// Set the restitution coefficient
inline void RigidBody::setRestitution(double restitution) throw(std::invalid_argument) {
inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_argument) {
// Check if the restitution coefficient is between 0 and 1
if (restitution >= 0.0 && restitution <= 1.0) {
this->restitution = restitution;

14
src/collision/CollisionDetection.cpp
View File

@ -28,9 +28,9 @@
#include "broadphase/SweepAndPruneAlgorithm.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "../body/Body.h"
#include "../shapes/BoxShape.h"
#include "../shapes/BoxCollider.h"
#include "../body/RigidBody.h"
#include "../constants.h"
#include "../configuration.h"
#include <cassert>
#include <complex>
#include <set>
@ -99,7 +99,6 @@ void CollisionDetection::computeBroadPhase() {
overlappingPairs.erase(*it);
}
// The current overlapping pairs become the last step overlapping pairs
lastStepOverlappingPairs = currentStepOverlappingPairs;
}
@ -120,14 +119,17 @@ bool CollisionDetection::computeNarrowPhase() {
(*it).second->update();
// Use the narrow-phase collision detection algorithm to check if there really are a contact
if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(),
body2->getShape(), body2->getTransform(), contactInfo)) {
if (narrowPhaseAlgorithm->testCollision(body1->getCollider(), body1->getTransform(),
body2->getCollider(), body2->getTransform(), contactInfo)) {
assert(contactInfo);
collisionExists = true;
// Create a new contact
Contact* contact = new(memoryPoolContacts.allocateObject()) Contact(contactInfo);
// Free the contact info memory
delete contactInfo;
// Add the contact to the contact cache of the corresponding overlapping pair
(*it).second->addContact(contact);
@ -146,7 +148,7 @@ bool CollisionDetection::computeNarrowPhase() {
// Allow the broadphase to notify the collision detection about an overlapping pair
// This method is called by a broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(Body* body1, Body* body2) {
// Construct the pair of index
// Construct the pair of body index
pair<luint, luint> indexPair = body1->getID() < body2->getID() ? make_pair(body1->getID(), body2->getID()) :
make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);

12
src/collision/CollisionDetection.h
View File

@ -68,12 +68,12 @@ class CollisionDetection {
bool computeNarrowPhase(); // Compute the narrow-phase collision detection
public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor
OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor
OverlappingPair* getOverlappingPair(luint body1ID, luint body2ID); // Return an overlapping pair or null
bool computeCollisionDetection(); // Compute the collision detection
void broadPhaseNotifyOverlappingPair(Body* body1, Body* body2); // Allow the broadphase to notify the collision detection about an overlapping pair
};
// Return an overlapping pair of bodies according to the given bodies ID

2
src/collision/ContactInfo.cpp
View File

@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor for GJK
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2,
const Transform& transform1, const Transform& transform2)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth),

6
src/collision/ContactInfo.h
View File

@ -27,7 +27,7 @@
#define CONTACT_INFO_H
// Libraries
#include "../shapes/BoxShape.h"
#include "../shapes/BoxCollider.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
@ -46,13 +46,13 @@ struct ContactInfo {
Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact
const Vector3 normal; // Normal vector the the collision contact in world space
const double penetrationDepth; // Penetration depth of the contact
const decimal penetrationDepth; // Penetration depth of the contact
const Vector3 localPoint1; // Contact point of body 1 in local space of body 1
const Vector3 localPoint2; // Contact point of body 2 in local space of body 2
const Vector3 worldPoint1; // Contact point of body 1 in world space
const Vector3 worldPoint2; // Contact point of body 2 in world space
ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth,
ContactInfo(Body* body1, Body* body2, const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2,
const Transform& transform1, const Transform& transform2); // Constructor for GJK
};

12
src/collision/PersistentContactCache.cpp
View File

@ -131,7 +131,7 @@ void PersistentContactCache::update(const Transform& transform1, const Transform
int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const {
assert(nbContacts == MAX_CONTACTS_IN_CACHE);
int indexMaxPenetrationDepth = -1;
double maxPenetrationDepth = newContact->getPenetrationDepth();
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<nbContacts; i++) {
@ -152,10 +152,10 @@ int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) co
// kept.
int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(nbContacts == MAX_CONTACTS_IN_CACHE);
double area0 = 0.0; // Area with contact 1,2,3 and newPoint
double area1 = 0.0; // Area with contact 0,2,3 and newPoint
double area2 = 0.0; // Area with contact 0,1,3 and newPoint
double area3 = 0.0; // Area with contact 0,1,2 and newPoint
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
@ -191,7 +191,7 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect
}
// Return the index of maximum area
int PersistentContactCache::getMaxArea(double area0, double area1, double area2, double area3) const {
int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;

12
src/collision/PersistentContactCache.h
View File

@ -59,11 +59,11 @@ class PersistentContactCache {
uint nbContacts; // Number of contacts in the cache
MemoryPool<Contact>& memoryPoolContacts; // Reference to the memory pool with the contacts
int getMaxArea(double area0, double area1, double area2, double area3) const; // Return the index of maximum area
int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed
void removeContact(int index); // Remove a contact from the cache
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; // Return true if two vectors are approximatively equal
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; // Return the index of maximum area
int getIndexOfDeepestPenetration(Contact* newContact) const; // Return the index of the contact with the larger penetration depth
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; // Return the index that will be removed
void removeContact(int index); // Remove a contact from the cache
bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; // Return true if two vectors are approximatively equal
public:
PersistentContactCache(Body* const body1, Body* const body2, MemoryPool<Contact>& memoryPoolContacts); // Constructor
@ -88,7 +88,7 @@ inline Contact* PersistentContactCache::getContact(uint index) const {
// Return true if two vectors are approximatively equal
inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, const Vector3& vector2) const {
const double epsilon = 0.1;
const decimal epsilon = 0.1;
return (approxEqual(vector1.getX(), vector2.getX(), epsilon) &&
approxEqual(vector1.getY(), vector2.getY(), epsilon) &&
approxEqual(vector1.getZ(), vector2.getZ(), epsilon));

8
src/collision/broadphase/SweepAndPruneAlgorithm.cpp
View File

@ -79,13 +79,13 @@ void SweepAndPruneAlgorithm::notifyAddedBodies(vector<RigidBody*> bodies) {
// the collision detection object about overlapping pairs using the
// broadPhaseNotifyOverlappingPair() method from the CollisionDetection class
void SweepAndPruneAlgorithm::computePossibleCollisionPairs() {
double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
decimal variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
decimal esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
decimal esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
vector<const AABB*>::iterator it2; // Second iterator
Vector3 center3D; // Center of the current AABB
double center[3]; // Coordinates of the center of the current AABB
decimal center[3]; // Coordinates of the center of the current AABB
int i;
const Body* body; // Body pointer on the body corresponding to an AABB
uint nbAABBs = sortedAABBs.size(); // Number of AABBs

5
src/collision/broadphase/SweepAndPruneAlgorithm.h
View File

@ -23,14 +23,13 @@
* *
********************************************************************************/
#ifndef SAP_ALGORITHM_H
#define SAP_ALGORITHM_H
#ifndef SWEEP_AND_PRUNE_ALGORITHM_H
#define SWEEP_AND_PRUNE_ALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../../shapes/AABB.h"
// TODO : Rename this class SweepAndPruneAlgorithm
// Namespace ReactPhysics3D
namespace reactphysics3d {

78
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -80,26 +80,26 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, co
// intersect. An initial simplex that contains origin has been computed with
// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo) {
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
// Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
Transform shape2ToShape1 = transform1.inverse() * transform2;
// Transform 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)
Transform body2Tobody1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance
double tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
unsigned int nbTriangles = 0;
@ -132,7 +132,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
int minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60)
const double sin60 = sqrt(3.0) * 0.5;
const decimal sin60 = sqrt(3.0) * 0.5;
// Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3
@ -147,18 +147,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3 v3 = rotationMat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v1), OBJECT_MARGIN);
suppPointsA[2] = collider1->getLocalSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v1), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v2), OBJECT_MARGIN);
suppPointsA[3] = collider1->getLocalSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v2), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v3), OBJECT_MARGIN);
suppPointsA[4] = collider1->getLocalSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-v3), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@ -221,10 +221,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX);
break;
}
@ -252,11 +252,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-n), OBJECT_MARGIN);
suppPointsA[3] = collider1->getLocalSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-n), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPoint(-n, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
suppPointsA[4] = collider1->getLocalSupportPoint(-n, OBJECT_MARGIN);
suppPointsB[4] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * n, OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces
@ -287,12 +287,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
// Add the candidate faces in the heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_MAX);
addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_MAX);
nbVertices = 5;
}
@ -307,7 +307,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
TriangleEPA* triangle = 0;
double upperBoundSquarePenDepth = DBL_MAX;
decimal upperBoundSquarePenDepth = DECIMAL_MAX;
do {
triangle = triangleHeap[0];
@ -325,23 +325,23 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
}
// Compute the support point of the Minkowski difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * (-triangle->getClosestPoint()), OBJECT_MARGIN);
suppPointsA[nbVertices] = collider1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * (-triangle->getClosestPoint()), OBJECT_MARGIN);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
nbVertices++;
// Update the upper bound of the penetration depth
double wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0);
double wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
}
// Compute the error
double error = wDotv - triangle->getDistSquare();
decimal error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] ||
@ -370,11 +370,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = shape2ToShape1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 pBLocal = body2Tobody1.inverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
double penetrationDepth = v.length();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal,
contactInfo = new ContactInfo(collider1->getBodyPointer(), collider2->getBodyPointer(), normal,
penetrationDepth, pALocal, pBLocal, transform1, transform2);
return true;

10
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../GJK/Simplex.h"
#include "../../../shapes/Shape.h"
#include "../../../shapes/Collider.h"
#include "../../ContactInfo.h"
#include "../../../mathematics/mathematics.h"
#include "TriangleEPA.h"
@ -75,7 +75,7 @@ class EPAAlgorithm {
TriangleComparison triangleComparison; // Triangle comparison operator
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap
uint& nbTriangles, decimal upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const; // Decide if the origin is in the tetrahedron
@ -83,14 +83,14 @@ class EPAAlgorithm {
EPAAlgorithm(); // Constructor
~EPAAlgorithm(); // Destructor
bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
bool computePenetrationDepthAndContactPoints(Simplex simplex, const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
Vector3& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, double upperBoundSquarePenDepth) {
uint& nbTriangles, decimal 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

12
src/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -56,11 +56,11 @@ bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
Vector3 v1 = vertices[indicesVertices[1]] - p0;
Vector3 v2 = vertices[indicesVertices[2]] - p0;
double v1Dotv1 = v1.dot(v1);
double v1Dotv2 = v1.dot(v2);
double v2Dotv2 = v2.dot(v2);
double p0Dotv1 = p0.dot(v1);
double p0Dotv2 = p0.dot(v2);
decimal v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2);
// Compute determinant
det = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
@ -126,7 +126,7 @@ bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex
// Mark the current triangle as obsolete because it
setIsObsolete(true);
// Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle
bool result = adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&

16
src/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../../../mathematics/mathematics.h"
#include "../../../constants.h"
#include "../../../configuration.h"
#include "EdgeEPA.h"
#include <cassert>
@ -51,11 +51,11 @@ class TriangleEPA {
uint indicesVertices[3]; // Indices of the vertices y_i of the triangle
EdgeEPA adjacentEdges[3]; // Three adjacent edges of the triangle (edges of other triangles)
bool isObsolete; // True if the triangle face is visible from the new support point
double det; // Determinant
Vector3 closestPoint; // Point v closest to the origin on the affine hull of the triangle
double lambda1; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
double lambda2; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
double distSquare; // Square distance of the point closest point v to the origin
decimal det; // Determinant
Vector3 closestPoint; // Point v closest to the origin on the affine hull of the triangle
decimal lambda1; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal lambda2; // Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal distSquare; // Square distance of the point closest point v to the origin
public:
TriangleEPA(); // Constructor
@ -64,7 +64,7 @@ class TriangleEPA {
EdgeEPA& getAdjacentEdge(int index); // Return an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA& edge); // Set an adjacent edge of the triangle
double getDistSquare() const; // Return the square distance of the closest point to origin
decimal getDistSquare() const; // Return the square distance of the closest point to origin
void setIsObsolete(bool isObsolete); // Set the isObsolete value
bool getIsObsolete() const; // Return true if the triangle face is obsolete
const Vector3& getClosestPoint() const; // Return the point closest to the origin
@ -93,7 +93,7 @@ inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
}
// Return the square distance of the closest point to origin
inline double TriangleEPA::getDistSquare() const {
inline decimal TriangleEPA::getDistSquare() const {
return distSquare;
}

2
src/collision/narrowphase/EPA/TrianglesStore.h
View File

@ -89,7 +89,7 @@ inline TriangleEPA& TrianglesStore::last() {
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2) {
TriangleEPA* newTriangle = 0;
// If we have not reach the maximum number of triangles
// If we have not reached the maximum number of triangles
if (nbTriangles != MAX_TRIANGLES) {
newTriangle = &triangles[nbTriangles++];
new (newTriangle) TriangleEPA(v0, v1, v2);

84
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -27,7 +27,7 @@
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "../../../constraint/Contact.h"
#include "../../../constants.h"
#include "../../../configuration.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
@ -58,31 +58,31 @@ GJKAlgorithm::~GJKAlgorithm() {
// 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.
bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
bool GJKAlgorithm::testCollision(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
ContactInfo*& contactInfo) {
assert(shape1 != shape2);
assert(collider1 != collider2);
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
double vDotw;
double prevDistSquare;
Body* const body1 = shape1->getBodyPointer();
Body* const body2 = shape2->getBodyPointer();
decimal vDotw;
decimal prevDistSquare;
Body* const body1 = collider1->getBodyPointer();
Body* const body2 = collider2->getBodyPointer();
// Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
Transform shape2ToShape1 = transform1.inverse() * transform2;
// Transform 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)
Transform body2Tobody1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects)
double margin = 2 * OBJECT_MARGIN;
double marginSquare = margin * margin;
decimal margin = 2 * OBJECT_MARGIN;
decimal marginSquare = margin * margin;
assert(margin > 0.0);
// Create a simplex set
@ -93,12 +93,12 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
// Initialize the upper bound for the square distance
double distSquare = DBL_MAX;
decimal distSquare = DECIMAL_MAX;
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPoint(-v);
suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v);
suppA = collider1->getLocalSupportPoint(-v);
suppB = body2Tobody1 * collider2->getLocalSupportPoint(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -118,14 +118,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
@ -146,14 +146,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
@ -172,14 +172,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
@ -206,14 +206,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Project those two points on the margins to have the closest points of both
// object with the margins
double dist = sqrt(distSquare);
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (OBJECT_MARGIN / dist) * v);
pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
@ -229,7 +229,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
// polytope to the EPA algorithm to compute the correct penetration depth and contact points between
// the enlarged objects.
return computePenetrationDepthForEnlargedObjects(shape1, transform1, shape2, transform2, contactInfo, v);
return computePenetrationDepthForEnlargedObjects(collider1, transform1, collider2, transform2, contactInfo, v);
}
// This method runs the GJK algorithm on the two enlarged objects (with margin)
@ -237,27 +237,27 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// assumed to intersect in the original objects (without margin). Therefore such
// a polytope must exist. Then, we give that polytope to the EPA algorithm to
// compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
const Shape* const shape2, const Transform& transform2,
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Collider* const collider1, const Transform& transform1,
const Collider* const collider2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3& v) {
Simplex simplex;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
double vDotw;
double distSquare = DBL_MAX;
double prevDistSquare;
decimal vDotw;
decimal distSquare = DECIMAL_MAX;
decimal prevDistSquare;
// Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
Transform shape2ToShape1 = transform1.inverse() * transform2;
// Transform 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)
Transform body2ToBody1 = transform1.inverse() * transform2;
// Matrix that transform a direction from body space of shape 1 into body space of shape 2
Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
do {
// Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPoint(-v, OBJECT_MARGIN);
suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
suppA = collider1->getLocalSupportPoint(-v, OBJECT_MARGIN);
suppB = body2ToBody1 * collider2->getLocalSupportPoint(rotateToBody2 * v, OBJECT_MARGIN);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -293,5 +293,5 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
// Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
// penetration depth and contact points between the two enlarged objects
return algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1, transform1, shape2, transform2, v, contactInfo);
return algoEPA.computePenetrationDepthAndContactPoints(simplex, collider1, transform1, collider2, transform2, v, contactInfo);
}

14
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -29,7 +29,7 @@
// Libraries
#include "../NarrowPhaseAlgorithm.h"
#include "../../ContactInfo.h"
#include "../../../shapes/Shape.h"
#include "../../../shapes/Collider.h"
#include "../EPA/EPAAlgorithm.h"
@ -37,8 +37,8 @@
namespace reactphysics3d {
// Constants
const double REL_ERROR = 1.0e-3;
const double REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const decimal REL_ERROR = 1.0e-3;
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
/* -------------------------------------------------------------------
Class GJKAlgorithm :
@ -61,16 +61,16 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
private :
EPAAlgorithm algoEPA; // EPA Algorithm
bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
bool computePenetrationDepthForEnlargedObjects(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects
public :
GJKAlgorithm(CollisionDetection& collisionDetection); // Constructor
~GJKAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
virtual bool testCollision(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};

12
src/collision/narrowphase/GJK/Simplex.cpp
View File

@ -222,7 +222,7 @@ bool Simplex::isProperSubset(Bits subset) const {
// Return true if the set is affinely dependent meaning that a point of the set
// is an affine combination of other points in the set
bool Simplex::isAffinelyDependent() const {
double sum = 0.0;
decimal sum = 0.0;
int i;
Bits bit;
@ -273,7 +273,7 @@ bool Simplex::isValidSubset(Bits subset) const {
// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
double deltaX = 0.0;
decimal deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0);
int i;
@ -290,7 +290,7 @@ void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
}
assert(deltaX > 0.0);
double factor = 1.0 / deltaX;
decimal factor = 1.0 / deltaX;
pA *= factor;
pB *= factor;
}
@ -327,13 +327,13 @@ bool Simplex::computeClosestPoint(Vector3& v) {
// Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3& v) {
double minDistSquare = DBL_MAX;
decimal minDistSquare = DECIMAL_MAX;
Bits bit;
for (bit = allBits; bit != 0x0; bit--) {
if (isSubset(bit, allBits) && isProperSubset(bit)) {
Vector3 u = computeClosestPointForSubset(bit);
double distSquare = u.dot(u);
decimal distSquare = u.dot(u);
if (distSquare < minDistSquare) {
minDistSquare = distSquare;
bitsCurrentSimplex = bit;
@ -348,7 +348,7 @@ void Simplex::backupClosestPointInSimplex(Vector3& v) {
Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
maxLengthSquare = 0.0;
double deltaX = 0.0; // deltaX = sum of all det[subset][i]
decimal deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i;
Bits bit;

12
src/collision/narrowphase/GJK/Simplex.h
View File

@ -49,13 +49,13 @@ typedef unsigned int Bits;
class Simplex {
private:
Vector3 points[4]; // Current points
double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2
double maxLengthSquare; // Maximum length of pointsLengthSquare[i]
decimal pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2
decimal maxLengthSquare; // Maximum length of pointsLengthSquare[i]
Vector3 suppPointsA[4]; // Support points of object A in local coordinates
Vector3 suppPointsB[4]; // Support points of object B in local coordinates
Vector3 diffLength[4][4]; // diff[i][j] contains points[i] - points[j]
double det[16][4]; // Cached determinant values
double normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2
decimal det[16][4]; // Cached determinant values
decimal normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2
Bits bitsCurrentSimplex; // 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 lastFound; // Number between 1 and 4 that identify the last found support point
@ -77,7 +77,7 @@ class Simplex {
bool isFull() const; // Return true if the simplex contains 4 points
bool isEmpty() const; // Return true if the simple is empty
unsigned int getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, Vector3* points) const; // Return the points of the simplex
double getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point
decimal getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3& point) const; // Return true if the point is in the simplex
bool isAffinelyDependent() const; // Return true if the set is affinely dependent
@ -107,7 +107,7 @@ inline bool Simplex::isEmpty() const {
}
// Return the maximum squared length of a point
inline double Simplex::getMaxLengthSquareOfAPoint() const {
inline decimal Simplex::getMaxLengthSquareOfAPoint() const {
return maxLengthSquare;
}

4
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -50,8 +50,8 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm(CollisionDetection& collisionDetection); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
virtual bool testCollision(const Collider* collider1, const Transform& transform1,
const Collider* collider2, const Transform& transform2,
ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
};

39
src/configuration.h
View File

@ -26,6 +26,11 @@
#ifndef CONFIGURATION_H
#define CONFIGURATION_H
// Libraries
#include <limits>
#include <cfloat>
#include "decimal.h"
// Windows platform
#if defined(WIN32) || defined(_WIN32) || defined(_WIN64) || defined(__WIN32__) || defined(__WINDOWS__)
#define WINDOWS_OS
@ -35,4 +40,38 @@
#define LINUX_OS
#endif
// Type definitions
typedef unsigned int uint;
typedef long unsigned int luint;
// Mathematical constants
const reactphysics3d::decimal DECIMAL_MIN = std::numeric_limits<reactphysics3d::decimal>::min(); // Minimun decimal value
const reactphysics3d::decimal DECIMAL_MAX = std::numeric_limits<reactphysics3d::decimal>::max(); // Maximum decimal value
const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits<reactphysics3d::decimal>::epsilon(); // Machine epsilon
const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits<reactphysics3d::decimal>::infinity(); // Infinity
const reactphysics3d::decimal PI = 3.14159265; // Pi constant
// Physics Engine constants
const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds
// GJK Algorithm parameters
const reactphysics3d::decimal OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm
// Contact constants
const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact
// Constraint solver constants
const int NB_MAX_BODIES = 100000; // Maximum number of bodies
const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation)
const int NB_MAX_CONSTRAINTS = 100000; // Maximum number of constraints
const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation)
// Constraint solver constants
const uint DEFAULT_LCP_ITERATIONS = 15; // Number of iterations when solving a LCP problem
const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; // Number of iterations when solving a LCP problem for error correction
const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; // True if the error correction projection (first order world) is active in the constraint solver
const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20; // Contacts with penetration depth (in meters) larger that this use error correction with projection
#endif

72
src/constants.h
View File

@ -1,72 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2012 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef CONSTANTS_H
#define CONSTANTS_H
// Libraries
#include <limits>
#include <cfloat>
// Type definitions
typedef unsigned int uint;
typedef long unsigned int luint;
// Mathematical constants
const double EPSILON = 1.0e-10; // Epsilon value
const double MACHINE_EPSILON = DBL_EPSILON; // Machine epsilon
const double EPSILON_TEST = 0.00001; // TODO : Try not to use this value
const double ONE_MINUS_EPSILON_TEST = 0.99999; // TODO : Try not to use this value
const double INFINITY_CONST = std::numeric_limits<double>::infinity(); // Infinity constant
const double PI = 3.14159265; // Pi constant
// Physics Engine constants
const double DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds
// GJK Algorithm parameters
const double OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm
// Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double DEFAULT_PENETRATION_FACTOR = 5.0; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact
const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact
const int NB_MAX_BODIES = 100000; // Maximum number of bodies
const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation)
const int NB_MAX_CONSTRAINTS = 100000; // Maximum number of constraints
const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation)
// Constraint solver constants
const uint DEFAULT_LCP_ITERATIONS = 15; // Number of iterations when solving a LCP problem
const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter
const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is
// less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
// is used to avoid to keep to much memory for a long time if the system doesn't
// need that memory. This value is between 0.0 and 1.0
#endif

4
src/constraint/Constraint.cpp
View File

@ -30,8 +30,8 @@
using namespace reactphysics3d;
// Constructor
Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active)
:body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) {
Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active, ConstraintType type)
:body1(body1), body2(body2), active(active), nbConstraints(nbConstraints), type(type) {
// Initialize the cached lambda values
for (int i=0; i<nbConstraints; i++) {

40
src/constraint/Constraint.h
View File

@ -32,6 +32,9 @@
// ReactPhysics3D namespace
namespace reactphysics3d {
// Enumeration for the type of a constraint
enum ConstraintType {CONTACT};
/* -------------------------------------------------------------------
Class Constraint :
@ -47,21 +50,24 @@ class Constraint {
Body* const body2; // Pointer to the second body of the constraint
bool active; // True if the constraint is active
uint nbConstraints; // Number mathematical constraints associated with this Constraint
std::vector<double> cachedLambdas; // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver
const ConstraintType type; // Type of the constraint
std::vector<decimal> cachedLambdas; // Cached lambda values of each mathematical constraint for more precise initializaton of LCP solver
public :
Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active); // Constructor // Constructor
virtual ~Constraint(); // Destructor
Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2
virtual void computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const=0; // Compute the error values for all the mathematical constraints
Constraint(Body* const body1, Body* const body2, uint nbConstraints,
bool active, ConstraintType type); // Constructor // Constructor
virtual ~Constraint(); // Destructor
Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2
ConstraintType getType() const; // Return the type of the constraint
virtual void computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0; // Compute the error values for all the mathematical constraints
unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints
double getCachedLambda(int index) const; // Get one cached lambda value
void setCachedLambda(int index, double lambda); // Set on cached lambda value
decimal getCachedLambda(int index) const; // Get one cached lambda value
void setCachedLambda(int index, decimal lambda); // Set on cached lambda value
};
// Return the reference to the body 1
@ -79,19 +85,25 @@ inline bool Constraint::isActive() const {
return active;
}
// Return the type of the constraint
inline ConstraintType Constraint::getType() const {
return type;
}
// Return the number auxiliary constraints
inline uint Constraint::getNbConstraints() const {
return nbConstraints;
}
// Get one previous lambda value
inline double Constraint::getCachedLambda(int index) const {
inline decimal Constraint::getCachedLambda(int index) const {
assert(index >= 0 && index < nbConstraints);
return cachedLambdas[index];
}
// Set on cached lambda value
inline void Constraint::setCachedLambda(int index, double lambda) {
inline void Constraint::setCachedLambda(int index, decimal lambda) {
assert(index >= 0 && index < nbConstraints);
cachedLambdas[index] = lambda;
}

20
src/constraint/Contact.cpp
View File

@ -31,7 +31,7 @@ using namespace std;
// Constructor
Contact::Contact(const ContactInfo* contactInfo)
: Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
: Constraint(contactInfo->body1, contactInfo->body2, 3, true, CONTACT), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
localPointOnBody1(contactInfo->localPoint1), localPointOnBody2(contactInfo->localPoint2),
worldPointOnBody1(contactInfo->worldPoint1), worldPointOnBody2(contactInfo->worldPoint2) {
assert(penetrationDepth > 0.0);
@ -55,7 +55,7 @@ Contact::~Contact() {
// fill in this matrix with all the jacobian matrix of the mathematical constraint
// of the contact. The argument "noConstraint", is the row were the method have
// to start to fill in the J_sp matrix.
void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
assert(body1);
assert(body2);
@ -128,7 +128,7 @@ void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][
// Compute the lowerbounds values for all the mathematical constraints. The
// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const {
void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint
@ -139,10 +139,10 @@ void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONS
// Compute the upperbounds values for all the mathematical constraints. The
// argument "upperBounds" is the upperbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const {
void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
upperBounds[noConstraint] = INFINITY_CONST; // Upper bound for the contact constraint
upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint
upperBounds[noConstraint + 1] = mu_mc_g; // Upper bound for the first friction constraint
upperBounds[noConstraint + 2] = mu_mc_g; // Upper bound for the second friction constraint
}
@ -150,11 +150,11 @@ void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONS
// Compute the error values for all the mathematical constraints. The argument
// "errorValues" is the error values vector of the constraint solver and this
// method has to fill in this vector starting from the row "noConstraint"
void Contact::computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const {
void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const {
assert(body1);
assert(body2);
// TODO : Do we need this casting anymore
// TODO : Do we need this casting anymore ?
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
@ -163,9 +163,9 @@ void Contact::computeErrorValue(int noConstraint, double errorValues[], double p
// Compute the error value for the contact constraint
Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3 velocity2 = rigidBody2->getLinearVelocity();
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + penetrationFactor * penetrationDepth;
decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
decimal errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2));
// Assign the error value to the vector of error values
errorValues[noConstraint] = errorValue; // Error value for contact constraint
errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint

25
src/constraint/Contact.h
View File

@ -30,11 +30,10 @@
#include "Constraint.h"
#include "../collision/ContactInfo.h"
#include "../body/RigidBody.h"
#include "../constants.h"
#include "../configuration.h"
#include "../mathematics/mathematics.h"
#include "../memory/MemoryPool.h"
#include "../configuration.h"
#include <new>
#if defined(VISUAL_DEBUG)
#if defined(APPLE_OS)
@ -65,13 +64,13 @@ namespace reactphysics3d {
class Contact : public Constraint {
protected :
const Vector3 normal; // Normal vector of the contact (From body1 toward body2) in world space
double penetrationDepth; // Penetration depth
decimal penetrationDepth; // Penetration depth
const Vector3 localPointOnBody1; // Contact point on body 1 in local space of body 1
const Vector3 localPointOnBody2; // Contact point on body 2 in local space of body 2
Vector3 worldPointOnBody1; // Contact point on body 1 in world space
Vector3 worldPointOnBody2; // Contact point on body 2 in world space
std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
double mu_mc_g;
decimal mu_mc_g;
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
@ -80,18 +79,18 @@ class Contact : public Constraint {
virtual ~Contact(); // Destructor
Vector3 getNormal() const; // Return the normal vector of the contact
void setPenetrationDepth(double penetrationDepth); // Set the penetration depth of the contact
void setPenetrationDepth(decimal penetrationDepth); // Set the penetration depth of the contact
Vector3 getLocalPointOnBody1() const; // Return the contact local point on body 1
Vector3 getLocalPointOnBody2() const; // Return the contact local point on body 2
Vector3 getWorldPointOnBody1() const; // Return the contact world point on body 1
Vector3 getWorldPointOnBody2() const; // Return the contact world point on body 2
void setWorldPointOnBody1(const Vector3& worldPoint); // Set the contact world point on body 1
void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2
virtual void computeJacobian(int noConstraint, double J_SP[NB_MAX_CONSTRAINTS][2*6]) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const; // Compute the error values for all the mathematical constraints
double getPenetrationDepth() const; // Return the penetration depth
virtual void computeJacobian(int noConstraint, decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const; // Compute the error values for all the mathematical constraints
decimal getPenetrationDepth() const; // Return the penetration depth
#ifdef VISUAL_DEBUG
void draw() const; // Draw the contact (for debugging)
#endif
@ -117,7 +116,7 @@ inline Vector3 Contact::getNormal() const {
}
// Set the penetration depth of the contact
inline void Contact::setPenetrationDepth(double penetrationDepth) {
inline void Contact::setPenetrationDepth(decimal penetrationDepth) {
this->penetrationDepth = penetrationDepth;
}
@ -152,7 +151,7 @@ inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) {
}
// Return the penetration depth of the contact
inline double Contact::getPenetrationDepth() const {
inline decimal Contact::getPenetrationDepth() const {
return penetrationDepth;
}
@ -162,7 +161,7 @@ inline void Contact::draw() const {
glColor3f(1.0, 0.0, 0.0);
glutSolidSphere(0.3, 20, 20);
}
#endif
#endif
} // End of the ReactPhysics3D namespace

378
src/engine/ConstraintSolver.cpp
View File

@ -34,7 +34,8 @@ using namespace std;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS),
penetrationFactor(DEFAULT_PENETRATION_FACTOR) {
nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION),
isErrorCorrectionActive(false) {
}
@ -46,8 +47,9 @@ ConstraintSolver::~ConstraintSolver() {
// Initialize the constraint solver before each solving
void ConstraintSolver::initialize() {
Constraint* constraint;
nbConstraints = 0;
nbConstraintsError = 0;
// For each constraint
vector<Constraint*>::iterator it;
@ -68,6 +70,11 @@ void ConstraintSolver::initialize() {
// Update the size of the jacobian matrix
nbConstraints += constraint->getNbConstraints();
// Update the size of the jacobian matrix for error correction projection
if (constraint->getType() == CONTACT) {
nbConstraintsError++;
}
}
}
@ -75,16 +82,19 @@ void ConstraintSolver::initialize() {
nbBodies = bodyNumberMapping.size();
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS);
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
}
// Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() {
void ConstraintSolver::fillInMatrices(decimal dt) {
Constraint* constraint;
decimal oneOverDt = 1.0 / dt;
// For each active constraint
int noConstraint = 0;
int noConstraintError = 0;
for (int c=0; c<activeConstraints.size(); c++) {
@ -104,13 +114,46 @@ void ConstraintSolver::fillInMatrices() {
constraint->computeUpperBound(noConstraint, upperBounds);
// Fill in the error vector
constraint->computeErrorValue(noConstraint, errorValues, penetrationFactor);
constraint->computeErrorValue(noConstraint, errorValues);
// Get the cached lambda values of the constraint
for (int i=0; i<constraint->getNbConstraints(); i++) {
lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
}
// If the constraint is a contact
if (constraint->getType() == CONTACT) {
Contact* contact = dynamic_cast<Contact*>(constraint);
decimal penetrationDepth = contact->getPenetrationDepth();
// If error correction with projection is active
if (isErrorCorrectionActive) {
// Fill in the error correction projection parameters
lowerBoundsError[noConstraintError] = lowerBounds[noConstraint];
upperBoundsError[noConstraintError] = upperBounds[noConstraint];
for (int i=0; i<12; i++) {
J_spError[noConstraintError][i] = J_sp[noConstraint][i];
}
bodyMappingError[noConstraintError][0] = constraint->getBody1();
bodyMappingError[noConstraintError][1] = constraint->getBody2();
penetrationDepths[noConstraintError] = contact->getPenetrationDepth();
// If the penetration depth is small
if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
// Use the Baumgarte error correction term for contacts instead of
// first order world projection
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
}
noConstraintError++;
}
else { // If error correction with projection is not active
// Add the Baumgarte error correction term for contacts
errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth();
}
}
noConstraint += constraint->getNbConstraints();
}
@ -129,21 +172,31 @@ void ConstraintSolver::fillInMatrices() {
// Compute the vector V1 with initial velocities values
Vector3 linearVelocity = rigidBody->getLinearVelocity();
Vector3 angularVelocity = rigidBody->getAngularVelocity();
int bodyIndexArray = 6 * bodyNumber;
V1[bodyIndexArray] = linearVelocity[0];
V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
int bodyIndexArray = 6 * bodyNumber;
V1[bodyIndexArray] = linearVelocity[0];
V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyIndexArray] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector Vconstraint with final constraint velocities
if (isErrorCorrectionActive) {
VconstraintError[bodyIndexArray] = 0.0;
VconstraintError[bodyIndexArray + 1] = 0.0;
VconstraintError[bodyIndexArray + 2] = 0.0;
VconstraintError[bodyIndexArray + 3] = 0.0;
VconstraintError[bodyIndexArray + 4] = 0.0;
VconstraintError[bodyIndexArray + 5] = 0.0;
}
// Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce();
@ -155,63 +208,78 @@ void ConstraintSolver::fillInMatrices() {
Fext[bodyIndexArray + 4] = externalTorque[1];
Fext[bodyIndexArray + 5] = externalTorque[2];
// Initialize the mass and inertia tensor matrices
// Initialize the mass and inertia tensor matrices
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
Minv_sp_mass_diag[bodyNumber] = 0.0;
// If the motion of the rigid body is enabled
if (rigidBody->getIsMotionEnabled()) {
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
Minv_sp_mass_diag[bodyNumber] = 0.0;
// If the motion of the rigid body is enabled
if (rigidBody->getIsMotionEnabled()) {
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
}
}
}
// Compute the vector b
void ConstraintSolver::computeVectorB(double dt) {
void ConstraintSolver::computeVectorB(decimal dt) {
uint indexBody1, indexBody2;
double oneOverDT = 1.0 / dt;
//b = errorValues * oneOverDT;
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<nbConstraints; c++) {
b[c] = errorValues[c] * oneOverDT;
// b = errorValues * oneOverDT;
b[c] = errorValues[c] * oneOverDT;
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
double multiplication = 0.0;
int body1ArrayIndex = 6 * indexBody1;
int body2ArrayIndex = 6 * indexBody2;
for (uint i=0; i<6; i++) {
multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
}
b[c] -= multiplication * oneOverDT ;
decimal multiplication = 0.0;
int body1ArrayIndex = 6 * indexBody1;
int body2ArrayIndex = 6 * indexBody2;
for (uint i=0; i<6; i++) {
multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
}
b[c] -= multiplication * oneOverDT ;
// Substract J*M^-1*F_ext to the vector b
double value1 = 0.0;
double value2 = 0.0;
double sum1, sum2;
value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
for (uint i=0; i<3; i++) {
sum1 = 0.0;
sum2 = 0.0;
for (uint j=0; j<3; j++) {
sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
}
value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
}
b[c] -= value1 + value2;
decimal value1 = 0.0;
decimal value2 = 0.0;
decimal sum1, sum2;
value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
for (uint i=0; i<3; i++) {
sum1 = 0.0;
sum2 = 0.0;
for (uint j=0; j<3; j++) {
sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
}
value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
}
b[c] -= value1 + value2;
}
}
// Compute the vector b for error correction projection
void ConstraintSolver::computeVectorBError(decimal dt) {
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<nbConstraintsError; c++) {
// b = errorValues * oneOverDT;
if (penetrationDepths[c] >= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) {
bError[c] = penetrationDepths[c] * oneOverDT;
}
else {
bError[c] = 0.0;
}
}
}
@ -222,24 +290,54 @@ void ConstraintSolver::computeMatrixB_sp() {
// For each constraint
for (uint c = 0; c<nbConstraints; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
for (uint i=0; i<3; i++) {
B_sp[0][indexConstraintArray + 3 + i] = 0.0;
B_sp[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
}
}
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
for (uint i=0; i<3; i++) {
B_sp[0][indexConstraintArray + 3 + i] = 0.0;
B_sp[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
}
}
}
}
// Same as B_sp matrix but for error correction projection and
// therefore, only for contact constraints
void ConstraintSolver::computeMatrixB_spErrorCorrection() {
uint indexConstraintArray, indexBody1, indexBody2;
// For each contact constraint
for (uint c = 0; c<nbConstraintsError; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMappingError[c][0]];
indexBody2 = bodyNumberMapping[bodyMappingError[c][1]];
B_spError[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_spError[c][0];
B_spError[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_spError[c][1];
B_spError[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_spError[c][2];
B_spError[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_spError[c][6];
B_spError[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_spError[c][7];
B_spError[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_spError[c][8];
for (uint i=0; i<3; i++) {
B_spError[0][indexConstraintArray + 3 + i] = 0.0;
B_spError[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_spError[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_spError[c][3 + j];
B_spError[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_spError[c][9 + j];
}
}
}
}
@ -249,8 +347,8 @@ void ConstraintSolver::computeMatrixB_sp() {
// Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B.
// This method is called after that the LCP solver has computed lambda
void ConstraintSolver::computeVectorVconstraint(double dt) {
uint indexBody1, indexBody2, indexBody1Array, indexBody2Array, indexConstraintArray;
void ConstraintSolver::computeVectorVconstraint(decimal dt) {
uint indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute dt * (M^-1 * J^T * lambda
@ -265,18 +363,35 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
}
}
// Same as computeVectorVconstraint() but for error correction projection
void ConstraintSolver::computeVectorVconstraintError(decimal dt) {
uint indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute M^-1 * J^T * lambda
for (uint i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
indexConstraintArray = 6 * i;
for (j=0; j<6; j++) {
VconstraintError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
VconstraintError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCP() {
for (uint i=0; i<nbConstraints; i++) {
lambda[i] = lambdaInit[i];
}
for (uint i=0; i<nbConstraints; i++) {
lambda[i] = lambdaInit[i];
}
uint indexBody1Array, indexBody2Array;
double deltaLambda;
double lambdaTemp;
decimal deltaLambda;
decimal lambdaTemp;
uint i, iter;
// Compute the vector a
@ -285,30 +400,78 @@ void ConstraintSolver::solveLCP() {
// For each constraint
for (i=0; i<nbConstraints; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
}
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCP; iter++) {
for (i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = b[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
}
deltaLambda /= d[i];
uint indexConstraintArray = 6 * i;
deltaLambda = b[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambda[i];
lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
deltaLambda = lambda[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
}
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
// Solve a LCP problem for error correction projection
// using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCPErrorCorrection() {
for (uint i=0; i<nbConstraintsError; i++) {
lambdaError[i] = 0;
}
uint indexBody1Array, indexBody2Array;
decimal deltaLambda;
decimal lambdaTemp;
uint i, iter;
// Compute the vector a
computeVectorAError();
// For each constraint
for (i=0; i<nbConstraintsError; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_spError[i][j] * B_spError[0][indexConstraintArray + j] + J_spError[i][6 + j] * B_spError[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCPErrorCorrection; iter++) {
for (i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = bError[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_spError[i][j] * aError[indexBody1Array + j] + J_spError[i][6 + j] * aError[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambdaError[i];
lambdaError[i] = std::max(lowerBoundsError[i], std::min(lambdaError[i] + deltaLambda, upperBoundsError[i]));
deltaLambda = lambdaError[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * deltaLambda;
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
@ -335,6 +498,27 @@ void ConstraintSolver::computeVectorA() {
}
}
// Same as computeVectorA() but for error correction projection
void ConstraintSolver::computeVectorAError() {
uint i;
uint indexBody1Array, indexBody2Array;
// Init the vector a with zero values
for (i=0; i<6*nbBodies; i++) {
aError[i] = 0.0;
}
for(i=0; i<nbConstraintsError; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMappingError[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMappingError[i][1]];
uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) {
aError[indexBody1Array + j] += B_spError[0][indexConstraintArray + j] * lambdaError[i];
aError[indexBody2Array + j] += B_spError[1][indexConstraintArray + j] * lambdaError[i];
}
}
}
// Cache the lambda values in order to reuse them in the next step
// to initialize the lambda vector

143
src/engine/ConstraintSolver.h
View File

@ -27,7 +27,7 @@
#define CONSTRAINT_SOLVER_H
// Libraries
#include "../constants.h"
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include "PhysicsWorld.h"
#include <map>
@ -44,63 +44,99 @@ namespace reactphysics3d {
Temporal Coherence" from Erin Catto. We keep the same notations as
in the paper. The idea is to construct a LCP problem and then solve
it using a Projected Gauss Seidel (PGS) solver.
The idea is to solve the following system for lambda :
JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0
By default, error correction using first world order projections (described
by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and
Versatile Multibody Dynamics Animation") is used for very large penetration
depths. Error correction with projection requires to solve another LCP problem
that is simpler than the above one and by considering only the contact
constraints. The LCP problem for error correction is the following one :
J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0
where "d" is a vector with penetration depths. If the penetration depth of
a given contact is not very large, we use the Baumgarte error correction (see
the paper from Erin Catto).
-------------------------------------------------------------------
*/
class ConstraintSolver {
private:
PhysicsWorld* physicsWorld; // Reference to the physics world
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
bool isErrorCorrectionActive; // True if error correction (with world order) is active
uint nbIterationsLCP; // Number of iterations of the LCP solver
uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints)
uint nbBodies; // Current number of bodies in the physics world
double penetrationFactor; // Penetration factor "beta" for penetration correction
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
double J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
// a 1x6 matrix
double B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
// a 6x1 matrix
double b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
double d[NB_MAX_CONSTRAINTS]; // Vector "d"
double a[6*NB_MAX_BODIES]; // Vector "a"
double lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
double lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
double errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
double lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
double upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
decimal J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints)
decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints)
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
decimal a[6*NB_MAX_BODIES]; // Vector "a"
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections
decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections
decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
double Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
double V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
// Each cell contains a 6x1 vector with linear and angular velocities
double Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
double Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque.
void initialize(); // Initialize the constraint solver before each solving
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(double dt); // Compute the vector b
void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(decimal dt); // Compute the vector b
void computeVectorBError(decimal dt); // Compute the vector b for error correction projection
void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorVconstraint(double dt); // Compute the vector V2
void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection
void computeVectorVconstraint(decimal dt); // Compute the vector V2
void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void computeVectorA(); // Compute the vector a used in the solve() method
void computeVectorAError(); // Same as computeVectorA() but for error correction projection
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
public:
ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
void cleanup(); // Cleanup of the constraint solver
void setPenetrationFactor(double penetrationFactor); // Set the penetration factor
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection
public:
ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(decimal dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction
Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction
void cleanup(); // Cleanup of the constraint solver
void setPenetrationFactor(decimal penetrationFactor); // Set the penetration factor
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
};
// Return true if the body is in at least one constraint
@ -118,7 +154,6 @@ inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body)
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
}
// Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
@ -126,6 +161,20 @@ inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body)
return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
}
// Return the constrained linear velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]);
}
// Return the constrained angular velocity of a body after solving the LCP problem for error correction
inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) {
//assert(isConstrainedBody(body));
uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]);
}
// Cleanup of the constraint solver
inline void ConstraintSolver::cleanup() {
bodyNumberMapping.clear();
@ -133,39 +182,53 @@ inline void ConstraintSolver::cleanup() {
activeConstraints.clear();
}
// Set the penetration factor
inline void ConstraintSolver::setPenetrationFactor(double factor) {
penetrationFactor = factor;
}
// Set the number of iterations of the LCP solver
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
nbIterationsLCP = nbIterations;
}
}
// Set the isErrorCorrectionActive value
inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
this->isErrorCorrectionActive = isErrorCorrectionActive;
}
// Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) {
inline void ConstraintSolver::solve(decimal dt) {
// Allocate memory for the matrices
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices();
fillInMatrices(dt);
// Compute the vector b
computeVectorB(dt);
if (isErrorCorrectionActive) {
computeVectorBError(dt);
}
// Compute the matrix B
computeMatrixB_sp();
if (isErrorCorrectionActive) {
computeMatrixB_spErrorCorrection();
}
// Solve the LCP problem (computation of lambda)
solveLCP();
if (isErrorCorrectionActive) {
solveLCPErrorCorrection();
}
// Cache the lambda values in order to use them in the next step
cacheLambda();
// Compute the vector Vconstraint
computeVectorVconstraint(dt);
if (isErrorCorrectionActive) {
computeVectorVconstraintError(dt);
}
}
} // End of ReactPhysics3D namespace

38
src/engine/PhysicsEngine.cpp
View File

@ -31,7 +31,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_TIMESTEP)
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, decimal timeStep = DEFAULT_TIMESTEP)
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) {
assert(world);
assert(timeStep > 0.0);
@ -57,7 +57,6 @@ void PhysicsEngine::update() {
// While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) {
existCollision = false;
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
@ -96,10 +95,12 @@ void PhysicsEngine::update() {
// This method uses the semi-implicit Euler method to update the position and
// orientation of the body
void PhysicsEngine::updateAllBodiesMotion() {
double dt = timer.getTimeStep();
decimal dt = timer.getTimeStep();
Vector3 newLinearVelocity;
Vector3 newAngularVelocity;
Vector3 linearVelocityErrorCorrection;
Vector3 angularVelocityErrorCorrection;
// For each body of thephysics world
for (vector<RigidBody*>::iterator it=world->getRigidBodiesBeginIterator(); it != world->getRigidBodiesEndIterator(); ++it) {
@ -110,12 +111,17 @@ void PhysicsEngine::updateAllBodiesMotion() {
if (rigidBody->getIsMotionEnabled()) {
newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body
if (constraintSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it);
newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it);
linearVelocityErrorCorrection = constraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody);
angularVelocityErrorCorrection = constraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody);
}
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
@ -126,9 +132,10 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Add the velocity V1 to the new velocity
newLinearVelocity += rigidBody->getLinearVelocity();
newAngularVelocity += rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity,
linearVelocityErrorCorrection, angularVelocityErrorCorrection);
// Update the AABB of the rigid body
rigidBody->updateAABB();
@ -139,8 +146,9 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body
void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity) {
double dt = timer.getTimeStep();
void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) {
decimal dt = timer.getTimeStep();
assert(rigidBody);
@ -150,21 +158,27 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Get current position and orientation of the body
const Vector3& currentPosition = rigidBody->getTransform().getPosition();
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
// Error correction projection
Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection;
Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt;
Vector3 newPosition = correctedPosition + newLinVelocity * dt;
Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt;
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
}
// Compute and set the interpolation factor to all bodies
void PhysicsEngine::setInterpolationFactorToAllBodies() {
// Compute the interpolation factor
double factor = timer.computeInterpolationFactor();
decimal factor = timer.computeInterpolationFactor();
assert(factor >= 0.0 && factor <= 1.0);
// Set the factor to all bodies

35
src/engine/PhysicsEngine.h
View File

@ -32,7 +32,7 @@
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../constants.h"
#include "../configuration.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -50,20 +50,23 @@ class PhysicsEngine {
CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity); // Update the position and orientation of a body
void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies
void applyGravity(); // Apply the gravity force to all bodies
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity,
const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection); // Update the position and orientation of a body
void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies
void applyGravity(); // Apply the gravity force to all bodies
public :
PhysicsEngine(PhysicsWorld* world, double timeStep); // Constructor
PhysicsEngine(PhysicsWorld* world, decimal timeStep); // Constructor
~PhysicsEngine(); // Destructor
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update(); // Update the physics simulation
void setPenetrationFactor(double factor); // Set the penetration factor for the constraint solver
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update(); // Update the physics simulation
void setPenetrationFactor(decimal factor); // Set the penetration factor for the constraint solver
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value
};
// --- Inline functions --- //
@ -78,14 +81,20 @@ inline void PhysicsEngine::stop() {
}
// Set the penetration factor for the constraint solver
inline void PhysicsEngine::setPenetrationFactor(double factor) {
inline void PhysicsEngine::setPenetrationFactor(decimal factor) {
constraintSolver.setPenetrationFactor(factor);
}
// Set the number of iterations of the LCP solver
inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) {
constraintSolver.setNbLCPIterations(nbIterations);
}
}
// Set the isErrorCorrectionActive value
inline void PhysicsEngine::setIsErrorCorrectionActive(bool isErrorCorrectionActive) {
constraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive);
}
}

14
src/engine/PhysicsWorld.cpp
View File

@ -44,10 +44,10 @@ PhysicsWorld::~PhysicsWorld() {
}
// Create a rigid body into the physics world
RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) {
RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, Collider* collider) {
// Create the rigid body
RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, shape, currentBodyID);
RigidBody* rigidBody = new (memoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collider, currentBodyID);
currentBodyID++;
@ -60,11 +60,11 @@ RigidBody* PhysicsWorld::createRigidBody(const Transform& transform, double mass
void PhysicsWorld::destroyRigidBody(RigidBody* rigidBody) {
removeRigidBody(rigidBody);
// Call the constructor of the rigid body
rigidBody->RigidBody::~RigidBody();
// Free the object from the memory pool
memoryPoolRigidBodies.freeObject(rigidBody);
// Call the constructor of the rigid body
rigidBody->RigidBody::~RigidBody();
// Free the object from the memory pool
memoryPoolRigidBodies.freeObject(rigidBody);
}
// Remove all collision contacts constraints

4
src/engine/PhysicsWorld.h
View File

@ -64,8 +64,8 @@ class PhysicsWorld {
PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor
RigidBody* createRigidBody(const Transform& transform, double mass,
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Create a rigid body into the physics world
RigidBody* createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal, Collider* collider); // Create a rigid body into the physics world
void destroyRigidBody(RigidBody* rigidBody); // Destroy a rigid body
void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
Vector3 getGravity() const; // Return the gravity vector of the world

8
src/mathematics/Matrix3x3.cpp
View File

@ -37,12 +37,12 @@ Matrix3x3::Matrix3x3() {
}
// Constructor
Matrix3x3::Matrix3x3(double value) {
Matrix3x3::Matrix3x3(decimal value) {
setAllValues(value, value, value, value, value, value, value, value, value);
}
// Constructor with arguments
Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) {
Matrix3x3::Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3, decimal c1, decimal c2, decimal c3) {
// Initialize the matrix with the values
setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
@ -55,11 +55,11 @@ Matrix3x3::~Matrix3x3() {
// Return the inverse matrix
Matrix3x3 Matrix3x3::getInverse() const {
// Compute the determinant of the matrix
double determinant = getDeterminant();
decimal determinant = getDeterminant();
// Check if the determinant is equal to zero
assert(determinant != 0.0);
double invDeterminant = 1.0 / determinant;
decimal invDeterminant = 1.0 / determinant;
Matrix3x3 tempMatrix;
// Compute the inverse of the matrix

44
src/mathematics/Matrix3x3.h
View File

@ -42,23 +42,23 @@ namespace reactphysics3d {
*/
class Matrix3x3 {
private :
double array[3][3]; // Array with the values of the matrix
decimal array[3][3]; // Array with the values of the matrix
public :
Matrix3x3(); // Constructor
Matrix3x3(double value); // Constructor
Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Constructor
Matrix3x3(decimal value); // Constructor
Matrix3x3(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
decimal c1, decimal c2, decimal c3); // Constructor
virtual ~Matrix3x3(); // Destructor
double getValue(int i, int j) const; // Get a value in the matrix
void setValue(int i, int j, double value); // Set a value in the matrix
void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Set all the values in the matrix
decimal getValue(int i, int j) const; // Get a value in the matrix
void setValue(int i, int j, decimal value); // Set a value in the matrix
void setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
decimal c1, decimal c2, decimal c3); // Set all the values in the matrix
Vector3 getColumn(int i) const; // Return a column
Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix
decimal getDeterminant() const; // Return the determinant of the matrix
decimal getTrace() const; // Return the trace of the matrix
Matrix3x3 getInverse() const; // Return the inverse matrix
Matrix3x3 getAbsoluteMatrix() const; // Return the matrix with absolute values
void setToIdentity(); // Set the matrix to the identity matrix
@ -68,8 +68,8 @@ class Matrix3x3 {
friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for addition
friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for substraction
friend Matrix3x3 operator-(const Matrix3x3& matrix); // Overloaded operator for the negative of the matrix
friend Matrix3x3 operator*(double nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number
friend Matrix3x3 operator*(const Matrix3x3& matrix, double nb); // Overloaded operator for multiplication with a matrix
friend Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number
friend Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb); // Overloaded operator for multiplication with a matrix
friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for matrix multiplication
friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); // Overloaded operator for multiplication with a vector
@ -77,25 +77,25 @@ class Matrix3x3 {
bool operator!= (const Matrix3x3& matrix) const; // Overloaded operator for the is different condition
Matrix3x3& operator+=(const Matrix3x3& matrix); // Overloaded operator for addition with assignment
Matrix3x3& operator-=(const Matrix3x3& matrix); // Overloaded operator for substraction with assignment
Matrix3x3& operator*=(double nb); // Overloaded operator for multiplication with a number with assignment
Matrix3x3& operator*=(decimal nb); // Overloaded operator for multiplication with a number with assignment
};
// Method to get a value in the matrix (inline)
inline double Matrix3x3::getValue(int i, int j) const {
inline decimal Matrix3x3::getValue(int i, int j) const {
assert(i>=0 && i<3 && j>=0 && j<3);
return array[i][j];
}
// Method to set a value in the matrix (inline)
inline void Matrix3x3::setValue(int i, int j, double value) {
inline void Matrix3x3::setValue(int i, int j, decimal value) {
assert(i>=0 && i<3 && j>=0 && j<3);
array[i][j] = value;
}
// Method to set all the values in the matrix
inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3) {
inline void Matrix3x3::setAllValues(decimal a1, decimal a2, decimal a3, decimal b1, decimal b2, decimal b3,
decimal c1, decimal c2, decimal c3) {
array[0][0] = a1; array[0][1] = a2; array[0][2] = a3;
array[1][0] = b1; array[1][1] = b2; array[1][2] = b3;
array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
@ -116,14 +116,14 @@ inline Matrix3x3 Matrix3x3::getTranspose() const {
}
// Return the determinant of the matrix
inline double Matrix3x3::getDeterminant() const {
inline decimal Matrix3x3::getDeterminant() const {
// Compute and return the determinant of the matrix
return (array[0][0]*(array[1][1]*array[2][2]-array[2][1]*array[1][2]) - array[0][1]*(array[1][0]*array[2][2]-array[2][0]*array[1][2]) +
array[0][2]*(array[1][0]*array[2][1]-array[2][0]*array[1][1]));
}
// Return the trace of the matrix
inline double Matrix3x3::getTrace() const {
inline decimal Matrix3x3::getTrace() const {
// Compute and return the trace
return (array[0][0] + array[1][1] + array[2][2]);
}
@ -170,14 +170,14 @@ inline Matrix3x3 operator-(const Matrix3x3& matrix) {
}
// Overloaded operator for multiplication with a number
inline Matrix3x3 operator*(double nb, const Matrix3x3& matrix) {
inline Matrix3x3 operator*(decimal nb, const Matrix3x3& matrix) {
return Matrix3x3(matrix.array[0][0] * nb, matrix.array[0][1] * nb, matrix.array[0][2] * nb,
matrix.array[1][0] * nb, matrix.array[1][1] * nb, matrix.array[1][2] * nb,
matrix.array[2][0] * nb, matrix.array[2][1] * nb, matrix.array[2][2] * nb);
}
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 operator*(const Matrix3x3& matrix, double nb) {
inline Matrix3x3 operator*(const Matrix3x3& matrix, decimal nb) {
return nb * matrix;
}
@ -230,7 +230,7 @@ inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) {
}
// Overloaded operator for multiplication with a number with assignment
inline Matrix3x3& Matrix3x3::operator*=(double nb) {
inline Matrix3x3& Matrix3x3::operator*=(decimal nb) {
array[0][0] *= nb; array[0][1] *= nb; array[0][2] *= nb;
array[1][0] *= nb; array[1][1] *= nb; array[1][2] *= nb;
array[2][0] *= nb; array[2][1] *= nb; array[2][2] *= nb;

59
src/mathematics/Quaternion.cpp
View File

@ -38,13 +38,13 @@ Quaternion::Quaternion()
}
// Constructor with arguments
Quaternion::Quaternion(double x, double y, double z, double w)
Quaternion::Quaternion(decimal x, decimal y, decimal z, decimal w)
:x(x), y(y), z(z), w(w) {
}
// Constructor with the component w and the vector v=(x y z)
Quaternion::Quaternion(double w, const Vector3& v)
Quaternion::Quaternion(decimal w, const Vector3& v)
:x(v.getX()), y(v.getY()), z(v.getZ()), w(w) {
}
@ -59,17 +59,17 @@ Quaternion::Quaternion(const Quaternion& quaternion)
Quaternion::Quaternion(const Matrix3x3& matrix) {
// Get the trace of the matrix
double trace = matrix.getTrace();
decimal trace = matrix.getTrace();
double array[3][3];
decimal array[3][3];
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
array[i][j] = matrix.getValue(i, j);
}
}
double r;
double s;
decimal r;
decimal s;
if (trace < 0.0) {
if (array[1][1] > array[0][0]) {
@ -135,7 +135,7 @@ Quaternion::~Quaternion() {
// Compute the rotation angle (in radians) and the 3D rotation axis
// This method is used to get the rotation angle (in radian) and the unit
// rotation axis of an orientation quaternion.
void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const {
void Quaternion::getRotationAngleAxis(decimal& angle, Vector3& axis) const {
Quaternion quaternion;
// If the quaternion is unit
@ -163,26 +163,26 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const {
// Return the orientation matrix corresponding to this quaternion
Matrix3x3 Quaternion::getMatrix() const {
double nQ = x*x + y*y + z*z + w*w;
double s = 0.0;
decimal nQ = x*x + y*y + z*z + w*w;
decimal s = 0.0;
if (nQ > 0.0) {
s = 2.0/nQ;
}
// Computations used for optimization (less multiplications)
double xs = x*s;
double ys = y*s;
double zs = z*s;
double wxs = w*xs;
double wys = w*ys;
double wzs = w*zs;
double xxs = x*xs;
double xys = x*ys;
double xzs = x*zs;
double yys = y*ys;
double yzs = y*zs;
double zzs = z*zs;
decimal xs = x*s;
decimal ys = y*s;
decimal zs = z*s;
decimal wxs = w*xs;
decimal wys = w*ys;
decimal wzs = w*zs;
decimal xxs = x*xs;
decimal xys = x*ys;
decimal xzs = x*zs;
decimal yys = y*ys;
decimal yzs = y*zs;
decimal zzs = z*zs;
// Create the matrix corresponding to the quaternion
return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
@ -192,13 +192,13 @@ Matrix3x3 Quaternion::getMatrix() const {
// Compute the spherical linear interpolation between two quaternions.
// The t argument has to be such that 0 <= t <= 1. This method is static.
Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, decimal t) {
assert(t >= 0.0 && t <= 1.0);
double invert = 1.0;
decimal invert = 1.0;
// Compute cos(theta) using the quaternion scalar product
double cosineTheta = quaternion1.dot(quaternion2);
decimal cosineTheta = quaternion1.dot(quaternion2);
// Take care of the sign of cosineTheta
if (cosineTheta < 0.0) {
@ -208,19 +208,20 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
// Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write
// sin((1-t)*theta) as (1-t) and sin(t*theta) as t
if(1-cosineTheta < EPSILON_TEST) {
const decimal epsilon = 0.00001;
if(1-cosineTheta < epsilon) {
return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
}
// Compute the theta angle
double theta = acos(cosineTheta);
decimal theta = acos(cosineTheta);
// Compute sin(theta)
double sineTheta = sin(theta);
decimal sineTheta = sin(theta);
// Compute the two coefficients that are in the spherical linear interpolation formula
double coeff1 = sin((1.0-t)*theta) / sineTheta;
double coeff2 = sin(t*theta) / sineTheta * invert;
decimal coeff1 = sin((1.0-t)*theta) / sineTheta;
decimal coeff2 = sin(t*theta) / sineTheta * invert;
// Compute and return the interpolated quaternion
return quaternion1 * coeff1 + quaternion2 * coeff2;

95
src/mathematics/Quaternion.h
View File

@ -30,6 +30,7 @@
#include <cmath>
#include "Vector3.h"
#include "Matrix3x3.h"
#include "../decimal.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -42,86 +43,86 @@ namespace reactphysics3d {
*/
class Quaternion {
private :
double x; // Component x of the quaternion
double y; // Component y of the quaternion
double z; // Component z of the quaternion
double w; // Component w of the quaternion
decimal x; // Component x of the quaternion
decimal y; // Component y of the quaternion
decimal z; // Component z of the quaternion
decimal w; // Component w of the quaternion
public :
Quaternion(); // Constructor
Quaternion(double x, double y, double z, double w); // Constructor with arguments
Quaternion(double w, const Vector3& v); // Constructor with the component w and the vector v=(x y z)
Quaternion(const Quaternion& quaternion); // Copy-constructor
Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor
double getX() const; // Return the component x of the quaternion
double getY() const; // Return the component y of the quaternion
double getZ() const; // Return the component z of the quaternion
double getW() const; // Return the component w of the quaternion
void setX(double x); // Set the value x
void setY(double y); // Set the value y
void setZ(double z); // Set the value z
void setW(double w); // Set the value w
Vector3 vectorV() const; // Return the vector v=(x y z) of the quaternion
double length() const; // Return the length of the quaternion
Quaternion getUnit() const; // Return the unit quaternion
Quaternion getConjugate() const; // Return the conjugate quaternion
Quaternion getInverse() const; // Return the inverse of the quaternion
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
static Quaternion identity(); // Return the identity quaternion
double dot(const Quaternion& quaternion) const; // Dot product between two quaternions
void getRotationAngleAxis(double& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis
Quaternion(); // Constructor
Quaternion(decimal x, decimal y, decimal z, decimal w); // Constructor with arguments
Quaternion(decimal w, const Vector3& v); // Constructor with the component w and the vector v=(x y z)
Quaternion(const Quaternion& quaternion); // Copy-constructor
Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor
decimal getX() const; // Return the component x of the quaternion
decimal getY() const; // Return the component y of the quaternion
decimal getZ() const; // Return the component z of the quaternion
decimal getW() const; // Return the component w of the quaternion
void setX(decimal x); // Set the value x
void setY(decimal y); // Set the value y
void setZ(decimal z); // Set the value z
void setW(decimal w); // Set the value w
Vector3 vectorV() const; // Return the vector v=(x y z) of the quaternion
decimal length() const; // Return the length of the quaternion
Quaternion getUnit() const; // Return the unit quaternion
Quaternion getConjugate() const; // Return the conjugate quaternion
Quaternion getInverse() const; // Return the inverse of the quaternion
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
static Quaternion identity(); // Return the identity quaternion
decimal dot(const Quaternion& quaternion) const; // Dot product between two quaternions
void getRotationAngleAxis(decimal& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis
static Quaternion slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions
const Quaternion& quaternion2, decimal t); // Compute the spherical linear interpolation between two quaternions
// --- Overloaded operators --- //
Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition
Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction
Quaternion operator*(double nb) const; // Overloaded operator for the multiplication with a constant
Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication
Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment
bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition
Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition
Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction
Quaternion operator*(decimal nb) const; // Overloaded operator for the multiplication with a constant
Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication
Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment
bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition
};
// --- Inline functions --- //
// Get the value x (inline)
inline double Quaternion::getX() const {
inline decimal Quaternion::getX() const {
return x;
}
// Get the value y (inline)
inline double Quaternion::getY() const {
inline decimal Quaternion::getY() const {
return y;
}
// Get the value z (inline)
inline double Quaternion::getZ() const {
inline decimal Quaternion::getZ() const {
return z;
}
// Get the value w (inline)
inline double Quaternion::getW() const {
inline decimal Quaternion::getW() const {
return w;
}
// Set the value x (inline)
inline void Quaternion::setX(double x) {
inline void Quaternion::setX(decimal x) {
this->x = x;
}
// Set the value y (inline)
inline void Quaternion::setY(double y) {
inline void Quaternion::setY(decimal y) {
this->y = y;
}
// Set the value z (inline)
inline void Quaternion::setZ(double z) {
inline void Quaternion::setZ(decimal z) {
this->z = z;
}
// Set the value w (inline)
inline void Quaternion::setW(double w) {
inline void Quaternion::setW(decimal w) {
this->w = w;
}
@ -132,13 +133,13 @@ inline Vector3 Quaternion::vectorV() const {
}
// Return the length of the quaternion (inline)
inline double Quaternion::length() const {
inline decimal Quaternion::length() const {
return sqrt(x*x + y*y + z*z + w*w);
}
// Return the unit quaternion
inline Quaternion Quaternion::getUnit() const {
double lengthQuaternion = length();
decimal lengthQuaternion = length();
// Check if the length is not equal to zero
assert (lengthQuaternion != 0.0);
@ -159,7 +160,7 @@ inline Quaternion Quaternion::getConjugate() const {
// Return the inverse of the quaternion (inline)
inline Quaternion Quaternion::getInverse() const {
double lengthQuaternion = length();
decimal lengthQuaternion = length();
lengthQuaternion = lengthQuaternion * lengthQuaternion;
assert (lengthQuaternion != 0.0);
@ -169,7 +170,7 @@ inline Quaternion Quaternion::getInverse() const {
}
// Scalar product between two quaternions
inline double Quaternion::dot(const Quaternion& quaternion) const {
inline decimal Quaternion::dot(const Quaternion& quaternion) const {
return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
}
@ -186,7 +187,7 @@ inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
}
// Overloaded operator for the multiplication with a constant
inline Quaternion Quaternion::operator*(double nb) const {
inline Quaternion Quaternion::operator*(decimal nb) const {
// Return the result
return Quaternion(nb*x, nb*y, nb*z, nb*w);
}

20
src/mathematics/Transform.h
View File

@ -46,22 +46,22 @@ class Transform {
Quaternion orientation; // Orientation
public :
Transform(); // Constructor
Transform(); // Constructor
Transform(const Vector3& position, const Matrix3x3& orientation); // Constructor
Transform(const Vector3& position, const Quaternion& orientation); // Constructor
~Transform(); // Destructor
~Transform(); // Destructor
const Vector3& getPosition() const; // Return the origin of the transform
void setPosition(const Vector3& position); // Set the origin of the transform
const Vector3& getPosition() const; // Return the origin of the transform
void setPosition(const Vector3& position); // Set the origin of the transform
const Quaternion& getOrientation() const; // Return the orientation quaternion
void setOrientation(const Quaternion& orientation); // Set the rotation quaternion
void setToIdentity(); // Set the transform to the identity transform
void setFromOpenGL(double* openglMatrix); // Set the transform from an OpenGL transform matrix
void getOpenGLMatrix(double* openglMatrix) const; // Get the OpenGL matrix of the transform
void setFromOpenGL(decimal* openglMatrix); // Set the transform from an OpenGL transform matrix
void getOpenGLMatrix(decimal* openglMatrix) const; // Get the OpenGL matrix of the transform
Transform inverse() const; // Return the inverse of the transform
static Transform interpolateTransforms(const Transform& oldTransform,
const Transform& newTransform,
double interpolationFactor); // Return an interpolated transform
decimal interpolationFactor); // Return an interpolated transform
Vector3 operator*(const Vector3& vector) const; // Return the transformed vector
Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one
@ -94,7 +94,7 @@ inline void Transform::setToIdentity() {
}
// Set the transform from an OpenGL transform matrix
inline void Transform::setFromOpenGL(double* openglMatrix) {
inline void Transform::setFromOpenGL(decimal* openglMatrix) {
Matrix3x3 matrix(openglMatrix[0], openglMatrix[4], openglMatrix[8],
openglMatrix[1], openglMatrix[5], openglMatrix[9],
openglMatrix[2], openglMatrix[6], openglMatrix[10]);
@ -103,7 +103,7 @@ inline void Transform::setFromOpenGL(double* openglMatrix) {
}
// Get the OpenGL matrix of the transform
inline void Transform::getOpenGLMatrix(double* openglMatrix) const {
inline void Transform::getOpenGLMatrix(decimal* openglMatrix) const {
const Matrix3x3& matrix = orientation.getMatrix();
openglMatrix[0] = matrix.getValue(0, 0); openglMatrix[1] = matrix.getValue(1, 0); openglMatrix[2] = matrix.getValue(2, 0); openglMatrix[3] = 0.0;
openglMatrix[4] = matrix.getValue(0, 1); openglMatrix[5] = matrix.getValue(1, 1); openglMatrix[6] = matrix.getValue(2, 1); openglMatrix[7] = 0.0;
@ -119,7 +119,7 @@ inline Transform Transform::inverse() const {
}
// Return an interpolated transform
inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, double interpolationFactor) {
inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, decimal interpolationFactor) {
Vector3 interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor;
Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
return Transform(interPosition, interOrientation);

8
src/mathematics/Vector3.cpp
View File

@ -40,7 +40,7 @@ Vector3::Vector3() {
}
// Constructor with arguments
Vector3::Vector3(double x, double y, double z) {
Vector3::Vector3(decimal x, decimal y, decimal z) {
values[0] = x;
values[1] = y;
values[2] = z;
@ -60,12 +60,12 @@ Vector3::~Vector3() {
// Return the corresponding unit vector
Vector3 Vector3::getUnit() const {
double lengthVector = length();
decimal lengthVector = length();
assert(lengthVector != 0.0);
// Compute and return the unit vector
double lengthInv = 1.0 / lengthVector;
decimal lengthInv = 1.0 / lengthVector;
return Vector3(values[0] * lengthInv, values[1] * lengthInv, values[2] * lengthInv);
}
@ -91,6 +91,6 @@ Vector3 Vector3::getOneOrthogonalVector() const {
vector1.setZ((-values[0]*values[2]-values[1]*vector1.getY())/values[2]);
}
assert(vector1.isUnit());
//assert(vector1.isUnit());
return vector1;
}

69
src/mathematics/Vector3.h
View File

@ -29,6 +29,7 @@
// Libraries
#include <cmath>
#include "mathematics_functions.h"
#include "../decimal.h"
// ReactPhysics3D namespace
@ -36,32 +37,32 @@ namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Vector3 :
This classrepresents 3 dimensionnal vector in space.
This class represents 3D vector in space.
-------------------------------------------------------------------
*/
class Vector3 {
private :
double values[3]; // Values of the 3D vector
decimal values[3]; // Values of the 3D vector
public :
Vector3(); // Constructor of the class Vector3D
Vector3(double x, double y, double z); // Constructor with arguments
Vector3(decimal x, decimal y, decimal z); // Constructor with arguments
Vector3(const Vector3& vector); // Copy-constructor
virtual ~Vector3(); // Destructor
double getX() const; // Get the x component of the vector
double getY() const; // Get the y component of the vector
double getZ() const; // Get the z component of the vector
void setX(double x); // Set the x component of the vector
void setY(double y); // Set the y component of the vector
void setZ(double z); // Set the z component of the vector
void setAllValues(double x, double y, double z); // Set all the values of the vector
double length() const; // Return the lenght of the vector
double lengthSquare() const; // Return the square of the length of the vector
decimal getX() const; // Get the x component of the vector
decimal getY() const; // Get the y component of the vector
decimal getZ() const; // Get the z component of the vector
void setX(decimal x); // Set the x component of the vector
void setY(decimal y); // Set the y component of the vector
void setZ(decimal z); // Set the z component of the vector
void setAllValues(decimal x, decimal y, decimal z); // Set all the values of the vector
decimal length() const; // Return the lenght of the vector
decimal lengthSquare() const; // Return the square of the length of the vector
Vector3 getUnit() const; // Return the corresponding unit vector
bool isUnit() const; // Return true if the vector is unit and false otherwise
bool isZero() const; // Return true if the current vector is the zero vector
Vector3 getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector
double dot(const Vector3& vector) const; // Dot product of two vectors
decimal dot(const Vector3& vector) const; // Dot product of two vectors
Vector3 cross(const Vector3& vector) const; // Cross product of two vectors
Vector3 getAbsoluteVector() const; // Return the corresponding absolute value vector
int getMinAxis() const; // Return the axis with the minimal value
@ -73,68 +74,68 @@ class Vector3 {
bool operator!= (const Vector3& vector) const; // Overloaded operator for the is different condition
Vector3& operator+=(const Vector3& vector); // Overloaded operator for addition with assignment
Vector3& operator-=(const Vector3& vector); // Overloaded operator for substraction with assignment
Vector3& operator*=(double number); // Overloaded operator for multiplication with a number with assignment
double& operator[] (int index); // Overloaded operator for value access
const double& operator[] (int index) const; // Overloaded operator for value access
Vector3& operator*=(decimal number); // Overloaded operator for multiplication with a number with assignment
decimal& operator[] (int index); // Overloaded operator for value access
const decimal& operator[] (int index) const; // Overloaded operator for value access
// Friend functions
friend Vector3 operator+(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator-(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator-(const Vector3& vector);
friend Vector3 operator*(const Vector3& vector, double number);
friend Vector3 operator*(double number, const Vector3& vector);
friend Vector3 operator*(const Vector3& vector, decimal number);
friend Vector3 operator*(decimal number, const Vector3& vector);
};
// Get the x component of the vector
inline double Vector3::getX() const {
inline decimal Vector3::getX() const {
return values[0];
}
// Get the y component of the vector
inline double Vector3::getY() const {
inline decimal Vector3::getY() const {
return values[1];
}
// Get the z component of the vector
inline double Vector3::getZ() const {
inline decimal Vector3::getZ() const {
return values[2];
}
// Set the x component of the vector
inline void Vector3::setX(double x) {
inline void Vector3::setX(decimal x) {
this->values[0] = x;
}
// Set the y component of the vector
inline void Vector3::setY(double y) {
inline void Vector3::setY(decimal y) {
this->values[1] = y;
}
// Set the z component of the vector
inline void Vector3::setZ(double z) {
inline void Vector3::setZ(decimal z) {
this->values[2] = z;
}
// Set all the values of the vector (inline)
inline void Vector3::setAllValues(double x, double y, double z) {
inline void Vector3::setAllValues(decimal x, decimal y, decimal z) {
values[0]= x;
values[1] = y;
values[2] = z;
}
// Return the length of the vector (inline)
inline double Vector3::length() const {
inline decimal Vector3::length() const {
// Compute and return the length of the vector
return sqrt(values[0]*values[0] + values[1]*values[1] + values[2]*values[2]);
}
// Return the square of the length of the vector
inline double Vector3::lengthSquare() const {
inline decimal Vector3::lengthSquare() const {
return values[0]*values[0] + values[1]*values[1] + values[2]*values[2];
}
// Scalar product of two vectors (inline)
inline double Vector3::dot(const Vector3& vector) const {
inline decimal Vector3::dot(const Vector3& vector) const {
// Compute and return the result of the scalar product
return (values[0] * vector.values[0] + values[1] * vector.values[1] + values[2] * vector.values[2]);
}
@ -154,7 +155,7 @@ inline Vector3 Vector3::getAbsoluteVector() const {
// Return true if two vectors are parallel
inline bool Vector3::isParallelWith(const Vector3& vector) const {
double scalarProd = this->dot(vector);
decimal scalarProd = this->dot(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
}
@ -206,7 +207,7 @@ inline Vector3& Vector3::operator-=(const Vector3& vector) {
}
// Overloaded operator for multiplication with a number with assignment
inline Vector3& Vector3::operator*=(double number) {
inline Vector3& Vector3::operator*=(decimal number) {
values[0] *= number;
values[1] *= number;
values[2] *= number;
@ -214,12 +215,12 @@ inline Vector3& Vector3::operator*=(double number) {
}
// Overloaded operator for value access
inline double& Vector3::operator[] (int index) {
inline decimal& Vector3::operator[] (int index) {
return values[index];
}
// Overloaded operator for value access
inline const double& Vector3::operator[] (int index) const {
inline const decimal& Vector3::operator[] (int index) const {
return values[index];
}
@ -239,12 +240,12 @@ inline Vector3 operator-(const Vector3& vector) {
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(const Vector3& vector, double number) {
inline Vector3 operator*(const Vector3& vector, decimal number) {
return Vector3(number * vector.values[0], number * vector.values[1], number * vector.values[2]);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(double number, const Vector3& vector) {
inline Vector3 operator*(decimal number, const Vector3& vector) {
return vector * number;
}

55
src/mathematics/mathematics.h
View File

@ -31,7 +31,7 @@
#include "Quaternion.h"
#include "Vector3.h"
#include "Transform.h"
#include "../constants.h"
#include "../configuration.h"
#include "mathematics_functions.h"
#include <vector>
#include <cstdio>
@ -61,15 +61,15 @@ inline reactphysics3d::Vector3 rotateVectorWithQuaternion(const reactphysics3d::
// P1 = point1 + alpha * d1
// P2 = point2 + beta * d2
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3& point1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& point2,
const reactphysics3d::Vector3& d2, double* alpha, double* beta) {
const reactphysics3d::Vector3& d2, decimal* alpha, decimal* beta) {
reactphysics3d::Vector3 r = point1 - point2;
double a = d1.dot(d1);
double b = d1.dot(d2);
double c = d1.dot(r);
double e = d2.dot(d2);
double f = d2.dot(r);
double d = a*e-b*b;
decimal a = d1.dot(d1);
decimal b = d1.dot(d2);
decimal c = d1.dot(r);
decimal e = d2.dot(d2);
decimal f = d2.dot(r);
decimal d = a*e-b*b;
// The two lines must not be parallel
assert(!reactphysics3d::approxEqual(d, 0.0));
@ -90,11 +90,11 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea
}
// Compute the length of the segment
double segmentLength = d.length();
decimal segmentLength = d.length();
// Compute the distance from point "P" to points "segPointA" and "segPointB"
double distA = dP.length();
double distB = (P - segPointB).length();
decimal distA = dP.length();
decimal distB = (P - segPointB).length();
// If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment
if (distA > segmentLength || distB > segmentLength) {
@ -111,7 +111,7 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const rea
inline reactphysics3d::Vector3 computeLinesIntersection(const reactphysics3d::Vector3& p1, const reactphysics3d::Vector3& d1,
const reactphysics3d::Vector3& p2, const reactphysics3d::Vector3& d2) {
// Computes the two closest points on the lines
double alpha, beta;
decimal alpha, beta;
closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
reactphysics3d::Vector3 point1 = p1 + alpha * d1;
reactphysics3d::Vector3 point2 = p2 + beta * d2;
@ -136,7 +136,7 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac
assert(!d1.isParallelWith(d2));
// Compute the closet points between the two lines
double alpha, beta;
decimal alpha, beta;
closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
reactphysics3d::Vector3 point1 = seg1PointA + alpha * d1;
reactphysics3d::Vector3 point2 = seg2PointA + beta * d2;
@ -147,7 +147,8 @@ inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reac
// If the two closest point aren't very close, there is no intersection between the segments
reactphysics3d::Vector3 d = point2 - point1;
assert(d.length() <= EPSILON_TEST);
const decimal epsilon = 0.00001;
assert(d.length() <= epsilon);
// They are very close so we return the intersection point (halfway between "point1" and "point2"
return 0.5 * (point1 + point2);
@ -192,7 +193,7 @@ inline std::vector<reactphysics3d::Vector3> projectPointsOntoPlane(const std::ve
// Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
inline decimal computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
assert(v.length() != 0);
return ((P-A).cross(v).length() / (v.length()));
}
@ -208,12 +209,12 @@ inline reactphysics3d::Vector3 computeOrthogonalProjectionOfPointOntoALine(const
// The result point Q will be in a segment of the rectangle
inline reactphysics3d::Vector3 computeNearestPointOnRectangle(const reactphysics3d::Vector3& P, const std::vector<reactphysics3d::Vector3> rectangle) {
assert(rectangle.size() == 4);
double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
decimal distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
decimal distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
decimal distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
decimal distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
decimal distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
decimal distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
Vector3 resultPoint;
// Check if P is between the lines of the first pair of parallel segments of the rectangle
@ -280,8 +281,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s
assert(d1.isParallelWith(d2));
// Compute the projection of the two points of the second segment onto the vector of segment 1
double projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA);
double projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA);
decimal projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA);
decimal projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA);
// The projections intervals should intersect
assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0));
@ -319,7 +320,7 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& s
// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle".
// This method returns the clipped segment.
inline std::vector<reactphysics3d::Vector3> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& segment, const std::vector<reactphysics3d::Vector3> clipRectangle) {
double const epsilon = 0.01;
decimal const epsilon = 0.01;
assert(segment.size() == 2);
assert(clipRectangle.size() == 4);
@ -375,7 +376,7 @@ inline std::vector<reactphysics3d::Vector3> clipSegmentWithRectangleInPlane(cons
// a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that
// they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon.
inline std::vector<reactphysics3d::Vector3> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& subjectPolygon, const std::vector<reactphysics3d::Vector3>& clipRectangle) {
double const epsilon = 0.1;
decimal const epsilon = 0.1;
assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3> outputPolygon;
@ -432,10 +433,10 @@ inline reactphysics3d::Vector3 intersectLineWithPlane(const reactphysics3d::Vect
assert(!approxEqual(lineVector.dot(planeNormal), 0.0));
// The plane is represented by the equation planeNormal dot X = d where X is a point of the plane
double d = planeNormal.dot(planePoint);
decimal d = planeNormal.dot(planePoint);
// Compute the parameter t
double t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector);
decimal t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector);
// Compute the intersection point
return linePoint + lineVector * t;

7
src/mathematics/mathematics_functions.h
View File

@ -27,7 +27,8 @@
#define MATHEMATICS_FUNCTIONS_H
// Libraries
#include "../constants.h"
#include "../configuration.h"
#include "../decimal.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -36,8 +37,8 @@ namespace reactphysics3d {
// function to test if two real numbers are (almost) equal
// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(double a, double b, double epsilon = EPSILON) {
double difference = a - b;
inline bool approxEqual(decimal a, decimal b, decimal epsilon = 1.0e-10) {
decimal difference = a - b;
return (difference < epsilon && difference > -epsilon);
}

6
src/memory/MemoryPool.h
View File

@ -27,7 +27,7 @@
#define MEMORY_POOL_H
// Libraries
#include "../constants.h"
#include "../configuration.h"
#include <cstddef>
#include <cstdlib>
#include <cassert>
@ -115,6 +115,10 @@ MemoryPool<T>::MemoryPool(uint nbObjectsToAllocate) throw(std::bad_alloc)
// Deallocate the block of memory that has been allocated previously
template<class T>
MemoryPool<T>::~MemoryPool() {
// Check if we have a memory leak
assert(currentNbObjects == 0);
// Release the whole memory block
free(pMemoryBlock);
}

10
src/reactphysics3d.h
View File

@ -40,11 +40,11 @@
#include "body/RigidBody.h"
#include "engine/PhysicsWorld.h"
#include "engine/PhysicsEngine.h"
#include "shapes/Shape.h"
#include "shapes/BoxShape.h"
#include "shapes/SphereShape.h"
#include "shapes/ConeShape.h"
#include "shapes/CylinderShape.h"
#include "shapes/Collider.h"
#include "shapes/BoxCollider.h"
#include "shapes/SphereCollider.h"
#include "shapes/ConeCollider.h"
#include "shapes/CylinderCollider.h"
#include "shapes/AABB.h"
// Alias to the ReactPhysics3D namespace

14
src/shapes/AABB.h
View File

@ -47,22 +47,22 @@ class AABB {
private :
Vector3 minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis
Vector3 maxCoordinates; // Maximum world coordinates of the AABB on the x,y and z axis
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public :
AABB(); // Constructor
AABB(); // Constructor
AABB(const Transform& transform, const Vector3& extents); // Constructor
virtual ~AABB(); // Destructor
virtual ~AABB(); // Destructor
Vector3 getCenter() const; // Return the center point
const Vector3& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3& getMaxCoordinates() const; // Return the maximum coordinates of the AABB
Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
virtual void update(const Transform& newTransform, const Vector3& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the AABB (only for testing purpose)
virtual void draw() const; // Draw the AABB (only for testing purpose)
#endif
};

26
src/shapes/BoxShape.cpp → src/shapes/BoxCollider.cpp
View File

@ -24,7 +24,7 @@
********************************************************************************/
// Libraries
#include "BoxShape.h"
#include "BoxCollider.h"
#include "../configuration.h"
#include <vector>
#include <cassert>
@ -46,21 +46,21 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
BoxShape::BoxShape(const Vector3& extent) : extent(extent) {
BoxCollider::BoxCollider(const Vector3& extent) : extent(extent) {
}
// Destructor
BoxShape::~BoxShape() {
BoxCollider::~BoxCollider() {
}
// Return the local inertia tensor of the shape
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
double factor = (1.0 / 3.0) * mass;
double xSquare = extent.getX() * extent.getX();
double ySquare = extent.getY() * extent.getY();
double zSquare = extent.getZ() * extent.getZ();
// Return the local inertia tensor of the collider
void BoxCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (1.0 / 3.0) * mass;
decimal xSquare = extent.getX() * extent.getX();
decimal ySquare = extent.getY() * extent.getY();
decimal zSquare = extent.getZ() * extent.getZ();
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
@ -68,10 +68,10 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
#ifdef VISUAL_DEBUG
// Draw the Box (only for testing purpose)
void BoxShape::draw() const {
double e1 = extent.getX();
double e2 = extent.getY();
double e3 = extent.getZ();
void BoxCollider::draw() const {
decimal e1 = extent.getX();
decimal e2 = extent.getY();
decimal e3 = extent.getZ();
// Draw in red
glColor3f(1.0, 0.0, 0.0);

32
src/shapes/BoxShape.h → src/shapes/BoxCollider.h
View File

@ -23,12 +23,12 @@
* *
********************************************************************************/
#ifndef BOX_SHAPE_H
#define BOX_SHAPE_H
#ifndef BOX_COLLIDER_H
#define BOX_COLLIDER_H
// Libraries
#include <cfloat>
#include "Shape.h"
#include "Collider.h"
#include "../mathematics/mathematics.h"
@ -36,26 +36,26 @@
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class BoxShape :
This class represents a 3D box. Those axis are unit length.
Class BoxCollider :
This class represents a 3D box collider. 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 gives an orientation and a position to the box.
-------------------------------------------------------------------
*/
class BoxShape : public Shape {
class BoxCollider : public Collider {
private :
Vector3 extent; // Extent sizes of the box in the x, y and z direction
public :
BoxShape(const Vector3& extent); // Constructor
virtual ~BoxShape(); // Destructor
BoxCollider(const Vector3& extent); // Constructor
virtual ~BoxCollider(); // Destructor
const Vector3& getExtent() const; // Return the extents of the box
void setExtent(const Vector3& extent); // Set the extents of the box
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape
virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a local support point in a given direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the Box (only for testing purpose)
@ -63,23 +63,23 @@ class BoxShape : public Shape {
};
// Return the extents of the box
inline const Vector3& BoxShape::getExtent() const {
inline const Vector3& BoxCollider::getExtent() const {
return extent;
}
// Set the extents of the box
inline void BoxShape::setExtent(const Vector3& extent) {
inline void BoxCollider::setExtent(const Vector3& extent) {
this->extent = extent;
}
// Return the local extents of the shape (half-width) in x,y and z local direction
// Return the local extents of the box (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box
inline Vector3 BoxShape::getLocalExtents(double margin) const {
inline Vector3 BoxCollider::getLocalExtents(decimal margin) const {
return extent + Vector3(margin, margin, margin);
}
// Return a local support point in a given direction
inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
inline Vector3 BoxCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
return Vector3(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin,

6
src/shapes/Shape.cpp → src/shapes/Collider.cpp
View File

@ -24,17 +24,17 @@
********************************************************************************/
// Libraries
#include "Shape.h"
#include "Collider.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Shape::Shape() {
Collider::Collider() {
}
// Destructor
Shape::~Shape() {
Collider::~Collider() {
}

26
src/shapes/Shape.h → src/shapes/Collider.h
View File

@ -23,8 +23,8 @@
* *
********************************************************************************/
#ifndef SHAPE_H
#define SHAPE_H
#ifndef COLLIDER_H
#define COLLIDER_H
// Libraries
#include <cassert>
@ -38,34 +38,34 @@ namespace reactphysics3d {
class Body;
/* -------------------------------------------------------------------
Class Shape :
This abstract class represents the shape of a body that is used during
the narrow-phase collision detection.
Class Collider :
This abstract class represents the collider associated with a
body that is used during the narrow-phase collision detection.
-------------------------------------------------------------------
*/
class Shape {
class Collider {
protected :
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public :
Shape(); // Constructor
virtual ~Shape(); // Destructor
Collider(); // Constructor
virtual ~Collider(); // Destructor
Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* bodyPointer); // Set the body pointer
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const=0; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const=0; // Return the local inertia tensor of the shape
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const=0; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; // Return the local inertia tensor of the collider
};
// Return the body pointer
inline Body* Shape::getBodyPointer() const {
inline Body* Collider::getBodyPointer() const {
assert(bodyPointer != 0);
return bodyPointer;
}
// Set the body pointer
inline void Shape::setBodyPointer(Body* bodyPointer) {
inline void Collider::setBodyPointer(Body* bodyPointer) {
this->bodyPointer = bodyPointer;
}

16
src/shapes/ConeShape.cpp → src/shapes/ConeCollider..cpp
View File

@ -26,7 +26,7 @@
// Libraries
#include <complex>
#include "../configuration.h"
#include "ConeShape.h"
#include "ConeCollider.h"
#if defined(VISUAL_DEBUG)
#if defined(APPLE_OS)
@ -44,7 +44,7 @@
using namespace reactphysics3d;
// Constructor
ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
ConeCollider::ConeCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
assert(radius > 0.0);
assert(halfHeight > 0.0);
@ -53,25 +53,25 @@ ConeShape::ConeShape(double radius, double height) : radius(radius), halfHeight(
}
// Destructor
ConeShape::~ConeShape() {
ConeCollider::~ConeCollider() {
}
// Return a local support point in a given direction
inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
inline Vector3 ConeCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
const Vector3& v = direction;
double sinThetaTimesLengthV = sinTheta * v.length();
decimal sinThetaTimesLengthV = sinTheta * v.length();
Vector3 supportPoint;
if (v.getY() >= sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, halfHeight, 0.0);
}
else {
double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
decimal projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
if (projectedLength > MACHINE_EPSILON) {
double d = radius / projectedLength;
decimal d = radius / projectedLength;
supportPoint = Vector3(v.getX() * d, -halfHeight, v.getZ() * d);
}
else {
@ -93,7 +93,7 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double
#ifdef VISUAL_DEBUG
// Draw the cone (only for debuging purpose)
void ConeShape::draw() const {
void ConeCollider::draw() const {
// Draw in red
glColor3f(1.0, 0.0, 0.0);

54
src/shapes/ConeShape.h → src/shapes/ConeCollider.h
View File

@ -23,19 +23,19 @@
* *
********************************************************************************/
#ifndef CONE_SHAPE_H
#define CONE_SHAPE_H
#ifndef CONE_COLLIDER_H
#define CONE_COLLIDER_H
// Libraries
#include "Shape.h"
#include "Collider.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class ConeShape :
This class represents a cone collision shape centered at the
Class ConeCollider :
This class represents a cone collider centered at the
origin and alligned with the Y axis. The cone is defined
by its height and by the radius of its base. The center of the
cone is at the half of the height. The "transform" of the
@ -43,23 +43,23 @@ namespace reactphysics3d {
to the cone.
-------------------------------------------------------------------
*/
class ConeShape : public Shape {
class ConeCollider : public Collider {
private :
double radius; // Radius of the base
double halfHeight; // Half height of the cone
double sinTheta; // sine of the semi angle at the apex point
decimal radius; // Radius of the base
decimal halfHeight; // Half height of the cone
decimal sinTheta; // sine of the semi angle at the apex point
public :
ConeShape(double radius, double height); // Constructor
virtual ~ConeShape(); // Destructor
ConeCollider(decimal radius, decimal height); // Constructor
virtual ~ConeCollider(); // Destructor
double getRadius() const; // Return the radius
void setRadius(double radius); // Set the radius
double getHeight() const; // Return the height
void setHeight(double height); // Set the height
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape
decimal getRadius() const; // Return the radius
void setRadius(decimal radius); // Set the radius
decimal getHeight() const; // Return the height
void setHeight(decimal height); // Set the height
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a support point in a given direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider
#ifdef VISUAL_DEBUG
@ -68,12 +68,12 @@ class ConeShape : public Shape {
};
// Return the radius
inline double ConeShape::getRadius() const {
inline decimal ConeCollider::getRadius() const {
return radius;
}
// Set the radius
inline void ConeShape::setRadius(double radius) {
inline void ConeCollider::setRadius(decimal radius) {
this->radius = radius;
// Update sine of the semi-angle at the apex point
@ -81,12 +81,12 @@ inline void ConeShape::setRadius(double radius) {
}
// Return the height
inline double ConeShape::getHeight() const {
inline decimal ConeCollider::getHeight() const {
return 2.0 * halfHeight;
}
// Set the height
inline void ConeShape::setHeight(double height) {
inline void ConeCollider::setHeight(decimal height) {
this->halfHeight = height / 2.0;
// Update the sine of the semi-angle at the apex point
@ -94,14 +94,14 @@ inline void ConeShape::setHeight(double height) {
}
// Return the local extents in x,y and z direction
inline Vector3 ConeShape::getLocalExtents(double margin) const {
inline Vector3 ConeCollider::getLocalExtents(decimal margin) const {
return Vector3(radius + margin, halfHeight + margin, radius + margin);
}
// Return the local inertia tensor of the shape
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
double rSquare = radius * radius;
double diagXZ = 0.15 * mass * (rSquare + halfHeight);
// Return the local inertia tensor of the collider
inline void ConeCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal rSquare = radius * radius;
decimal diagXZ = 0.15 * mass * (rSquare + halfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ);
}

14
src/shapes/CylinderShape.cpp → src/shapes/CylinderCollider.cpp
View File

@ -24,7 +24,7 @@
********************************************************************************/
// Libraries
#include "CylinderShape.h"
#include "CylinderCollider.h"
#include "../configuration.h"
#if defined(VISUAL_DEBUG)
@ -43,23 +43,23 @@
using namespace reactphysics3d;
// Constructor
CylinderShape::CylinderShape(double radius, double height) : radius(radius), halfHeight(height/2.0) {
CylinderCollider::CylinderCollider(decimal radius, decimal height) : radius(radius), halfHeight(height/2.0) {
}
// Destructor
CylinderShape::~CylinderShape() {
CylinderCollider::~CylinderCollider() {
}
// Return a local support point in a given direction
Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
Vector3 CylinderCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
Vector3 supportPoint(0.0, 0.0, 0.0);
double uDotv = direction.getY();
decimal uDotv = direction.getY();
Vector3 w(direction.getX(), 0.0, direction.getZ());
double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
decimal lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
if (lengthW != 0.0) {
if (uDotv < 0.0) supportPoint.setY(-halfHeight);
@ -85,7 +85,7 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double mar
#ifdef VISUAL_DEBUG
// Draw the cone (only for debuging purpose)
void CylinderShape::draw() const {
void CylinderCollider::draw() const {
// Draw in red
glColor3f(1.0, 0.0, 0.0);

50
src/shapes/CylinderShape.h → src/shapes/CylinderCollider.h
View File

@ -23,11 +23,11 @@
* *
********************************************************************************/
#ifndef CYLINDER_SHAPE_H
#define CYLINDER_SHAPE_H
#ifndef CYLINDER_COLLIDER_H
#define CYLINDER_COLLIDER_H
// Libraries
#include "Shape.h"
#include "Collider.h"
#include "../mathematics/mathematics.h"
@ -35,29 +35,29 @@
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class CylinderShape :
This class represents a cylinder collision shape around the Y axis
Class CylinderCollider :
This class represents a cylinder collision collider around the Y axis
and centered at the origin. The cylinder is defined by its height
and the radius of its base. The "transform" of the corresponding
rigid body gives an orientation and a position to the cylinder.
-------------------------------------------------------------------
*/
class CylinderShape : public Shape {
class CylinderCollider : public Collider {
private :
double radius; // Radius of the base
double halfHeight; // Half height of the cone
decimal radius; // Radius of the base
decimal halfHeight; // Half height of the cone
public :
CylinderShape(double radius, double height); // Constructor
virtual ~CylinderShape(); // Destructor
CylinderCollider(decimal radius, decimal height); // Constructor
virtual ~CylinderCollider(); // Destructor
double getRadius() const; // Return the radius
void setRadius(double radius); // Set the radius
double getHeight() const; // Return the height
void setHeight(double height); // Set the height
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape
decimal getRadius() const; // Return the radius
void setRadius(decimal radius); // Set the radius
decimal getHeight() const; // Return the height
void setHeight(decimal height); // Set the height
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a support point in a given direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the sphere (only for testing purpose)
@ -65,34 +65,34 @@ class CylinderShape : public Shape {
};
// Return the radius
inline double CylinderShape::getRadius() const {
inline decimal CylinderCollider::getRadius() const {
return radius;
}
// Set the radius
inline void CylinderShape::setRadius(double radius) {
inline void CylinderCollider::setRadius(decimal radius) {
this->radius = radius;
}
// Return the height
inline double CylinderShape::getHeight() const {
inline decimal CylinderCollider::getHeight() const {
return halfHeight * 2.0;
}
// Set the height
inline void CylinderShape::setHeight(double height) {
inline void CylinderCollider::setHeight(decimal height) {
this->halfHeight = height / 2.0;
}
// Return the local extents in x,y and z direction
inline Vector3 CylinderShape::getLocalExtents(double margin) const {
inline Vector3 CylinderCollider::getLocalExtents(decimal margin) const {
return Vector3(radius + margin, halfHeight + margin, radius + margin);
}
// Return the local inertia tensor of the cylinder
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
double height = 2.0 * halfHeight;
double diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height);
inline void CylinderCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal height = 2.0 * halfHeight;
decimal diag = (1.0 / 12.0) * mass * (3 * radius * radius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * radius * radius, 0.0, 0.0, 0.0, diag);
}

8
src/shapes/SphereShape.cpp → src/shapes/SphereCollider.cpp
View File

@ -24,7 +24,7 @@
********************************************************************************/
// Libraries
#include "SphereShape.h"
#include "SphereCollider.h"
#include "../configuration.h"
#include <cassert>
@ -45,18 +45,18 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
SphereShape::SphereShape(double radius) : radius(radius) {
SphereCollider::SphereCollider(decimal radius) : radius(radius) {
}
// Destructor
SphereShape::~SphereShape() {
SphereCollider::~SphereCollider() {
}
#ifdef VISUAL_DEBUG
// Draw the sphere (only for testing purpose)
void SphereShape::draw() const {
void SphereCollider::draw() const {
// Draw in red
glColor3f(1.0, 0.0, 0.0);

44
src/shapes/SphereShape.h → src/shapes/SphereCollider.h
View File

@ -23,35 +23,35 @@
* *
********************************************************************************/
#ifndef SPHERE_SHAPE_H
#define SPHERE_SHAPE_H
#ifndef SPHERE_COLLIDER_H
#define SPHERE_COLLIDER_H
// Libraries
#include "Shape.h"
#include "Collider.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class SphereShape :
This class represents a sphere collision shape that is centered
Class SphereCollider :
This class represents a sphere collider that is centered
at the origin and defined by its radius.
-------------------------------------------------------------------
*/
class SphereShape : public Shape {
class SphereCollider : public Collider {
private :
double radius; // Radius of the sphere
decimal radius; // Radius of the sphere
public :
SphereShape(double radius); // Constructor
virtual ~SphereShape(); // Destructor
SphereCollider(decimal radius); // Constructor
virtual ~SphereCollider(); // Destructor
double getRadius() const; // Return the radius of the sphere
void setRadius(double radius); // Set the radius of the sphere
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const; // Return the local inertia tensor of the shape
decimal getRadius() const; // Return the radius of the sphere
void setRadius(decimal radius); // Set the radius of the sphere
virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(decimal margin=0.0) const; // Return the local extents in x,y and z direction
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; // Return the local inertia tensor of the collider
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the sphere (only for testing purpose)
@ -59,19 +59,19 @@ class SphereShape : public Shape {
};
// Get the radius of the sphere
inline double SphereShape::getRadius() const {
inline decimal SphereCollider::getRadius() const {
return radius;
}
// Set the radius of the sphere
inline void SphereShape::setRadius(double radius) {
inline void SphereCollider::setRadius(decimal radius) {
this->radius = radius;
}
// Return a local support point in a given direction
inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
inline Vector3 SphereCollider::getLocalSupportPoint(const Vector3& direction, decimal margin) const {
assert(margin >= 0.0);
double length = direction.length();
decimal length = direction.length();
// If the direction vector is not the zero vector
if (length > 0.0) {
@ -84,15 +84,15 @@ inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, doubl
return Vector3(0, radius + margin, 0);
}
// Return the local extents of the shape (half-width) in x,y and z local direction
// Return the local extents of the collider (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box
inline Vector3 SphereShape::getLocalExtents(double margin) const {
inline Vector3 SphereCollider::getLocalExtents(decimal margin) const {
return Vector3(radius + margin, radius + margin, radius + margin);
}
// Return the local inertia tensor of the sphere
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, double mass) const {
double diag = 0.4 * mass * radius * radius;
inline void SphereCollider::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal diag = 0.4 * mass * radius * radius;
tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag);
}