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

Changes and optimizations in the Vector3 and Matrix3x3 classes

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@437 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-08-18 21:02:48 +00:00
parent 03557bbff6
commit 40ec21b398
52 changed files with 826 additions and 814 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/body/RigidBody.h
View File

@ -43,10 +43,10 @@ namespace reactphysics3d {
*/ */
class RigidBody : public Body { class RigidBody : public Body {
protected : protected :
Vector3D linearVelocity; // Linear velocity of the body Vector3 linearVelocity; // Linear velocity of the body
Vector3D angularVelocity; // Angular velocity of the body Vector3 angularVelocity; // Angular velocity of the body
Vector3D externalForce; // Current external force on the body Vector3 externalForce; // Current external force on the body
Vector3D externalTorque; // Current external torque on the body Vector3 externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the 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 massInverse; // Inverse of the mass of the body
@ -57,15 +57,15 @@ class RigidBody : public Body {
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor virtual ~RigidBody(); // Destructor
Vector3D getLinearVelocity() const; // Return the linear velocity Vector3 getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body
Vector3D getAngularVelocity() const; // Return the angular velocity Vector3 getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity void setAngularVelocity(const Vector3& angularVelocity); // Set the angular velocity
void setMassInverse(double massInverse); // Set the inverse of the mass void setMassInverse(double massInverse); // Set the inverse of the mass
Vector3D getExternalForce() const; // Return the current external force of the body Vector3 getExternalForce() const; // Return the current external force of the body
void setExternalForce(const Vector3D& force); // Set the current external force on the body void setExternalForce(const Vector3& force); // Set the current external force on the body
Vector3D getExternalTorque() const; // Return the current external torque of the body Vector3 getExternalTorque() const; // Return the current external torque of the body
void setExternalTorque(const Vector3D& torque); // Set 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 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) 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) void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates)
@ -78,16 +78,16 @@ class RigidBody : public Body {
}; };
// Return the linear velocity // Return the linear velocity
inline Vector3D RigidBody::getLinearVelocity() const { inline Vector3 RigidBody::getLinearVelocity() const {
return linearVelocity; return linearVelocity;
} }
// Return the angular velocity of the body // Return the angular velocity of the body
inline Vector3D RigidBody::getAngularVelocity() const { inline Vector3 RigidBody::getAngularVelocity() const {
return angularVelocity; return angularVelocity;
} }
inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) { inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
this->angularVelocity = angularVelocity; this->angularVelocity = angularVelocity;
} }
@ -102,22 +102,22 @@ inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const {
} }
// Return the external force on the body // Return the external force on the body
inline Vector3D RigidBody::getExternalForce() const { inline Vector3 RigidBody::getExternalForce() const {
return externalForce; return externalForce;
} }
// Set the external force on the body // Set the external force on the body
inline void RigidBody::setExternalForce(const Vector3D& force) { inline void RigidBody::setExternalForce(const Vector3& force) {
this->externalForce = force; this->externalForce = force;
} }
// Return the current external torque on the body // Return the current external torque on the body
inline Vector3D RigidBody::getExternalTorque() const { inline Vector3 RigidBody::getExternalTorque() const {
return externalTorque; return externalTorque;
} }
// Set the current external torque on the body // Set the current external torque on the body
inline void RigidBody::setExternalTorque(const Vector3D& torque) { inline void RigidBody::setExternalTorque(const Vector3& torque) {
this->externalTorque = torque; this->externalTorque = torque;
} }
@ -155,7 +155,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
} }
// Set the linear velocity of the rigid body // Set the linear velocity of the rigid body
inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) { inline void RigidBody::setLinearVelocity(const Vector3& linearVelocity) {
// If the body is able to move // If the body is able to move
if (isMotionEnabled) { if (isMotionEnabled) {
// Update the linear velocity of the current body state // Update the linear velocity of the current body state

6
src/collision/ContactInfo.cpp
View File

@ -29,14 +29,14 @@ using namespace reactphysics3d;
// Constructor for SAT // Constructor for SAT
// TODO : DELETE THIS // TODO : DELETE THIS
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth) ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal, double penetrationDepth)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) { : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) {
} }
// Constructor for GJK // Constructor for GJK
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal, ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2) double penetrationDepth, const Vector3& point1, const Vector3& point2)
: body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) { : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) {
} }

12
src/collision/ContactInfo.h
View File

@ -44,15 +44,15 @@ struct ContactInfo {
public: public:
Body* const body1; // Pointer to the first body of the contact Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact Body* const body2; // Pointer to the second body of the contact
const Vector3D point1; // Contact point of body 1 const Vector3 point1; // Contact point of body 1
const Vector3D point2; // Contact point of body 2 const Vector3 point2; // Contact point of body 2
const Vector3D normal; // Normal vector the the collision contact const Vector3 normal; // Normal vector the the collision contact
const double penetrationDepth; // Penetration depth of the contact const double penetrationDepth; // Penetration depth of the contact
ContactInfo(Body* body1, Body* body2, const Vector3D& normal, ContactInfo(Body* body1, Body* body2, const Vector3& normal,
double penetrationDepth); // Constructor for SAT double penetrationDepth); // Constructor for SAT
ContactInfo(Body* body1, Body* body2, const Vector3D& normal, ContactInfo(Body* body1, Body* body2, const Vector3& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK double penetrationDepth, const Vector3& point1, const Vector3& point2); // Constructor for GJK
}; };
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

50
src/collision/EPA/EPAAlgorithm.cpp
View File

@ -43,28 +43,28 @@ EPAAlgorithm::~EPAAlgorithm() {
// Decide if the origin is in the tetrahedron // Decide if the origin is in the tetrahedron
// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of // Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
// the vertex that is wrong if the origin is not in the tetrahedron // the vertex that is wrong if the origin is not in the tetrahedron
int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p4) const { int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4) const {
// Check vertex 1 // Check vertex 1
Vector3D normal1 = (p2-p1).cross(p3-p1); Vector3 normal1 = (p2-p1).cross(p3-p1);
if (normal1.dot(p1) > 0.0 == normal1.dot(p4) > 0.0) { if (normal1.dot(p1) > 0.0 == normal1.dot(p4) > 0.0) {
return 4; return 4;
} }
// Check vertex 2 // Check vertex 2
Vector3D normal2 = (p4-p2).cross(p3-p2); Vector3 normal2 = (p4-p2).cross(p3-p2);
if (normal2.dot(p2) > 0.0 == normal2.dot(p1) > 0.0) { if (normal2.dot(p2) > 0.0 == normal2.dot(p1) > 0.0) {
return 1; return 1;
} }
// Check vertex 3 // Check vertex 3
Vector3D normal3 = (p4-p3).cross(p1-p3); Vector3 normal3 = (p4-p3).cross(p1-p3);
if (normal3.dot(p3) > 0.0 == normal3.dot(p2) > 0.0) { if (normal3.dot(p3) > 0.0 == normal3.dot(p2) > 0.0) {
return 2; return 2;
} }
// Check vertex 4 // Check vertex 4
Vector3D normal4 = (p2-p4).cross(p1-p4); Vector3 normal4 = (p2-p4).cross(p1-p4);
if (normal4.dot(p4) > 0.0 == normal4.dot(p3) > 0.0) { if (normal4.dot(p4) > 0.0 == normal4.dot(p3) > 0.0) {
return 3; return 3;
} }
@ -81,10 +81,10 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
// the correct penetration depth // the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1, bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, const Shape* shape2, const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo) { Vector3& v, ContactInfo*& contactInfo) {
Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3D points[MAX_SUPPORT_POINTS]; // Current points Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
@ -125,7 +125,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// v1, v2 and v3. // v1, v2 and v3.
// Direction of the segment // Direction of the segment
Vector3D d = (points[1] - points[0]).getUnit(); Vector3 d = (points[1] - points[0]).getUnit();
// Choose the coordinate axis from the minimal absolute component of the vector d // Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.getAbsoluteVector().getMinAxis(); int minAxis = d.getAbsoluteVector().getMinAxis();
@ -141,23 +141,23 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Matrix3x3 rotationMat = rotationQuat.getMatrix(); Matrix3x3 rotationMat = rotationQuat.getMatrix();
// Compute the vector v1, v2, v3 // Compute the vector v1, v2, v3
Vector3D v1 = d.cross(Vector3D(minAxis == 0, minAxis == 1, minAxis == 2)); Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3D v2 = rotationMat * v1; Vector3 v2 = rotationMat * v1;
Vector3D v3 = rotationMat * v2; Vector3 v3 = rotationMat * v2;
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN); suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN); suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v1), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2]; points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2 // Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN); suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN); suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v2), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3 // Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN); suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN); suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v3), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
@ -246,15 +246,15 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// normal of the triangle. // normal of the triangle.
// Compute the normal of the triangle // Compute the normal of the triangle
Vector3D v1 = points[1] - points[0]; Vector3 v1 = points[1] - points[0];
Vector3D v2 = points[2] - points[0]; Vector3 v2 = points[2] - points[0];
Vector3D n = v1.cross(v2); Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron // Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN); suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN); suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-n), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3]; points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPoint(n.getOpposite(), OBJECT_MARGIN); suppPointsA[4] = shape1->getLocalSupportPoint(-n, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN); suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n, OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
@ -325,7 +325,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the support point of the Minkowski difference (A-B) in the closest point direction // Compute the support point of the Minkowski difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); suppPointsA[nbVertices] = shape1->getLocalSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN); suppPointsB[nbVertices] = shape2ToShape1 *shape2->getLocalSupportPoint(rotateToShape2 * (-triangle->getClosestPoint()), OBJECT_MARGIN);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices; int indexNewVertex = nbVertices;
@ -368,9 +368,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the contact info (in world-space) // Compute the contact info (in world-space)
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint(); v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA); Vector3 pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB); Vector3 pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3D normal = v.getUnit(); Vector3 normal = v.getUnit();
double penetrationDepth = v.length(); double penetrationDepth = v.length();
assert(penetrationDepth > 0.0); assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, penetrationDepth, pA, pB);

6
src/collision/EPA/EPAAlgorithm.h
View File

@ -75,8 +75,8 @@ class EPAAlgorithm {
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap
int isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2, int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3D& p3, const Vector3D& p4) const; // Decide if the origin is in the tetrahedron const Vector3& p3, const Vector3& p4) const; // Decide if the origin is in the tetrahedron
public: public:
EPAAlgorithm(); // Constructor EPAAlgorithm(); // Constructor
@ -84,7 +84,7 @@ class EPAAlgorithm {
bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1, bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, const Shape* shape2, const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm Vector3& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
}; };
// Add a triangle face in the candidate triangle heap in the EPA algorithm // Add a triangle face in the candidate triangle heap in the EPA algorithm

2
src/collision/EPA/EdgeEPA.cpp
View File

@ -59,7 +59,7 @@ uint EdgeEPA::getTargetVertexIndex() const {
} }
// Execute the recursive silhouette algorithm from this edge // Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) { bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
// If the edge has not already been visited // If the edge has not already been visited
if (!ownerTriangle->getIsObsolete()) { if (!ownerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point // If the triangle of this edge is not visible from the given point

2
src/collision/EPA/EdgeEPA.h
View File

@ -56,7 +56,7 @@ class EdgeEPA {
int getIndex() const; // Return the index of the edge in the triangle int getIndex() const; // Return the index of the edge in the triangle
uint getSourceVertexIndex() const; // Return index of the source vertex of the edge uint getSourceVertexIndex() const; // Return index of the source vertex of the edge
uint getTargetVertexIndex() const; // Return the index of the target vertex of the edge uint getTargetVertexIndex() const; // Return the index of the target vertex of the edge
bool computeSilhouette(const Vector3D* vertices, uint index, bool computeSilhouette(const Vector3* vertices, uint index,
TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this edge TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this edge
}; };

10
src/collision/EPA/TriangleEPA.cpp
View File

@ -50,11 +50,11 @@ TriangleEPA::~TriangleEPA() {
} }
// Compute the point v closest to the origin of this triangle // Compute the point v closest to the origin of this triangle
bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) { bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3D& p0 = vertices[indicesVertices[0]]; const Vector3& p0 = vertices[indicesVertices[0]];
Vector3D v1 = vertices[indicesVertices[1]] - p0; Vector3 v1 = vertices[indicesVertices[1]] - p0;
Vector3D v2 = vertices[indicesVertices[2]] - p0; Vector3 v2 = vertices[indicesVertices[2]] - p0;
double v1Dotv1 = v1.dot(v1); double v1Dotv1 = v1.dot(v1);
double v1Dotv2 = v1.dot(v2); double v1Dotv2 = v1.dot(v2);
double v2Dotv2 = v2.dot(v2); double v2Dotv2 = v2.dot(v2);
@ -118,7 +118,7 @@ void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
// are seen and faces that are not seen from the new vertex. This method starts from the nearest face from the new vertex, // are seen and faces that are not seen from the new vertex. This method starts from the nearest face from the new vertex,
// computes the silhouette and create the new faces from the new vertex in order that we always have a convex polytope. The // computes the silhouette and create the new faces from the new vertex in order that we always have a convex polytope. The
// faces visible from the new vertex are set obselete and will not be considered as being a candidate face in the future. // faces visible from the new vertex are set obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) { bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles(); uint first = triangleStore.getNbTriangles();

22
src/collision/EPA/TriangleEPA.h
View File

@ -51,7 +51,7 @@ class TriangleEPA {
EdgeEPA adjacentEdges[3]; // Three adjacent edges of the triangle (edges of other triangles) 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 bool isObsolete; // True if the triangle face is visible from the new support point
double det; // Determinant double det; // Determinant
Vector3D closestPoint; // Point v closest to the origin on the affine hull of the triangle 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 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 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 double distSquare; // Square distance of the point closest point v to the origin
@ -66,12 +66,12 @@ class TriangleEPA {
double getDistSquare() const; // Return the square distance of the closest point to origin double getDistSquare() const; // Return the square distance of the closest point to origin
void setIsObsolete(bool isObsolete); // Set the isObsolete value void setIsObsolete(bool isObsolete); // Set the isObsolete value
bool getIsObsolete() const; // Return true if the triangle face is obsolete bool getIsObsolete() const; // Return true if the triangle face is obsolete
const Vector3D& getClosestPoint() const; // Return the point closest to the origin const Vector3& getClosestPoint() const; // Return the point closest to the origin
bool isClosestPointInternalToTriangle() const; // Return true if the closest point on affine hull is inside the triangle bool isClosestPointInternalToTriangle() const; // Return true if the closest point on affine hull is inside the triangle
bool isVisibleFromVertex(const Vector3D* vertices, uint index) const; // Return true if the triangle is visible from a given vertex bool isVisibleFromVertex(const Vector3* vertices, uint index) const; // Return true if the triangle is visible from a given vertex
bool computeClosestPoint(const Vector3D* vertices); // Compute the point v closest to the origin of this triangle bool computeClosestPoint(const Vector3* vertices); // Compute the point v closest to the origin of this triangle
Vector3D computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const; // Compute the point of an object closest to the origin Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; // Compute the point of an object closest to the origin
bool computeSilhouette(const Vector3D* vertices, uint index, bool computeSilhouette(const Vector3* vertices, uint index,
TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this triangle face TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this triangle face
uint operator[](int i) const; // Access operator uint operator[](int i) const; // Access operator
@ -107,7 +107,7 @@ inline bool TriangleEPA::getIsObsolete() const {
} }
// Return the point closest to the origin // Return the point closest to the origin
inline const Vector3D& TriangleEPA::getClosestPoint() const { inline const Vector3& TriangleEPA::getClosestPoint() const {
return closestPoint; return closestPoint;
} }
@ -117,14 +117,14 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
} }
// Return true if the triangle is visible from a given vertex // Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3D* vertices, uint index) const { inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
Vector3D closestToVert = vertices[index] - closestPoint; Vector3 closestToVert = vertices[index] - closestPoint;
return (closestPoint.dot(closestToVert) > 0.0); return (closestPoint.dot(closestToVert) > 0.0);
} }
// Compute the point of an object closest to the origin // Compute the point of an object closest to the origin
inline Vector3D TriangleEPA::computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const { inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const {
const Vector3D& p0 = supportPointsOfObject[indicesVertices[0]]; const Vector3& p0 = supportPointsOfObject[indicesVertices[0]];
return p0 + 1.0/det * (lambda1 * (supportPointsOfObject[indicesVertices[1]] - p0) + return p0 + 1.0/det * (lambda1 * (supportPointsOfObject[indicesVertices[1]] - p0) +
lambda2 * (supportPointsOfObject[indicesVertices[2]] - p0)); lambda2 * (supportPointsOfObject[indicesVertices[2]] - p0));
} }

4
src/collision/EPA/TrianglesStore.h
View File

@ -58,7 +58,7 @@ class TrianglesStore {
void setNbTriangles(int backup); // Set the number of triangles void setNbTriangles(int backup); // Set the number of triangles
TriangleEPA& last(); // Return the last triangle TriangleEPA& last(); // Return the last triangle
TriangleEPA* newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2); // Create a new triangle TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2); // Create a new triangle
TriangleEPA& operator[](int i); // Access operator TriangleEPA& operator[](int i); // Access operator
}; };
@ -85,7 +85,7 @@ inline TriangleEPA& TrianglesStore::last() {
} }
// Create a new triangle // Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2) { inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2) {
TriangleEPA* newTriangle = 0; TriangleEPA* newTriangle = 0;
// If we have not reach the maximum number of triangles // If we have not reach the maximum number of triangles

38
src/collision/GJK/GJKAlgorithm.cpp
View File

@ -62,11 +62,11 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
assert(shape1 != shape2); assert(shape1 != shape2);
Vector3D suppA; // Support point of object A Vector3 suppA; // Support point of object A
Vector3D suppB; // Support point of object B Vector3 suppB; // Support point of object B
Vector3D w; // Support point of Minkowski difference A-B Vector3 w; // Support point of Minkowski difference A-B
Vector3D pA; // Closest point of object A Vector3 pA; // Closest point of object A
Vector3D pB; // Closest point of object B Vector3 pB; // Closest point of object B
double vDotw; double vDotw;
double prevDistSquare; double prevDistSquare;
Body* const body1 = shape1->getBodyPointer(); Body* const body1 = shape1->getBodyPointer();
@ -89,14 +89,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Get the last point V (last separating axis) // Get the last point V (last separating axis)
// TODO : Implement frame coherence. For each pair of body, store // TODO : Implement frame coherence. For each pair of body, store
// the last separating axis and use it to initialize the v vector // the last separating axis and use it to initialize the v vector
Vector3D v(1.0, 1.0, 1.0); Vector3 v(1.0, 1.0, 1.0);
// Initialize the upper bound for the square distance // Initialize the upper bound for the square distance
double distSquare = DBL_MAX; double distSquare = DBL_MAX;
do { do {
// Compute the support points for original objects (without margins) A and B // Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPoint(v.getOpposite()); suppA = shape1->getLocalSupportPoint(-v);
suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v); suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
@ -123,7 +123,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -147,7 +147,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -169,7 +169,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -179,14 +179,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Store and update the squared distance of the closest point // Store and update the squared distance of the closest point
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.dot(v); distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot // If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v); simplex.backupClosestPointInSimplex(v);
// Get the new squared distance // Get the new squared distance
distSquare = v.dot(v); distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins) // Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB); simplex.computeClosestPointsOfAandB(pA, pB);
@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v); pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info // Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit(); Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist; double penetrationDepth = margin - dist;
contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB);
@ -222,11 +222,11 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// compute the correct penetration depth and contact points of the enlarged objects. // compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1, bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
const Shape* const shape2, const Transform& transform2, const Shape* const shape2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v) { ContactInfo*& contactInfo, Vector3& v) {
Simplex simplex; Simplex simplex;
Vector3D suppA; Vector3 suppA;
Vector3D suppB; Vector3 suppB;
Vector3D w; Vector3 w;
double vDotw; double vDotw;
double distSquare = DBL_MAX; double distSquare = DBL_MAX;
double prevDistSquare; double prevDistSquare;
@ -239,7 +239,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
do { do {
// Compute the support points for the enlarged object A and B // Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPoint(v.getOpposite(), OBJECT_MARGIN); suppA = shape1->getLocalSupportPoint(-v, OBJECT_MARGIN);
suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN); suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);
// Compute the support point for the Minkowski difference A-B // Compute the support point for the Minkowski difference A-B
@ -266,7 +266,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
// Store and update the square distance // Store and update the square distance
prevDistSquare = distSquare; prevDistSquare = distSquare;
distSquare = v.dot(v); distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false; return false;

2
src/collision/GJK/GJKAlgorithm.h
View File

@ -63,7 +63,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1, bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2, const Shape* shape2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects ContactInfo*& contactInfo, Vector3& v); // Compute the penetration depth for enlarged objects
public : public :
GJKAlgorithm(); // Constructor GJKAlgorithm(); // Constructor

30
src/collision/GJK/Simplex.cpp
View File

@ -43,7 +43,7 @@ Simplex::~Simplex() {
// suppPointA : support point of object A in a direction -v // suppPointA : support point of object A in a direction -v
// suppPointB : support point of object B in a direction v // suppPointB : support point of object B in a direction v
// point : support point of object (A-B) => point = suppPointA - suppPointB // point : support point of object (A-B) => point = suppPointA - suppPointB
void Simplex::addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB) { void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) {
assert(!isFull()); assert(!isFull());
lastFound = 0; lastFound = 0;
@ -75,7 +75,7 @@ void Simplex::addPoint(const Vector3D& point, const Vector3D& suppPointA, const
} }
// Return true if the point is in the simplex // Return true if the point is in the simplex
bool Simplex::isPointInSimplex(const Vector3D& point) const { bool Simplex::isPointInSimplex(const Vector3& point) const {
int i; int i;
Bits bit; Bits bit;
@ -100,7 +100,7 @@ void Simplex::updateCache() {
// Compute the distance between two points in the possible simplex set // Compute the distance between two points in the possible simplex set
diffLength[i][lastFound] = points[i] - points[lastFound]; diffLength[i][lastFound] = points[i] - points[lastFound];
diffLength[lastFound][i] = diffLength[i][lastFound].getOpposite(); diffLength[lastFound][i] = -diffLength[i][lastFound];
// Compute the squared length of the vector distances from points in the possible simplex set // Compute the squared length of the vector distances from points in the possible simplex set
normSquare[i][lastFound] = normSquare[lastFound][i] = diffLength[i][lastFound].dot(diffLength[i][lastFound]); normSquare[i][lastFound] = normSquare[lastFound][i] = diffLength[i][lastFound].dot(diffLength[i][lastFound]);
@ -109,7 +109,7 @@ void Simplex::updateCache() {
} }
// Return the points of the simplex // Return the points of the simplex
unsigned int Simplex::getSimplex(Vector3D* suppPointsA, Vector3D* suppPointsB, Vector3D* points) const { unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, Vector3* points) const {
unsigned int nbVertices = 0; unsigned int nbVertices = 0;
int i; int i;
Bits bit; Bits bit;
@ -271,7 +271,7 @@ bool Simplex::isValidSubset(Bits subset) const {
// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A // pA = sum(lambda_i * a_i) where "a_i" are the support points of object A
// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B // pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
// with lambda_i = deltaX_i / deltaX // with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const { void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
double deltaX = 0.0; double deltaX = 0.0;
pA.setAllValues(0.0, 0.0, 0.0); pA.setAllValues(0.0, 0.0, 0.0);
pB.setAllValues(0.0, 0.0, 0.0); pB.setAllValues(0.0, 0.0, 0.0);
@ -283,22 +283,22 @@ void Simplex::computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const {
// If the current point is part of the simplex // If the current point is part of the simplex
if (overlap(bitsCurrentSimplex, bit)) { if (overlap(bitsCurrentSimplex, bit)) {
deltaX += det[bitsCurrentSimplex][i]; deltaX += det[bitsCurrentSimplex][i];
pA = pA + det[bitsCurrentSimplex][i] * suppPointsA[i]; pA += det[bitsCurrentSimplex][i] * suppPointsA[i];
pB = pB + det[bitsCurrentSimplex][i] * suppPointsB[i]; pB += det[bitsCurrentSimplex][i] * suppPointsB[i];
} }
} }
assert(deltaX > 0.0); assert(deltaX > 0.0);
double factor = 1.0 / deltaX; double factor = 1.0 / deltaX;
pA = factor * pA; pA *= factor;
pB = factor * pB; pB *= factor;
} }
// Compute the closest point "v" to the origin of the current simplex // Compute the closest point "v" to the origin of the current simplex
// This method executes the Jonhnson's algorithm for computing the point // This method executes the Jonhnson's algorithm for computing the point
// "v" of simplex that is closest to the origin. The method returns true // "v" of simplex that is closest to the origin. The method returns true
// if a closest point has been found. // if a closest point has been found.
bool Simplex::computeClosestPoint(Vector3D& v) { bool Simplex::computeClosestPoint(Vector3& v) {
Bits subset; Bits subset;
// For each possible simplex set // For each possible simplex set
@ -325,13 +325,13 @@ bool Simplex::computeClosestPoint(Vector3D& v) {
} }
// Backup the closest point // Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3D& v) { void Simplex::backupClosestPointInSimplex(Vector3& v) {
double minDistSquare = DBL_MAX; double minDistSquare = DBL_MAX;
Bits bit; Bits bit;
for (bit = allBits; bit != 0x0; bit--) { for (bit = allBits; bit != 0x0; bit--) {
if (isSubset(bit, allBits) && isProperSubset(bit)) { if (isSubset(bit, allBits) && isProperSubset(bit)) {
Vector3D u = computeClosestPointForSubset(bit); Vector3 u = computeClosestPointForSubset(bit);
double distSquare = u.dot(u); double distSquare = u.dot(u);
if (distSquare < minDistSquare) { if (distSquare < minDistSquare) {
minDistSquare = distSquare; minDistSquare = distSquare;
@ -344,8 +344,8 @@ void Simplex::backupClosestPointInSimplex(Vector3D& v) {
// Return the closest point "v" in the convex hull of the points in the subset // Return the closest point "v" in the convex hull of the points in the subset
// represented by the bits "subset" // represented by the bits "subset"
Vector3D Simplex::computeClosestPointForSubset(Bits subset) { Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3D v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
maxLengthSquare = 0.0; maxLengthSquare = 0.0;
double deltaX = 0.0; // deltaX = sum of all det[subset][i] double deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i; int i;
@ -363,7 +363,7 @@ Vector3D Simplex::computeClosestPointForSubset(Bits subset) {
} }
// Closest point v = sum(lambda_i * points[i]) // Closest point v = sum(lambda_i * points[i])
v = v + det[subset][i] * points[i]; v += det[subset][i] * points[i];
} }
} }

22
src/collision/GJK/Simplex.h
View File

@ -47,12 +47,12 @@ typedef unsigned int Bits;
*/ */
class Simplex { class Simplex {
private: private:
Vector3D points[4]; // Current points Vector3 points[4]; // Current points
double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2 double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2
double maxLengthSquare; // Maximum length of pointsLengthSquare[i] double maxLengthSquare; // Maximum length of pointsLengthSquare[i]
Vector3D suppPointsA[4]; // Support points of object A in local coordinates Vector3 suppPointsA[4]; // Support points of object A in local coordinates
Vector3D suppPointsB[4]; // Support points of object B in local coordinates Vector3 suppPointsB[4]; // Support points of object B in local coordinates
Vector3D diffLength[4][4]; // diff[i][j] contains points[i] - points[j] Vector3 diffLength[4][4]; // diff[i][j] contains points[i] - points[j]
double det[16][4]; // Cached determinant values double det[16][4]; // Cached determinant values
double normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2 double normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2
Bits bitsCurrentSimplex; // 4 bits that identify the current points of the simplex Bits bitsCurrentSimplex; // 4 bits that identify the current points of the simplex
@ -67,7 +67,7 @@ class Simplex {
bool isProperSubset(Bits subset) const; // Return true if the subset is a proper subset bool isProperSubset(Bits subset) const; // Return true if the subset is a proper subset
void updateCache(); // Update the cached values used during the GJK algorithm void updateCache(); // Update the cached values used during the GJK algorithm
void computeDeterminants(); // Compute the cached determinant values void computeDeterminants(); // Compute the cached determinant values
Vector3D computeClosestPointForSubset(Bits subset); // Return the closest point "v" in the convex hull of a subset of points Vector3 computeClosestPointForSubset(Bits subset); // Return the closest point "v" in the convex hull of a subset of points
public: public:
Simplex(); // Constructor Simplex(); // Constructor
@ -75,14 +75,14 @@ class Simplex {
bool isFull() const; // Return true if the simplex contains 4 points bool isFull() const; // Return true if the simplex contains 4 points
bool isEmpty() const; // Return true if the simple is empty bool isEmpty() const; // Return true if the simple is empty
unsigned int getSimplex(Vector3D* suppPointsA, Vector3D* suppPointsB, Vector3D* points) const; // Return the points of the simplex 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 double getMaxLengthSquareOfAPoint() const; // Return the maximum squared length of a point
void addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB); // Addd a point to the simplex void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3D& point) const; // Return true if the point is in 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 bool isAffinelyDependent() const; // Return true if the set is affinely dependent
void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point void backupClosestPointInSimplex(Vector3& point); // Backup the closest point
void computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const; // Compute the closest points of object A and B void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; // Compute the closest points of object A and B
bool computeClosestPoint(Vector3D& v); // Compute the closest point to the origin of the current simplex bool computeClosestPoint(Vector3& v); // Compute the closest point to the origin of the current simplex
}; };
// Return true if some bits of "a" overlap with bits of "b" // Return true if some bits of "a" overlap with bits of "b"

4
src/collision/SAPAlgorithm.cpp
View File

@ -84,7 +84,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
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 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
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
vector<const AABB*>::iterator it2; // Second iterator vector<const AABB*>::iterator it2; // Second iterator
Vector3D center3D; // Center of the current AABB Vector3 center3D; // Center of the current AABB
double center[3]; // Coordinates of the center of the current AABB double center[3]; // Coordinates of the center of the current AABB
int i; int i;
const Body* body; // Body pointer on the body corresponding to an AABB const Body* body; // Body pointer on the body corresponding to an AABB
@ -123,7 +123,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
// Test collision against all possible overlapping AABBs following the current one // Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) { for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) {
// Stop when the tested AABBs are beyond the end of the current AABB // Stop when the tested AABBs are beyond the end of the current AABB
if ((*it2)->getMinCoordinates().getValue(sortAxis) > (*it)->getMaxCoordinates().getValue(sortAxis)) { if ((*it2)->getMinCoordinates()[sortAxis] > (*it)->getMaxCoordinates()[sortAxis]) {
break; break;
} }

2
src/collision/SAPAlgorithm.h
View File

@ -72,7 +72,7 @@ class SAPAlgorithm : public BroadPhaseAlgorithm {
// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same // sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
// axis. // axis.
inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) { inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) {
return (a->getMinCoordinates().getValue(sortAxis) < b->getMinCoordinates().getValue(sortAxis)); return (a->getMinCoordinates()[sortAxis] < b->getMinCoordinates()[sortAxis]);
} }
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

4
src/collision/SATAlgorithm.cpp
View File

@ -92,7 +92,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
double max1; // Maximum of interval 1 double max1; // Maximum of interval 1
double min2; // Minimm of interval 2 double min2; // Minimm of interval 2
double max2; // Maximum of interval 2 double max2; // Maximum of interval 2
Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth) Vector3 normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth)
// The contact normal point out of OBB1 toward OBB2 // The contact normal point out of OBB1 toward OBB2
double minPenetrationDepth = DBL_MAX; // Minimum penetration depth detected among all separated axis double minPenetrationDepth = DBL_MAX; // Minimum penetration depth detected among all separated axis
const double cutoff = 0.99; // Cutoff for cosine of angles between box axes const double cutoff = 0.99; // Cutoff for cosine of angles between box axes
@ -109,7 +109,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center) double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center)
//Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs //Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs
Vector3D boxDistance = transform2.getPosition() - transform1.getPosition(); // Vector between the centers of the OBBs Vector3 boxDistance = transform2.getPosition() - transform1.getPosition(); // Vector between the centers of the OBBs
// Axis A0 // Axis A0
for (int i=0; i<3; ++i) { for (int i=0; i<3; ++i) {

6
src/collision/SATAlgorithm.h
View File

@ -54,7 +54,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
const BoxShape* obb2, const Transform& transform2, const BoxShape* obb2, const Transform& transform2,
ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide
double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal Vector3 computeContactNormal(const Vector3& axis, const Vector3& distanceOfOBBs) const; // Compute a contact normal
public : public :
SATAlgorithm(); // Constructor SATAlgorithm(); // Constructor
@ -65,12 +65,12 @@ class SATAlgorithm : public NarrowPhaseAlgorithm {
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the
// collision occurs and "distanceOfOBBs" is the vector (obb2.center - obb1.center). // collision occurs and "distanceOfOBBs" is the vector (obb2.center - obb1.center).
inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { inline Vector3 SATAlgorithm::computeContactNormal(const Vector3& axis, const Vector3& distanceOfOBBs) const {
if (distanceOfOBBs.dot(axis) >= 0.0) { if (distanceOfOBBs.dot(axis) >= 0.0) {
return axis; return axis;
} }
else { else {
return axis.getOpposite(); return -axis;
} }
} }

24
src/constraint/Contact.cpp
View File

@ -56,14 +56,14 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
assert(body1); assert(body1);
assert(body2); assert(body2);
Vector3D body1Position = body1->getTransform().getPosition(); Vector3 body1Position = body1->getTransform().getPosition();
Vector3D body2Position = body2->getTransform().getPosition(); Vector3 body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index int currentIndex = noConstraint; // Current constraint index
Vector3D r1 = pointOnBody1 - body1Position; Vector3 r1 = pointOnBody1 - body1Position;
Vector3D r2 = pointOnBody2 - body2Position; Vector3 r2 = pointOnBody2 - body2Position;
Vector3D r1CrossN = r1.cross(normal); Vector3 r1CrossN = r1.cross(normal);
Vector3D r2CrossN = r2.cross(normal); Vector3 r2CrossN = r2.cross(normal);
// Compute the jacobian matrix for the body 1 for the contact constraint // Compute the jacobian matrix for the body 1 for the contact constraint
J_sp[currentIndex][0].setValue(0, -normal.getX()); J_sp[currentIndex][0].setValue(0, -normal.getX());
@ -84,10 +84,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
currentIndex++; currentIndex++;
// Compute the jacobian matrix for the body 1 for the first friction constraint // Compute the jacobian matrix for the body 1 for the first friction constraint
Vector3D r1CrossU1 = r1.cross(frictionVectors[0]); Vector3 r1CrossU1 = r1.cross(frictionVectors[0]);
Vector3D r2CrossU1 = r2.cross(frictionVectors[0]); Vector3 r2CrossU1 = r2.cross(frictionVectors[0]);
Vector3D r1CrossU2 = r1.cross(frictionVectors[1]); Vector3 r1CrossU2 = r1.cross(frictionVectors[1]);
Vector3D r2CrossU2 = r2.cross(frictionVectors[1]); Vector3 r2CrossU2 = r2.cross(frictionVectors[1]);
J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX()); J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY()); J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ()); J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ());
@ -157,8 +157,8 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent()); assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent());
// Compute the error value for the contact constraint // Compute the error value for the contact constraint
Vector3D velocity1 = rigidBody1->getLinearVelocity(); Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3D velocity2 = rigidBody2->getLinearVelocity(); Vector3 velocity2 = rigidBody2->getLinearVelocity();
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth; double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth;

22
src/constraint/Contact.h
View File

@ -51,11 +51,11 @@ namespace reactphysics3d {
*/ */
class Contact : public Constraint { class Contact : public Constraint {
protected : protected :
const Vector3D normal; // Normal vector of the contact (From body1 toward body2) const Vector3 normal; // Normal vector of the contact (From body1 toward body2)
const double penetrationDepth; // Penetration depth const double penetrationDepth; // Penetration depth
const Vector3D pointOnBody1; // Contact point on body 1 const Vector3 pointOnBody1; // Contact point on body 1
const Vector3D pointOnBody2; // Contact point on body 2 const Vector3 pointOnBody2; // Contact point on body 2
std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
double mu_mc_g; double mu_mc_g;
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
@ -64,9 +64,9 @@ class Contact : public Constraint {
Contact(const ContactInfo* contactInfo); // Constructor Contact(const ContactInfo* contactInfo); // Constructor
virtual ~Contact(); // Destructor virtual ~Contact(); // Destructor
Vector3D getNormal() const; // Return the normal vector of the contact Vector3 getNormal() const; // Return the normal vector of the contact
Vector3D getPointOnBody1() const; // Return the contact point on body 1 Vector3 getPointOnBody1() const; // Return the contact point on body 1
Vector3D getPointOnBody2() const; // Return the contact point on body 2 Vector3 getPointOnBody2() const; // Return the contact point on body 2
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints
@ -84,7 +84,7 @@ inline void Contact::computeFrictionVectors() {
frictionVectors.clear(); frictionVectors.clear();
// Compute the first orthogonal vector // Compute the first orthogonal vector
Vector3D vector1 = normal.getOneOrthogonalVector(); Vector3 vector1 = normal.getOneOrthogonalVector();
frictionVectors.push_back(vector1); frictionVectors.push_back(vector1);
// Compute the second orthogonal vector using the cross product // Compute the second orthogonal vector using the cross product
@ -92,17 +92,17 @@ inline void Contact::computeFrictionVectors() {
} }
// Return the normal vector of the contact // Return the normal vector of the contact
inline Vector3D Contact::getNormal() const { inline Vector3 Contact::getNormal() const {
return normal; return normal;
} }
// Return the contact point on body 1 // Return the contact point on body 1
inline Vector3D Contact::getPointOnBody1() const { inline Vector3 Contact::getPointOnBody1() const {
return pointOnBody1; return pointOnBody1;
} }
// Return the contact point on body 2 // Return the contact point on body 2
inline Vector3D Contact::getPointOnBody2() const { inline Vector3 Contact::getPointOnBody2() const {
return pointOnBody2; return pointOnBody2;
} }

32
src/engine/ConstraintSolver.cpp
View File

@ -241,25 +241,29 @@ void ConstraintSolver::fillInMatrices() {
// TODO : Use polymorphism and remove this downcasting // TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body); rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody); assert(rigidBody);
// Compute the vector V1 with initial velocities values // Compute the vector V1 with initial velocities values
V1[bodyNumber].setValue(0, rigidBody->getLinearVelocity().getValue(0)); Vector3 linearVelocity = rigidBody->getLinearVelocity();
V1[bodyNumber].setValue(1, rigidBody->getLinearVelocity().getValue(1)); Vector3 angularVelocity = rigidBody->getAngularVelocity();
V1[bodyNumber].setValue(2, rigidBody->getLinearVelocity().getValue(2)); V1[bodyNumber].setValue(0, linearVelocity[0]);
V1[bodyNumber].setValue(3, rigidBody->getAngularVelocity().getValue(0)); V1[bodyNumber].setValue(1, linearVelocity[1]);
V1[bodyNumber].setValue(4, rigidBody->getAngularVelocity().getValue(1)); V1[bodyNumber].setValue(2, linearVelocity[2]);
V1[bodyNumber].setValue(5, rigidBody->getAngularVelocity().getValue(2)); V1[bodyNumber].setValue(3, angularVelocity[0]);
V1[bodyNumber].setValue(4, angularVelocity[1]);
V1[bodyNumber].setValue(5, angularVelocity[2]);
// Compute the vector Vconstraint with final constraint velocities // Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyNumber].initWithValue(0.0); Vconstraint[bodyNumber].initWithValue(0.0);
// Compute the vector with forces and torques values // Compute the vector with forces and torques values
Fext[bodyNumber].setValue(0, rigidBody->getExternalForce().getValue(0)); Vector3 externalForce = rigidBody->getExternalForce();
Fext[bodyNumber].setValue(1, rigidBody->getExternalForce().getValue(1)); Vector3 externalTorque = rigidBody->getExternalTorque();
Fext[bodyNumber].setValue(2, rigidBody->getExternalForce().getValue(2)); Fext[bodyNumber].setValue(0, externalForce[0]);
Fext[bodyNumber].setValue(3, rigidBody->getExternalTorque().getValue(0)); Fext[bodyNumber].setValue(1, externalForce[1]);
Fext[bodyNumber].setValue(4, rigidBody->getExternalTorque().getValue(1)); Fext[bodyNumber].setValue(2, externalForce[2]);
Fext[bodyNumber].setValue(5, rigidBody->getExternalTorque().getValue(2)); Fext[bodyNumber].setValue(3, externalTorque[0]);
Fext[bodyNumber].setValue(4, externalTorque[1]);
Fext[bodyNumber].setValue(5, externalTorque[2]);
// Compute the inverse sparse mass matrix // Compute the inverse sparse mass matrix
Minv_sp[bodyNumber].initWithValue(0.0); Minv_sp[bodyNumber].initWithValue(0.0);
@ -351,7 +355,7 @@ void ConstraintSolver::updateContactCache() {
if (contact) { if (contact) {
// Get all the contact points of the contact // Get all the contact points of the contact
vector<Vector3D> points; vector<Vector3> points;
vector<double> lambdas; vector<double> lambdas;
points.push_back(contact->getPointOnBody1()); points.push_back(contact->getPointOnBody1());
points.push_back(contact->getPointOnBody2()); points.push_back(contact->getPointOnBody2());

12
src/engine/ConstraintSolver.h
View File

@ -98,8 +98,8 @@ class ConstraintSolver {
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem 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 bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3D getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3D getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular 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(); void cleanup();
}; };
@ -112,18 +112,18 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
} }
// Return the constrained linear velocity of a body after solving the LCP problem // Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]]; const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2)); return Vector3(vec.getValue(0), vec.getValue(1), vec.getValue(2));
} }
// Return the constrained angular velocity of a body after solving the LCP problem // Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]]; const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
return Vector3D(vec.getValue(3), vec.getValue(4), vec.getValue(5)); return Vector3(vec.getValue(3), vec.getValue(4), vec.getValue(5));
} }
// Cleanup of the constraint solver // Cleanup of the constraint solver

4
src/engine/ContactCache.cpp
View File

@ -71,7 +71,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
posZ = contact->getPointOnBody1().getZ(); posZ = contact->getPointOnBody1().getZ();
// Get the position of the old contact // Get the position of the old contact
Vector3D& contactPos1 = contactInfo->positions[0]; Vector3& contactPos1 = contactInfo->positions[0];
// If the old contact point doesn't match the current one // If the old contact point doesn't match the current one
if (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE || if (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE ||
@ -88,7 +88,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
posZ = contact->getPointOnBody2().getZ(); posZ = contact->getPointOnBody2().getZ();
// Get the position of the old contact // Get the position of the old contact
Vector3D& contactPos2 = contactInfo->positions[1]; Vector3& contactPos2 = contactInfo->positions[1];
// If the old contact point doesn't match the current one // If the old contact point doesn't match the current one
if (posX > contactPos2.getX() + POSITION_TOLERANCE || posX < contactPos2.getX() - POSITION_TOLERANCE || if (posX > contactPos2.getX() + POSITION_TOLERANCE || posX < contactPos2.getX() - POSITION_TOLERANCE ||

2
src/engine/ContactCachingInfo.cpp
View File

@ -29,7 +29,7 @@ using namespace reactphysics3d;
using namespace std; using namespace std;
// Constructor // Constructor
ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector<Vector3D>& positions, const vector<double>& lambdas) ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector<Vector3>& positions, const vector<double>& lambdas)
: body1(body1), body2(body2), positions(positions), lambdas(lambdas) { : body1(body1), body2(body2), positions(positions), lambdas(lambdas) {
} }

4
src/engine/ContactCachingInfo.h
View File

@ -43,10 +43,10 @@ struct ContactCachingInfo {
public: public:
Body* body1; // Body pointer of the first bounding volume Body* body1; // Body pointer of the first bounding volume
Body* body2; // Body pointer of the second bounding volume Body* body2; // Body pointer of the second bounding volume
std::vector<Vector3D> positions; // Positions of the contact points std::vector<Vector3> positions; // Positions of the contact points
std::vector<double> lambdas; // Last lambdas value for the constraint std::vector<double> lambdas; // Last lambdas value for the constraint
ContactCachingInfo(Body* body1, Body* body2, const std::vector<Vector3D>& positions, const std::vector<double>& lambdas); // Constructor ContactCachingInfo(Body* body1, Body* body2, const std::vector<Vector3>& positions, const std::vector<double>& lambdas); // Constructor
}; };
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

99
src/engine/PhysicsEngine.cpp
View File

@ -30,19 +30,10 @@ using namespace reactphysics3d;
using namespace std; using namespace std;
// Constructor // Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_TIMESTEP) throw (invalid_argument) PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_TIMESTEP)
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) { : world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) {
// Check if the pointer to the world is not NULL assert(world);
if (world == 0) { assert(timeStep > 0.0);
// Throw an exception
throw invalid_argument("Error : The argument world to the PhysicsEngine constructor cannot be NULL");
}
// Check if the timeStep is positive
if (timeStep <= 0.0) {
// Throw an exception
throw invalid_argument("Error : The timeStep argument to the PhysicsEngine constructor have to be greater than zero");
}
} }
// Destructor // Destructor
@ -51,52 +42,46 @@ PhysicsEngine::~PhysicsEngine() {
} }
// Update the physics simulation // Update the physics simulation
void PhysicsEngine::update() throw (logic_error) { void PhysicsEngine::update() {
bool existCollision = false; bool existCollision = false;
// Check that the timer is running assert(timer.getIsRunning());
if (timer.getIsRunning()) {
// Compute the time since the last update() call and update the timer // Compute the time since the last update() call and update the timer
timer.update(); timer.update();
// Apply the gravity force to all bodies // Apply the gravity force to all bodies
applyGravity(); applyGravity();
// While the time accumulator is not empty // While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) { while(timer.isPossibleToTakeStep()) {
existCollision = false; existCollision = false;
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
// Solve constraints // Compute the collision detection
constraintSolver.solve(timer.getTimeStep()); if (collisionDetection.computeCollisionDetection()) {
} existCollision = true;
// Update the timer // Solve constraints
timer.nextStep(); constraintSolver.solve(timer.getTimeStep());
// Update the position and orientation of each body
updateAllBodiesMotion();
// Cleanup of the constraint solver
if (existCollision) {
constraintSolver.cleanup();
}
// Clear the added and removed bodies from last update() method call
world->clearAddedAndRemovedBodies();
} }
// Compute and set the interpolation factor to all the bodies // Update the timer
setInterpolationFactorToAllBodies(); timer.nextStep();
}
else { // Call to update() but the timer is not running // Update the position and orientation of each body
// Throw an exception updateAllBodiesMotion();
throw logic_error("Error : The PhysicsEngine::start() method have to be called before calling PhysicsEngine::update()");
// Cleanup of the constraint solver
if (existCollision) {
constraintSolver.cleanup();
}
// Clear the added and removed bodies from last update() method call
world->clearAddedAndRemovedBodies();
} }
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
} }
// Compute the motion of all bodies and update their positions and orientations // Compute the motion of all bodies and update their positions and orientations
@ -110,8 +95,8 @@ void PhysicsEngine::update() throw (logic_error) {
// orientation of the body // orientation of the body
void PhysicsEngine::updateAllBodiesMotion() { void PhysicsEngine::updateAllBodiesMotion() {
double dt = timer.getTimeStep(); double dt = timer.getTimeStep();
Vector3D newLinearVelocity; Vector3 newLinearVelocity;
Vector3D newAngularVelocity; Vector3 newAngularVelocity;
// For each body of thephysics world // For each body of thephysics world
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) { for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
@ -133,12 +118,12 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
// external forces and torques. // external forces and torques.
newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Add the velocity V1 to the new velocity // Add the velocity V1 to the new velocity
newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity(); newLinearVelocity += rigidBody->getLinearVelocity();
newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity(); newAngularVelocity += rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity // Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
@ -152,7 +137,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Update the position and orientation of a body // Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body // orientation of the body
void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) { void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity) {
double dt = timer.getTimeStep(); double dt = timer.getTimeStep();
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body); RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
@ -166,10 +151,10 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
rigidBody->setAngularVelocity(newAngVelocity); rigidBody->setAngularVelocity(newAngVelocity);
// Get current position and orientation of the body // Get current position and orientation of the body
const Vector3D& currentPosition = rigidBody->getTransform().getPosition(); const Vector3& currentPosition = rigidBody->getTransform().getPosition();
const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation();
Vector3D newPosition = currentPosition + newLinVelocity * dt; Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit()); Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform); rigidBody->setTransform(newTransform);

12
src/engine/PhysicsEngine.h
View File

@ -50,17 +50,17 @@ class PhysicsEngine {
ConstraintSolver constraintSolver; // Constraint solver ConstraintSolver constraintSolver; // Constraint solver
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body void updatePositionAndOrientationOfBody(Body* 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 setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies
void applyGravity(); // Apply the gravity force to all bodies void applyGravity(); // Apply the gravity force to all bodies
public : public :
PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor PhysicsEngine(PhysicsWorld* world, double timeStep); // Constructor
~PhysicsEngine(); // Destructor ~PhysicsEngine(); // Destructor
void start(); // Start the physics simulation void start(); // Start the physics simulation
void stop(); // Stop the physics simulation void stop(); // Stop the physics simulation
void update() throw (std::logic_error); // Update the physics simulation void update(); // Update the physics simulation
}; };
// --- Inline functions --- // // --- Inline functions --- //

2
src/engine/PhysicsWorld.cpp
View File

@ -31,7 +31,7 @@ using namespace reactphysics3d;
using namespace std; using namespace std;
// Constructor // Constructor
PhysicsWorld::PhysicsWorld(const Vector3D& gravity) PhysicsWorld::PhysicsWorld(const Vector3& gravity)
: gravity(gravity), isGravityOn(true) { : gravity(gravity), isGravityOn(true) {
} }

8
src/engine/PhysicsWorld.h
View File

@ -49,17 +49,17 @@ class PhysicsWorld {
std::vector<Body*> addedBodies; // Added bodies since last update std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update std::vector<Body*> removedBodies; // Removed bodies since last update
std::vector<Constraint*> constraints; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3D gravity; // Gravity vector of the world Vector3 gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on bool isGravityOn; // True if the gravity force is on
public : public :
PhysicsWorld(const Vector3D& gravity); // Constructor PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor virtual ~PhysicsWorld(); // Destructor
void addBody(Body* body); // Add a body to the physics world void addBody(Body* body); // Add a body to the physics world
void removeBody(Body const* const body); // Remove a body from the physics world void removeBody(Body const* const body); // Remove a body from the physics world
void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
Vector3D getGravity() const; // Return the gravity vector of the world Vector3 getGravity() const; // Return the gravity vector of the world
bool getIsGravityOn() const; // Return if the gravity is on bool getIsGravityOn() const; // Return if the gravity is on
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
void addConstraint(Constraint* constraint); // Add a constraint void addConstraint(Constraint* constraint); // Add a constraint
@ -129,7 +129,7 @@ inline void PhysicsWorld::clearAddedAndRemovedBodies() {
} }
// Return the gravity vector of the world // Return the gravity vector of the world
inline Vector3D PhysicsWorld::getGravity() const { inline Vector3 PhysicsWorld::getGravity() const {
return gravity; return gravity;
} }

13
src/mathematics/Matrix3x3.cpp
View File

@ -70,18 +70,5 @@ Matrix3x3 Matrix3x3::getInverse() const {
return (invDeterminant * tempMatrix.getTranspose()); return (invDeterminant * tempMatrix.getTranspose());
} }
// Overloaded operator for assignment
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) {
// Check for self-assignment
if (this != &matrix2) {
setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2],
matrix2.array[1][0], matrix2.array[1][1], matrix2.array[1][2],
matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
}
// Return a reference to the matrix
return *this;
}

160
src/mathematics/Matrix3x3.h
View File

@ -28,7 +28,7 @@
// Libraries // Libraries
#include <cassert> #include <cassert>
#include "Vector3D.h" #include "Vector3.h"
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -54,7 +54,7 @@ class Matrix3x3 {
void setValue(int i, int j, double value); // Set 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, 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 double c1, double c2, double c3); // Set all the values in the matrix
Vector3D getColumn(int i) const; // Return a column Vector3 getColumn(int i) const; // Return a column
Matrix3x3 getTranspose() const; // Return the transpose matrix Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix double getTrace() const; // Return the trace of the matrix
@ -64,13 +64,19 @@ class Matrix3x3 {
static Matrix3x3 identity(); // Return the 3x3 identity matrix static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- // // --- Overloaded operators --- //
Matrix3x3 operator+(const Matrix3x3& matrix2) const; // Overloaded operator for addition friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for addition
Matrix3x3 operator-(const Matrix3x3& matrix2) const ; // Overloaded operator for substraction friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for substraction
Matrix3x3 operator*(double nb) const; // Overloaded operator for multiplication with a number friend Matrix3x3 operator-(const Matrix3x3& matrix); // Overloaded operator for the negative of the matrix
Matrix3x3 operator*(const Matrix3x3& matrix2) const; // Overloaded operator for multiplication with a matrix friend Matrix3x3 operator*(double nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number
Vector3D operator*(const Vector3D& vector3d) const; // Overloaded operator for multiplication with a vector friend Matrix3x3 operator*(const Matrix3x3& matrix, double nb); // Overloaded operator for multiplication with a matrix
Matrix3x3& operator=(const Matrix3x3& matrix2); // Overloaded operator for assignment friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for matrix multiplication
bool operator==(const Matrix3x3& matrix2) const; // Overloaded operator for equality condition friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); // Overloaded operator for multiplication with a vector
bool operator==(const Matrix3x3& matrix) const; // Overloaded operator for equality condition
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
}; };
@ -88,16 +94,16 @@ inline void Matrix3x3::setValue(int i, int j, double value) {
// Method to set all the values in the matrix // 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, inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3) { double c1, double c2, double c3) {
array[0][0] = a1; array[0][1] = a2; array[0][2] = a3; 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[1][0] = b1; array[1][1] = b2; array[1][2] = b3;
array[2][0] = c1; array[2][1] = c2; array[2][2] = c3; array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
} }
// Return a column // Return a column
inline Vector3D Matrix3x3::getColumn(int i) const { inline Vector3 Matrix3x3::getColumn(int i) const {
assert(i>= 0 && i<3); assert(i>= 0 && i<3);
return Vector3D(array[0][i], array[1][i], array[2][i]); return Vector3(array[0][i], array[1][i], array[2][i]);
} }
// Return the transpose matrix // Return the transpose matrix
@ -121,27 +127,6 @@ inline double Matrix3x3::getTrace() const {
return (array[0][0] + array[1][1] + array[2][2]); return (array[0][0] + array[1][1] + array[2][2]);
} }
// Overloaded operator for multiplication between a number and a Matrix3x3 (inline)
inline Matrix3x3 operator*(double number, const Matrix3x3& matrix) {
// Return the multiplied matrix
return matrix * number;
}
// Overloaded operator for multiplication with a vector
inline Vector3D Matrix3x3::operator*(const Vector3D& vector3d) const {
// Compute and return the result
return Vector3D(array[0][0]*vector3d.getX() + array[0][1]*vector3d.getY() + array[0][2]*vector3d.getZ(),
array[1][0]*vector3d.getX() + array[1][1]*vector3d.getY() + array[1][2]*vector3d.getZ(),
array[2][0]*vector3d.getX() + array[2][1]*vector3d.getY() + array[2][2]*vector3d.getZ());
}
// Overloaded operator for equality condition
inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const {
return (array[0][0] == matrix2.array[0][0] && array[0][1] == matrix2.array[0][1] && array[0][2] == matrix2.array[0][2] &&
array[1][0] == matrix2.array[1][0] && array[1][1] == matrix2.array[1][1] && array[1][2] == matrix2.array[1][2] &&
array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]);
}
// Set the matrix to the identity matrix // Set the matrix to the identity matrix
inline void Matrix3x3::setToIdentity() { inline void Matrix3x3::setToIdentity() {
array[0][0] = 1.0; array[0][1] = 0.0; array[0][2] = 0.0; array[0][0] = 1.0; array[0][1] = 0.0; array[0][2] = 0.0;
@ -162,42 +147,93 @@ inline Matrix3x3 Matrix3x3::getAbsoluteMatrix() const {
fabs(array[2][0]), fabs(array[2][1]), fabs(array[2][2])); fabs(array[2][0]), fabs(array[2][1]), fabs(array[2][2]));
} }
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
// Compute and return the multiplication of the matrices
return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
}
// Overloaded operator for addition // Overloaded operator for addition
inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const { inline Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
// Return the sum matrix return Matrix3x3(matrix1.array[0][0] + matrix2.array[0][0], matrix1.array[0][1] + matrix2.array[0][1], matrix1.array[0][2] + matrix2.array[0][2],
return Matrix3x3(array[0][0] + matrix2.array[0][0], array[0][1] + matrix2.array[0][1], array[0][2] + matrix2.array[0][2], matrix1.array[1][0] + matrix2.array[1][0], matrix1.array[1][1] + matrix2.array[1][1], matrix1.array[1][2] + matrix2.array[1][2],
array[1][0] + matrix2.array[1][0], array[1][1] + matrix2.array[1][1], array[1][2] + matrix2.array[1][2], matrix1.array[2][0] + matrix2.array[2][0], matrix1.array[2][1] + matrix2.array[2][1], matrix1.array[2][2] + matrix2.array[2][2]);
array[2][0] + matrix2.array[2][0], array[2][1] + matrix2.array[2][1], array[2][2] + matrix2.array[2][2]);
} }
// Overloaded operator for substraction // Overloaded operator for substraction
inline Matrix3x3 Matrix3x3::operator-(const Matrix3x3& matrix2) const { inline Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
// Return the substraction matrix return Matrix3x3(matrix1.array[0][0] - matrix2.array[0][0], matrix1.array[0][1] - matrix2.array[0][1], matrix1.array[0][2] - matrix2.array[0][2],
return Matrix3x3(array[0][0] - matrix2.array[0][0], array[0][1] - matrix2.array[0][1], array[0][2] - matrix2.array[0][2], matrix1.array[1][0] - matrix2.array[1][0], matrix1.array[1][1] - matrix2.array[1][1], matrix1.array[1][2] - matrix2.array[1][2],
array[1][0] - matrix2.array[1][0], array[1][1] - matrix2.array[1][1], array[1][2] - matrix2.array[1][2], matrix1.array[2][0] - matrix2.array[2][0], matrix1.array[2][1] - matrix2.array[2][1], matrix1.array[2][2] - matrix2.array[2][2]);
array[2][0] - matrix2.array[2][0], array[2][1] - matrix2.array[2][1], array[2][2] - matrix2.array[2][2]); }
// Overloaded operator for the negative of the matrix
inline Matrix3x3 operator-(const Matrix3x3& matrix) {
return Matrix3x3(-matrix.array[0][0], -matrix.array[0][1], -matrix.array[0][2],
-matrix.array[1][0], -matrix.array[1][1], -matrix.array[1][2],
-matrix.array[2][0], -matrix.array[2][1], -matrix.array[2][2]);
} }
// Overloaded operator for multiplication with a number // Overloaded operator for multiplication with a number
inline Matrix3x3 Matrix3x3::operator*(double nb) const { inline Matrix3x3 operator*(double nb, const Matrix3x3& matrix) {
// Return multiplied matrix return Matrix3x3(matrix.array[0][0] * nb, matrix.array[0][1] * nb, matrix.array[0][2] * nb,
return Matrix3x3(array[0][0] * nb, array[0][1] * nb, array[0][2] * nb, matrix.array[1][0] * nb, matrix.array[1][1] * nb, matrix.array[1][2] * nb,
array[1][0] * nb, array[1][1] * nb, array[1][2] * nb, matrix.array[2][0] * nb, matrix.array[2][1] * nb, matrix.array[2][2] * nb);
array[2][0] * nb, array[2][1] * nb, array[2][2] * nb); }
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 operator*(const Matrix3x3& matrix, double nb) {
return nb * matrix;
}
// Overloaded operator for matrix multiplication
inline Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2) {
return Matrix3x3(matrix1.array[0][0]*matrix2.array[0][0] + matrix1.array[0][1]*matrix2.array[1][0] + matrix1.array[0][2]*matrix2.array[2][0],
matrix1.array[0][0]*matrix2.array[0][1] + matrix1.array[0][1]*matrix2.array[1][1] + matrix1.array[0][2]*matrix2.array[2][1],
matrix1.array[0][0]*matrix2.array[0][2] + matrix1.array[0][1]*matrix2.array[1][2] + matrix1.array[0][2]*matrix2.array[2][2],
matrix1.array[1][0]*matrix2.array[0][0] + matrix1.array[1][1]*matrix2.array[1][0] + matrix1.array[1][2]*matrix2.array[2][0],
matrix1.array[1][0]*matrix2.array[0][1] + matrix1.array[1][1]*matrix2.array[1][1] + matrix1.array[1][2]*matrix2.array[2][1],
matrix1.array[1][0]*matrix2.array[0][2] + matrix1.array[1][1]*matrix2.array[1][2] + matrix1.array[1][2]*matrix2.array[2][2],
matrix1.array[2][0]*matrix2.array[0][0] + matrix1.array[2][1]*matrix2.array[1][0] + matrix1.array[2][2]*matrix2.array[2][0],
matrix1.array[2][0]*matrix2.array[0][1] + matrix1.array[2][1]*matrix2.array[1][1] + matrix1.array[2][2]*matrix2.array[2][1],
matrix1.array[2][0]*matrix2.array[0][2] + matrix1.array[2][1]*matrix2.array[1][2] + matrix1.array[2][2]*matrix2.array[2][2]);
}
// Overloaded operator for multiplication with a vector
inline Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector) {
return Vector3(matrix.array[0][0]*vector.getX() + matrix.array[0][1]*vector.getY() + matrix.array[0][2]*vector.getZ(),
matrix.array[1][0]*vector.getX() + matrix.array[1][1]*vector.getY() + matrix.array[1][2]*vector.getZ(),
matrix.array[2][0]*vector.getX() + matrix.array[2][1]*vector.getY() + matrix.array[2][2]*vector.getZ());
}
// Overloaded operator for equality condition
inline bool Matrix3x3::operator==(const Matrix3x3& matrix) const {
return (array[0][0] == matrix.array[0][0] && array[0][1] == matrix.array[0][1] && array[0][2] == matrix.array[0][2] &&
array[1][0] == matrix.array[1][0] && array[1][1] == matrix.array[1][1] && array[1][2] == matrix.array[1][2] &&
array[2][0] == matrix.array[2][0] && array[2][1] == matrix.array[2][1] && array[2][2] == matrix.array[2][2]);
}
// Overloaded operator for the is different condition
inline bool Matrix3x3::operator!= (const Matrix3x3& matrix) const {
return !(*this == matrix);
}
// Overloaded operator for addition with assignment
inline Matrix3x3& Matrix3x3::operator+=(const Matrix3x3& matrix) {
array[0][0] += matrix.array[0][0]; array[0][1] += matrix.array[0][1]; array[0][2] += matrix.array[0][2];
array[1][0] += matrix.array[1][0]; array[1][1] += matrix.array[1][1]; array[1][2] += matrix.array[1][2];
array[2][0] += matrix.array[2][0]; array[2][1] += matrix.array[2][1]; array[2][2] += matrix.array[2][2];
return *this;
}
// Overloaded operator for substraction with assignment
inline Matrix3x3& Matrix3x3::operator-=(const Matrix3x3& matrix) {
array[0][0] -= matrix.array[0][0]; array[0][1] -= matrix.array[0][1]; array[0][2] -= matrix.array[0][2];
array[1][0] -= matrix.array[1][0]; array[1][1] -= matrix.array[1][1]; array[1][2] -= matrix.array[1][2];
array[2][0] -= matrix.array[2][0]; array[2][1] -= matrix.array[2][1]; array[2][2] -= matrix.array[2][2];
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Matrix3x3& Matrix3x3::operator*=(double 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;
return *this;
} }
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

8
src/mathematics/Quaternion.cpp
View File

@ -24,7 +24,7 @@
// Libraries // Libraries
#include "Quaternion.h" #include "Quaternion.h"
#include "Vector3D.h" #include "Vector3.h"
#include <cassert> #include <cassert>
// Namespace // Namespace
@ -43,7 +43,7 @@ Quaternion::Quaternion(double x, double y, double z, double w)
} }
// Constructor with the component w and the vector v=(x y z) // Constructor with the component w and the vector v=(x y z)
Quaternion::Quaternion(double w, const Vector3D& v) Quaternion::Quaternion(double w, const Vector3& v)
:x(v.getX()), y(v.getY()), z(v.getZ()), w(w) { :x(v.getX()), y(v.getY()), z(v.getZ()), w(w) {
} }
@ -134,7 +134,7 @@ Quaternion::~Quaternion() {
// Compute the rotation angle (in radians) and the 3D rotation axis // 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 // This method is used to get the rotation angle (in radian) and the unit
// rotation axis of an orientation quaternion. // rotation axis of an orientation quaternion.
void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const { void Quaternion::getRotationAngleAxis(double& angle, Vector3& axis) const {
Quaternion quaternion; Quaternion quaternion;
// If the quaternion is unit // If the quaternion is unit
@ -150,7 +150,7 @@ void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
angle = acos(quaternion.w) * 2.0; angle = acos(quaternion.w) * 2.0;
// Compute the 3D rotation axis // Compute the 3D rotation axis
Vector3D rotationAxis(quaternion.x, quaternion.y, quaternion.z); Vector3 rotationAxis(quaternion.x, quaternion.y, quaternion.z);
// Normalize the rotation axis // Normalize the rotation axis
rotationAxis = rotationAxis.getUnit(); rotationAxis = rotationAxis.getUnit();

12
src/mathematics/Quaternion.h
View File

@ -27,7 +27,7 @@
// Libraries // Libraries
#include <cmath> #include <cmath>
#include "Vector3D.h" #include "Vector3.h"
#include "Matrix3x3.h" #include "Matrix3x3.h"
#include "exceptions.h" #include "exceptions.h"
@ -50,7 +50,7 @@ class Quaternion {
public : public :
Quaternion(); // Constructor Quaternion(); // Constructor
Quaternion(double x, double y, double z, double w); // Constructor with arguments Quaternion(double x, double y, double z, double w); // Constructor with arguments
Quaternion(double w, const Vector3D& v); // Constructor with the component w and the vector v=(x y z) 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 Quaternion& quaternion); // Copy-constructor
Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor ~Quaternion(); // Destructor
@ -62,7 +62,7 @@ class Quaternion {
void setY(double y); // Set the value y void setY(double y); // Set the value y
void setZ(double z); // Set the value z void setZ(double z); // Set the value z
void setW(double w); // Set the value w void setW(double w); // Set the value w
Vector3D vectorV() const; // Return the vector v=(x y z) of the quaternion Vector3 vectorV() const; // Return the vector v=(x y z) of the quaternion
double length() const; // Return the length of the quaternion double length() const; // Return the length of the quaternion
Quaternion getUnit() const; // Return the unit quaternion Quaternion getUnit() const; // Return the unit quaternion
Quaternion getConjugate() const; // Return the conjugate quaternion Quaternion getConjugate() const; // Return the conjugate quaternion
@ -70,7 +70,7 @@ class Quaternion {
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
static Quaternion identity(); // Return the identity quaternion static Quaternion identity(); // Return the identity quaternion
double dot(const Quaternion& quaternion) const; // Dot product between two quaternions double dot(const Quaternion& quaternion) const; // Dot product between two quaternions
void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis void getRotationAngleAxis(double& angle, Vector3& axis) const; // Compute the rotation angle (in radians) and the axis
static Quaternion slerp(const Quaternion& quaternion1, static Quaternion slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions
@ -126,9 +126,9 @@ inline void Quaternion::setW(double w) {
} }
// Return the vector v=(x y z) of the quaternion // Return the vector v=(x y z) of the quaternion
inline Vector3D Quaternion::vectorV() const { inline Vector3 Quaternion::vectorV() const {
// Return the vector v // Return the vector v
return Vector3D(x, y, z); return Vector3(x, y, z);
} }
// Return the length of the quaternion (inline) // Return the length of the quaternion (inline)

6
src/mathematics/Transform.cpp
View File

@ -30,18 +30,18 @@ using namespace reactphysics3d;
// Constructor // Constructor
Transform::Transform() { Transform::Transform() {
position = Vector3D(0.0, 0.0, 0.0); position = Vector3(0.0, 0.0, 0.0);
orientation = Quaternion::identity(); orientation = Quaternion::identity();
} }
// Constructor // Constructor
Transform::Transform(const Vector3D& position, const Matrix3x3& orientation) { Transform::Transform(const Vector3& position, const Matrix3x3& orientation) {
this->position = position; this->position = position;
this->orientation = Quaternion(orientation); this->orientation = Quaternion(orientation);
} }
// Constructor // Constructor
Transform::Transform(const Vector3D& position, const Quaternion& orientation) { Transform::Transform(const Vector3& position, const Quaternion& orientation) {
this->position = position; this->position = position;
this->orientation = orientation; this->orientation = orientation;
} }

26
src/mathematics/Transform.h
View File

@ -27,7 +27,7 @@
// Libraries // Libraries
#include "Matrix3x3.h" #include "Matrix3x3.h"
#include "Vector3D.h" #include "Vector3.h"
#include "Quaternion.h" #include "Quaternion.h"
// ReactPhysiscs3D namespace // ReactPhysiscs3D namespace
@ -41,17 +41,17 @@ namespace reactphysics3d {
*/ */
class Transform { class Transform {
private : private :
Vector3D position; // Position Vector3 position; // Position
Quaternion orientation; // Orientation Quaternion orientation; // Orientation
public : public :
Transform(); // Constructor Transform(); // Constructor
Transform(const Vector3D& position, const Matrix3x3& orientation); // Constructor Transform(const Vector3& position, const Matrix3x3& orientation); // Constructor
Transform(const Vector3D& position, const Quaternion& orientation); // Constructor Transform(const Vector3& position, const Quaternion& orientation); // Constructor
~Transform(); // Destructor ~Transform(); // Destructor
const Vector3D& getPosition() const; // Return the origin of the transform const Vector3& getPosition() const; // Return the origin of the transform
void setPosition(const Vector3D& position); // Set the origin of the transform void setPosition(const Vector3& position); // Set the origin of the transform
const Quaternion& getOrientation() const; // Return the orientation quaternion const Quaternion& getOrientation() const; // Return the orientation quaternion
void setOrientation(const Quaternion& orientation); // Set the rotation quaternion void setOrientation(const Quaternion& orientation); // Set the rotation quaternion
void setToIdentity(); // Set the transform to the identity transform void setToIdentity(); // Set the transform to the identity transform
@ -62,17 +62,17 @@ class Transform {
const Transform& newTransform, const Transform& newTransform,
double interpolationFactor); // Return an interpolated transform double interpolationFactor); // Return an interpolated transform
Vector3D operator*(const Vector3D& vector) const; // Return the transformed vector 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 Transform operator*(const Transform& transform2) const; // Operator of multiplication of a transform with another one
}; };
// Return the position of the transform // Return the position of the transform
inline const Vector3D& Transform::getPosition() const { inline const Vector3& Transform::getPosition() const {
return position; return position;
} }
// Set the origin of the transform // Set the origin of the transform
inline void Transform::setPosition(const Vector3D& position) { inline void Transform::setPosition(const Vector3& position) {
this->position = position; this->position = position;
} }
@ -88,7 +88,7 @@ inline void Transform::setOrientation(const Quaternion& orientation) {
// Set the transform to the identity transform // Set the transform to the identity transform
inline void Transform::setToIdentity() { inline void Transform::setToIdentity() {
position = Vector3D(0.0, 0.0, 0.0); position = Vector3(0.0, 0.0, 0.0);
orientation = Quaternion::identity(); orientation = Quaternion::identity();
} }
@ -114,18 +114,18 @@ inline void Transform::getOpenGLMatrix(double* openglMatrix) const {
inline Transform Transform::inverse() const { inline Transform Transform::inverse() const {
const Quaternion& invQuaternion = orientation.getInverse(); const Quaternion& invQuaternion = orientation.getInverse();
Matrix3x3 invMatrix = invQuaternion.getMatrix(); Matrix3x3 invMatrix = invQuaternion.getMatrix();
return Transform(invMatrix * position.getOpposite(), invQuaternion); return Transform(invMatrix * (-position), invQuaternion);
} }
// Return an interpolated transform // 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, double interpolationFactor) {
Vector3D interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor; Vector3 interPosition = oldTransform.position * (1.0 - interpolationFactor) + newTransform.position * interpolationFactor;
Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor); Quaternion interOrientation = Quaternion::slerp(oldTransform.orientation, newTransform.orientation, interpolationFactor);
return Transform(interPosition, interOrientation); return Transform(interPosition, interOrientation);
} }
// Return the transformed vector // Return the transformed vector
inline Vector3D Transform::operator*(const Vector3D& vector) const { inline Vector3 Transform::operator*(const Vector3& vector) const {
return (orientation.getMatrix() * vector) + position; return (orientation.getMatrix() * vector) + position;
} }

2
src/mathematics/Vector.cpp
View File

@ -66,7 +66,7 @@ Vector::Vector(const Vector& vector) {
} }
// Conversion from Vector3D to Vector // Conversion from Vector3D to Vector
Vector::Vector(const Vector3D& vector3d) { Vector::Vector(const Vector3& vector3d) {
nbComponent = 3; nbComponent = 3;
tab = new double[3]; tab = new double[3];

4
src/mathematics/Vector.h
View File

@ -26,7 +26,7 @@
#define VECTOR_H #define VECTOR_H
// Libraries // Libraries
#include "Vector3D.h" #include "Vector3.h"
#include "../constants.h" #include "../constants.h"
#include "mathematics_functions.h" #include "mathematics_functions.h"
#include "exceptions.h" #include "exceptions.h"
@ -52,7 +52,7 @@ class Vector {
Vector(); // Constructor without argument Vector(); // Constructor without argument
Vector(int n) throw(std::invalid_argument); // Constructor of the class Vector Vector(int n) throw(std::invalid_argument); // Constructor of the class Vector
Vector(const Vector& vector); // Copy-constructor of the class Vector Vector(const Vector& vector); // Copy-constructor of the class Vector
Vector(const Vector3D& vector3d); // Conversion from Vector3D to Vector Vector(const Vector3& vector3d); // Conversion from Vector3D to Vector
virtual ~Vector(); // Destructor of the class Vector virtual ~Vector(); // Destructor of the class Vector
double getValue(int n) const throw(std::invalid_argument); // Get a component of the vector double getValue(int n) const throw(std::invalid_argument); // Get a component of the vector
void setValue(int n, double value) throw(std::invalid_argument); // Set the value of a component of the vector void setValue(int n, double value) throw(std::invalid_argument); // Set the value of a component of the vector

95
src/mathematics/Vector3.cpp Normal file
View File

@ -0,0 +1,95 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Vector3.h"
#include <iostream>
#include <cassert>
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Vector3D
Vector3::Vector3() {
values[0] = 0.0;
values[1] = 0.0;
values[2] = 0.0;
}
// Constructor with arguments
Vector3::Vector3(double x, double y, double z) {
values[0] = x;
values[1] = y;
values[2] = z;
}
// Copy-constructor
Vector3::Vector3(const Vector3& vector) {
values[0] = vector.values[0];
values[1] = vector.values[1];
values[2] = vector.values[2];
}
// Destructor
Vector3::~Vector3() {
}
// Return the corresponding unit vector
Vector3 Vector3::getUnit() const {
double lengthVector = length();
assert(lengthVector != 0.0);
// Compute and return the unit vector
double lengthInv = 1.0 / lengthVector;
return Vector3(values[0] * lengthInv, values[1] * lengthInv, values[2] * lengthInv);
}
// Return two unit orthogonal vectors of the current vector
Vector3 Vector3::getOneOrthogonalVector() const {
assert(!this->isZero());
// Compute a first orthogonal vector
Vector3 vector1;
if (!approxEqual(values[0], 0.0)) { // If x != 0
vector1.setY(values[0]);
vector1.setZ((-2*values[0]*values[1]*values[2] + 2*values[0]*values[2])/(2*(values[2]*values[2] + values[0]*values[0])));
vector1.setX((-values[0]*values[1]-values[2]*vector1.getZ())/values[0]);
}
else if (!approxEqual(values[1], 0.0)) { // If y != 0
vector1.setZ(values[1]);
vector1.setX((-2*values[0]*values[1]*values[2] + 2*values[0]*values[1])/(2*(values[1]*values[1] + values[0]*values[0])));
vector1.setY((-values[2]*values[1]-values[0]*vector1.getX())/values[1]);
}
else if (!approxEqual(values[2], 0.0)) { // If z != 0
vector1.setX(values[2]);
vector1.setY((-2*values[0]*values[1]*values[2] + 2*values[1]*values[2])/(2*(values[2]*values[2] + values[1]*values[1])));
vector1.setZ((-values[0]*values[2]-values[1]*vector1.getY())/values[2]);
}
assert(vector1.isUnit());
return vector1;
}

256
src/mathematics/Vector3.h Normal file
View File

@ -0,0 +1,256 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef VECTOR3_H
#define VECTOR3_H
// Libraries
#include <cmath>
#include "exceptions.h"
#include "mathematics_functions.h"
// TODO : Remove the methods getX(), getY(), getZ(), setX(), setY(), setZ() and replace the attributes
// x, y and z by an array values[3]
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Vector3 :
This classrepresents 3 dimensionnal vector in space.
-------------------------------------------------------------------
*/
class Vector3 {
private :
double 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(const Vector3& vector); // Copy-constructor
virtual ~Vector3(); // Destructor
void setValue(int index, double value) throw(std::invalid_argument); // Set a component of the vector
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
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
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
int getMaxAxis() const; // Return the axis with the maximal value
bool isParallelWith(const Vector3& vector) const; // Return true if two vectors are parallel
// --- Overloaded operators --- //
bool operator== (const Vector3& vector) const; // Overloaded operator for the equality condition
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
// 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);
};
// Get the x component of the vector
inline double Vector3::getX() const {
return values[0];
}
// Get the y component of the vector
inline double Vector3::getY() const {
return values[1];
}
// Get the z component of the vector
inline double Vector3::getZ() const {
return values[2];
}
// Set the x component of the vector
inline void Vector3::setX(double x) {
this->values[0] = x;
}
// Set the y component of the vector
inline void Vector3::setY(double y) {
this->values[1] = y;
}
// Set the z component of the vector
inline void Vector3::setZ(double z) {
this->values[2] = z;
}
// Set all the values of the vector (inline)
inline void Vector3::setAllValues(double x, double y, double z) {
values[0]= x;
values[1] = y;
values[2] = z;
}
// Return the length of the vector (inline)
inline double 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 {
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 {
// 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]);
}
// Cross product of two vectors (inline)
inline Vector3 Vector3::cross(const Vector3& vector) const {
// Compute and return the cross product
return Vector3(values[1] * vector.values[2] - values[2] * vector.values[1],
values[2] * vector.values[0] - values[0] * vector.values[2],
values[0] * vector.values[1] - values[1] * vector.values[0]);
}
// Return the corresponding absolute value vector
inline Vector3 Vector3::getAbsoluteVector() const {
return Vector3(std::abs(values[0]), std::abs(values[1]), std::abs(values[2]));
}
// Return true if two vectors are parallel
inline bool Vector3::isParallelWith(const Vector3& vector) const {
double scalarProd = this->dot(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
}
// Return the axis with the minimal value
inline int Vector3::getMinAxis() const {
return (values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2));
}
// Return the axis with the maximal value
inline int Vector3::getMaxAxis() const {
return (values[0] < values[1] ? (values[1] < values[2] ? 2 : 1) : (values[0] < values[2] ? 2 : 0));
}
// Return true if the vector is unit and false otherwise
inline bool Vector3::isUnit() const {
return approxEqual(values[0] * values[0] + values[1] * values[1] + values[2] * values[2], 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector3::isZero() const {
return approxEqual(values[0] * values[0] + values[1] * values[1] + values[2] * values[2], 0.0);
}
// Overloaded operator for the equality condition
inline bool Vector3::operator== (const Vector3& vector) const {
return (values[0] == vector.values[0] && values[1] == vector.values[1] && values[2] == vector.values[2]);
}
// Overloaded operator for the is different condition
inline bool Vector3::operator!= (const Vector3& vector) const {
return !(*this == vector);
}
// Overloaded operator for addition with assignment
inline Vector3& Vector3::operator+=(const Vector3& vector) {
values[0] += vector.values[0];
values[1] += vector.values[1];
values[2] += vector.values[2];
return *this;
}
// Overloaded operator for substraction with assignment
inline Vector3& Vector3::operator-=(const Vector3& vector) {
values[0] -= vector.values[0];
values[1] -= vector.values[1];
values[2] -= vector.values[2];
return *this;
}
// Overloaded operator for multiplication with a number with assignment
inline Vector3& Vector3::operator*=(double number) {
values[0] *= number;
values[1] *= number;
values[2] *= number;
return *this;
}
// Overloaded operator for value access
inline double& Vector3::operator[] (int index) {
return values[index];
}
// Overloaded operator for value access
inline const double& Vector3::operator[] (int index) const {
return values[index];
}
// Overloaded operator for addition
inline Vector3 operator+(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.values[0] + vector2.values[0], vector1.values[1] + vector2.values[1], vector1.values[2] + vector2.values[2]);
}
// Overloaded operator for substraction
inline Vector3 operator-(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.values[0] - vector2.values[0], vector1.values[1] - vector2.values[1], vector1.values[2] - vector2.values[2]);
}
// Overloaded operator for the negative of a vector
inline Vector3 operator-(const Vector3& vector) {
return Vector3(-vector.values[0], -vector.values[1], -vector.values[2]);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(const Vector3& vector, double 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) {
return vector * number;
}
} // End of the ReactPhysics3D namespace
#endif

130
src/mathematics/Vector3D.cpp
View File

@ -1,130 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Vector3D.h"
#include <iostream>
#include <cassert>
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Vector3D
Vector3D::Vector3D()
:x(0.0), y(0.0), z(0.0) {
}
// Constructor with arguments
Vector3D::Vector3D(double x, double y, double z)
:x(x), y(y), z(z) {
}
// Copy-constructor
Vector3D::Vector3D(const Vector3D& vector)
:x(vector.x), y(vector.y), z(vector.z) {
}
// Destructor
Vector3D::~Vector3D() {
}
// Return the corresponding unit vector
Vector3D Vector3D::getUnit() const throw(MathematicsException) {
double lengthVector = length();
// Check if the length is equal to zero
if (lengthVector != 0) {
// Compute and return the unit vector
double lengthInv = 1.0 / lengthVector;
return Vector3D(x * lengthInv, y * lengthInv, z*lengthInv);
}
else {
// Throw an exception because the length of the vector is zero
throw MathematicsException("MathematicsException : Impossible to compute the unit vector because the length of the vector is zero");
}
}
// Return two unit orthogonal vectors of the current vector
Vector3D Vector3D::getOneOrthogonalVector() const {
assert(!this->isZero());
Vector3D unitVector = this->getUnit();
// Compute a first orthogonal vector
Vector3D vector1;
if (!approxEqual(x, 0.0)) { // If x != 0
vector1.setY(x);
vector1.setZ((-2*x*y*z + 2*x*z)/(2*(z*z + x*x)));
vector1.setX((-x*y-z*vector1.getZ())/x);
}
else if (!approxEqual(y, 0.0)) { // If y != 0
vector1.setZ(y);
vector1.setX((-2*x*y*z + 2*x*y)/(2*(y*y + x*x)));
vector1.setY((-z*y-x*vector1.getX())/y);
}
else if (!approxEqual(z, 0.0)) { // If z != 0
vector1.setX(z);
vector1.setY((-2*x*y*z + 2*y*z)/(2*(z*z + y*y)));
vector1.setZ((-x*z-y*vector1.getY())/z);
}
assert(vector1.isUnit());
return vector1;
}
// Overloaded operator for addition
Vector3D Vector3D::operator+(const Vector3D& vector) const {
// Compute and return the sum of the two vectors
return Vector3D(x + vector.x, y + vector.y, z + vector.z);
}
// Overloaded operator for substraction
Vector3D Vector3D::operator-(const Vector3D& vector) const {
// Compute and return the substraction of the two vectors
return Vector3D(x - vector.x, y - vector.y, z - vector.z);
}
// Overloaded operator for multiplication with a number
Vector3D Vector3D::operator*(double number) const {
// Compute and return the result
return Vector3D(x * number, y * number, z * number);
}
// Overloaded operator for the assignement to a Vector
Vector3D& Vector3D::operator=(const Vector3D& vector) {
// Check for self-assignment
if (this != &vector) {
// Copy the vector
x = vector.x;
y = vector.y;
z = vector.z;
}
// Return a reference to the vector
return *this;
}

218
src/mathematics/Vector3D.h
View File

@ -1,218 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef VECTOR3D_H
#define VECTOR3D_H
// Libraries
#include <cmath>
#include "exceptions.h"
#include "mathematics_functions.h"
// TODO : Remove the methods getX(), getY(), getZ(), setX(), setY(), setZ() and replace the attributes
// x, y and z by an array values[3]
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Vector3D :
This classrepresents 3 dimensionnal vector in space.
-------------------------------------------------------------------
*/
class Vector3D {
private :
double x; // X component of the vector
double y; // Y component of the vector
double z; // Z component of the vector
public :
Vector3D(); // Constructor of the class Vector3D
Vector3D(double x, double y, double z); // Constructor with arguments
Vector3D(const Vector3D& vector); // Copy-constructor
virtual ~Vector3D(); // Destructor
double getValue(int index) const throw(std::invalid_argument); // Get a component of the vector
void setValue(int index, double value) throw(std::invalid_argument); // Set a component of the vector
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
Vector3D getUnit() const throw(MathematicsException); // 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
Vector3D getOpposite() const; // Return the vector in the opposite direction
Vector3D getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector
double dot(const Vector3D& vector) const; // Dot product of two vectors
Vector3D cross(const Vector3D& vector) const; // Cross product of two vectors
Vector3D getAbsoluteVector() const; // Return the corresponding absolute value vector
int getMinAxis() const; // Return the axis with the minimal value
int getMaxAxis() const; // Return the axis with the maximal value
bool isParallelWith(const Vector3D& vector) const; // Return true if two vectors are parallel
// --- Overloaded operators --- //
Vector3D operator+(const Vector3D& vector) const; // Overloaded operator for addition
Vector3D operator-(const Vector3D& vector) const ; // Overloaded operator for substraction
Vector3D operator*(double number) const; // Overloaded operator for multiplication with a number
Vector3D& operator=(const Vector3D& vector); // Overloaded operator for the assignement to a Vector
bool operator==(const Vector3D& vector) const; // Overloaded operator for the equality condition
};
// Get the x component of the vector (inline)
inline double Vector3D::getX() const {
return x;
}
// Get the y component of the vector (inline)
inline double Vector3D::getY() const {
return y;
}
// Get the z component of the vector (inline)
inline double Vector3D::getZ() const {
return z;
}
// Set the x component of the vector (inline)
inline void Vector3D::setX(double x) {
this->x = x;
}
// Set the y component of the vector (inline)
inline void Vector3D::setY(double y) {
this->y = y;
}
// Set the z component of the vector (inline)
inline void Vector3D::setZ(double z) {
this->z = z;
}
// Get a component of the vector
inline double Vector3D::getValue(int index) const throw(std::invalid_argument) {
switch(index) {
case 0: return x;
case 1: return y;
case 2: return z;
default: // Throw an exception because of the wrong argument
throw std::invalid_argument("The argument is outside the bounds of the Vector3D");
}
}
// Set a component of the vector
inline void Vector3D::setValue(int index, double value) throw(std::invalid_argument) {
switch(index) {
case 0: x = value;
case 1: y = value;
case 2: z = value;
default: // Throw an exception because of the wrong argument
throw std::invalid_argument("The argument is outside the bounds of the Vector3D");
}
}
// Set all the values of the vector (inline)
inline void Vector3D::setAllValues(double x, double y, double z) {
this->x = x;
this->y = y;
this->z = z;
}
// Return the length of the vector (inline)
inline double Vector3D::length() const {
// Compute and return the length of the vector
return sqrt(x*x + y*y + z*z);
}
// Return the square of the length of the vector
inline double Vector3D::lengthSquare() const {
return x*x + y*y + z*z;
}
// Return the vector in the opposite direction
inline Vector3D Vector3D::getOpposite() const {
return (Vector3D(0.0, 0.0, 0.0) - *this);
}
// Scalar product of two vectors (inline)
inline double Vector3D::dot(const Vector3D& vector) const {
// Compute and return the result of the scalar product
return (x * vector.x + y * vector.y + z * vector.z);
}
// Cross product of two vectors (inline)
inline Vector3D Vector3D::cross(const Vector3D& vector) const {
// Compute and return the cross product
return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x);
}
// Return the corresponding absolute value vector
inline Vector3D Vector3D::getAbsoluteVector() const {
return Vector3D(std::abs(x), std::abs(y), std::abs(z));
}
// Return true if two vectors are parallel
inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
double scalarProd = this->dot(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
}
// Return the axis with the minimal value
inline int Vector3D::getMinAxis() const {
return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2));
}
// Return the axis with the maximal value
inline int Vector3D::getMaxAxis() const {
return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0));
}
// Return true if the vector is unit and false otherwise
inline bool Vector3D::isUnit() const {
return approxEqual(x*x+y*y+z*z, 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector3D::isZero() const {
return approxEqual(x*x+y*y+z*z, 0.0);
}
// Overloaded operator for multiplication between a number and a Vector3D (inline)
inline Vector3D operator * (double number, const Vector3D& vector) {
// Compute and return the result vector
return vector * number;
}
// Overloaded operator for the equality condition
inline bool Vector3D::operator == (const Vector3D& vector) const {
return (x == vector.x && y == vector.y && z == vector.z);
}
} // End of the ReactPhysics3D namespace
#endif

116
src/mathematics/mathematics.h
View File

@ -30,7 +30,7 @@
#include "Matrix3x3.h" #include "Matrix3x3.h"
#include "Quaternion.h" #include "Quaternion.h"
#include "Vector.h" #include "Vector.h"
#include "Vector3D.h" #include "Vector3.h"
#include "Transform.h" #include "Transform.h"
#include "../constants.h" #include "../constants.h"
#include "exceptions.h" #include "exceptions.h"
@ -47,7 +47,7 @@ namespace reactphysics3d {
// Rotate a vector according to a rotation quaternion. // Rotate a vector according to a rotation quaternion.
// The function returns the vector rotated according to the quaternion in argument // The function returns the vector rotated according to the quaternion in argument
inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) { inline reactphysics3d::Vector3 rotateVectorWithQuaternion(const reactphysics3d::Vector3& vector, const reactphysics3d::Quaternion& quaternion) {
// Convert the vector into a quaternion // Convert the vector into a quaternion
reactphysics3d::Quaternion vectorQuaternion(0, vector); reactphysics3d::Quaternion vectorQuaternion(0, vector);
@ -62,10 +62,10 @@ inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d:
// "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that // "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that
// P1 = point1 + alpha * d1 // P1 = point1 + alpha * d1
// P2 = point2 + beta * d2 // P2 = point2 + beta * d2
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2, inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3& point1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& point2,
const reactphysics3d::Vector3D& d2, double* alpha, double* beta) { const reactphysics3d::Vector3& d2, double* alpha, double* beta) {
reactphysics3d::Vector3D r = point1 - point2; reactphysics3d::Vector3 r = point1 - point2;
double a = d1.dot(d1); double a = d1.dot(d1);
double b = d1.dot(d2); double b = d1.dot(d2);
double c = d1.dot(r); double c = d1.dot(r);
@ -82,11 +82,11 @@ inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1,
} }
// This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise // This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise
inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const reactphysics3d::Vector3D& segPointB, const reactphysics3d::Vector3D& P) { inline bool isPointOnSegment(const reactphysics3d::Vector3& segPointA, const reactphysics3d::Vector3& segPointB, const reactphysics3d::Vector3& P) {
// Check if the point P is on the line between "segPointA" and "segPointB" // Check if the point P is on the line between "segPointA" and "segPointB"
reactphysics3d::Vector3D d = segPointB - segPointA; reactphysics3d::Vector3 d = segPointB - segPointA;
reactphysics3d::Vector3D dP = P - segPointA; reactphysics3d::Vector3 dP = P - segPointA;
if (!d.isParallelWith(dP)) { if (!d.isParallelWith(dP)) {
return false; return false;
} }
@ -110,13 +110,13 @@ inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const re
// Given two lines in 3D that intersect, this method returns the intersection point between the two lines. // Given two lines in 3D that intersect, this method returns the intersection point between the two lines.
// The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2". // The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2".
inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::Vector3D& p1, const reactphysics3d::Vector3D& d1, inline reactphysics3d::Vector3 computeLinesIntersection(const reactphysics3d::Vector3& p1, const reactphysics3d::Vector3& d1,
const reactphysics3d::Vector3D& p2, const reactphysics3d::Vector3D& d2) { const reactphysics3d::Vector3& p2, const reactphysics3d::Vector3& d2) {
// Computes the two closest points on the lines // Computes the two closest points on the lines
double alpha, beta; double alpha, beta;
closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta); closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = p1 + alpha * d1; reactphysics3d::Vector3 point1 = p1 + alpha * d1;
reactphysics3d::Vector3D point2 = p2 + beta * d2; reactphysics3d::Vector3 point2 = p2 + beta * d2;
// The two points must be very close // The two points must be very close
//assert((point1-point2).length() <= 0.1); //assert((point1-point2).length() <= 0.1);
@ -128,11 +128,11 @@ inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::V
// Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments. // Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments.
// This method returns the intersection point. // This method returns the intersection point.
inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB, inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3& seg1PointA, const reactphysics3d::Vector3& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB) { const reactphysics3d::Vector3& seg2PointA, const reactphysics3d::Vector3& seg2PointB) {
// Determine the lines of both segments // Determine the lines of both segments
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA; reactphysics3d::Vector3 d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA; reactphysics3d::Vector3 d2 = seg2PointB - seg2PointA;
// The segments must not be parallel // The segments must not be parallel
assert(!d1.isParallelWith(d2)); assert(!d1.isParallelWith(d2));
@ -140,15 +140,15 @@ inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const rea
// Compute the closet points between the two lines // Compute the closet points between the two lines
double alpha, beta; double alpha, beta;
closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta); closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = seg1PointA + alpha * d1; reactphysics3d::Vector3 point1 = seg1PointA + alpha * d1;
reactphysics3d::Vector3D point2 = seg2PointA + beta * d2; reactphysics3d::Vector3 point2 = seg2PointA + beta * d2;
// The closest points have to be on the segments, otherwise there is no intersection between the segments // The closest points have to be on the segments, otherwise there is no intersection between the segments
assert(isPointOnSegment(seg1PointA, seg1PointB, point1)); assert(isPointOnSegment(seg1PointA, seg1PointB, point1));
assert(isPointOnSegment(seg2PointA, seg2PointB, point2)); assert(isPointOnSegment(seg2PointA, seg2PointB, point2));
// If the two closest point aren't very close, there is no intersection between the segments // If the two closest point aren't very close, there is no intersection between the segments
reactphysics3d::Vector3D d = point2 - point1; reactphysics3d::Vector3 d = point2 - point1;
assert(d.length() <= EPSILON_TEST); assert(d.length() <= EPSILON_TEST);
// They are very close so we return the intersection point (halfway between "point1" and "point2" // They are very close so we return the intersection point (halfway between "point1" and "point2"
@ -158,8 +158,8 @@ inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const rea
// Move a set of points by a given vector. // Move a set of points by a given vector.
// The method returns a set of points moved by the given vector. // The method returns a set of points moved by the given vector.
inline std::vector<reactphysics3d::Vector3D> movePoints(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& vector) { inline std::vector<reactphysics3d::Vector3> movePoints(const std::vector<reactphysics3d::Vector3>& points, const reactphysics3d::Vector3& vector) {
std::vector<reactphysics3d::Vector3D> result; std::vector<reactphysics3d::Vector3> result;
// For each point of the set // For each point of the set
for (unsigned int i=0; i<points.size(); ++i) { for (unsigned int i=0; i<points.size(); ++i) {
@ -174,12 +174,12 @@ inline std::vector<reactphysics3d::Vector3D> movePoints(const std::vector<reactp
// Compute the projection of a set of 3D points onto a 3D plane. The set of points is given by "points" and the plane is given by // Compute the projection of a set of 3D points onto a 3D plane. The set of points is given by "points" and the plane is given by
// a point "A" and a normal vector "normal". This method returns the initial set of points projected onto the plane. // a point "A" and a normal vector "normal". This method returns the initial set of points projected onto the plane.
inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& A, inline std::vector<reactphysics3d::Vector3> projectPointsOntoPlane(const std::vector<reactphysics3d::Vector3>& points, const reactphysics3d::Vector3& A,
const reactphysics3d::Vector3D& normal) { const reactphysics3d::Vector3& normal) {
assert(normal.length() != 0.0); assert(normal.length() != 0.0);
std::vector<Vector3D> projectedPoints; std::vector<Vector3> projectedPoints;
reactphysics3d::Vector3D n = normal.getUnit(); reactphysics3d::Vector3 n = normal.getUnit();
// For each point of the set // For each point of the set
for (unsigned int i=0; i<points.size(); ++i) { for (unsigned int i=0; i<points.size(); ++i) {
@ -194,21 +194,21 @@ inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::v
// Compute the distance between a point "P" and a line (given by a point "A" and a vector "v") // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) { inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
assert(v.length() != 0); assert(v.length() != 0);
return ((P-A).cross(v).length() / (v.length())); return ((P-A).cross(v).length() / (v.length()));
} }
// Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v") // Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v")
inline reactphysics3d::Vector3D computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) { inline reactphysics3d::Vector3 computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3& P, const reactphysics3d::Vector3& A, const reactphysics3d::Vector3& v) {
return (A + ((P-A).dot(v) / (v.dot(v))) * v); return (A + ((P-A).dot(v) / (v.dot(v))) * v);
} }
// Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes // Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes
// the point Q that is the nearest point to P that is inside (on a border of) the rectangle. The point P should be outside the rectangle. // the point Q that is the nearest point to P that is inside (on a border of) the rectangle. The point P should be outside the rectangle.
// The result point Q will be in a segment of the rectangle // The result point Q will be in a segment of the rectangle
inline reactphysics3d::Vector3D computeNearestPointOnRectangle(const reactphysics3d::Vector3D& P, const std::vector<reactphysics3d::Vector3D> rectangle) { inline reactphysics3d::Vector3 computeNearestPointOnRectangle(const reactphysics3d::Vector3& P, const std::vector<reactphysics3d::Vector3> rectangle) {
assert(rectangle.size() == 4); assert(rectangle.size() == 4);
double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]); double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]); double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
@ -216,7 +216,7 @@ inline reactphysics3d::Vector3D computeNearestPointOnRectangle(const reactphysic
double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]); double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]); double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]); double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
Vector3D resultPoint; Vector3 resultPoint;
// Check if P is between the lines of the first pair of parallel segments of the rectangle // Check if P is between the lines of the first pair of parallel segments of the rectangle
if (distPSegment1 <= distSegment1Segment3 && distPSegment3 <= distSegment1Segment3) { if (distPSegment1 <= distSegment1Segment3 && distPSegment3 <= distSegment1Segment3) {
@ -271,12 +271,12 @@ inline reactphysics3d::Vector3D computeNearestPointOnRectangle(const reactphysic
// segment is between the points "seg2PointA" and "seg2PointB"). The result is the segment intersection (represented by the points "resultPointA" // segment is between the points "seg2PointA" and "seg2PointB"). The result is the segment intersection (represented by the points "resultPointA"
// and "resultPointB". Because the two given segments don't have to be on the same exact line, the result intersection segment will a segment // and "resultPointB". Because the two given segments don't have to be on the same exact line, the result intersection segment will a segment
// halway between the first and the second given segments. // halway between the first and the second given segments.
inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB, inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& seg1PointA, const reactphysics3d::Vector3& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB, const reactphysics3d::Vector3& seg2PointA, const reactphysics3d::Vector3& seg2PointB,
reactphysics3d::Vector3D& resultPointA, reactphysics3d::Vector3D& resultPointB) { reactphysics3d::Vector3& resultPointA, reactphysics3d::Vector3& resultPointB) {
// Compute the segment vectors // Compute the segment vectors
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA; reactphysics3d::Vector3 d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA; reactphysics3d::Vector3 d2 = seg2PointB - seg2PointA;
// The two segments should be parallel // The two segments should be parallel
assert(d1.isParallelWith(d2)); assert(d1.isParallelWith(d2));
@ -290,8 +290,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D&
assert(!(projSeg2PointA > d1.length() && projSeg2PointB > d1.length())); assert(!(projSeg2PointA > d1.length() && projSeg2PointB > d1.length()));
// Compute the vector "v" from a point on the line 1 to the orthogonal point of the line 2 // Compute the vector "v" from a point on the line 1 to the orthogonal point of the line 2
reactphysics3d::Vector3D point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1); reactphysics3d::Vector3 point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1);
reactphysics3d::Vector3D v = seg2PointA - point; reactphysics3d::Vector3 v = seg2PointA - point;
// Return the segment intersection according to the configuration of two projection intervals // Return the segment intersection according to the configuration of two projection intervals
if (projSeg2PointA >= 0 && projSeg2PointA <= d1.length() && projSeg2PointB >= d1.length()) { if (projSeg2PointA >= 0 && projSeg2PointA <= d1.length() && projSeg2PointB >= d1.length()) {
@ -320,14 +320,14 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D&
// also assume that the segment is not completely outside the clipping rectangle. // also assume that the segment is not completely outside the clipping rectangle.
// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle". // 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. // This method returns the clipped segment.
inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& segment, const std::vector<reactphysics3d::Vector3D> clipRectangle) { inline std::vector<reactphysics3d::Vector3> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& segment, const std::vector<reactphysics3d::Vector3> clipRectangle) {
double const epsilon = 0.01; double const epsilon = 0.01;
assert(segment.size() == 2); assert(segment.size() == 2);
assert(clipRectangle.size() == 4); assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> inputSegment = segment; std::vector<reactphysics3d::Vector3> inputSegment = segment;
std::vector<reactphysics3d::Vector3D> outputSegment; std::vector<reactphysics3d::Vector3> outputSegment;
// For each edge of the clip rectangle // For each edge of the clip rectangle
for (unsigned int i=0; i<4; ++i) { for (unsigned int i=0; i<4; ++i) {
@ -335,13 +335,13 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
// Current clipped segment // Current clipped segment
//assert(inputSegment.size() == 2); //assert(inputSegment.size() == 2);
reactphysics3d::Vector3D S = inputSegment[0]; reactphysics3d::Vector3 S = inputSegment[0];
reactphysics3d::Vector3D P = inputSegment[1]; reactphysics3d::Vector3 P = inputSegment[1];
// Edge of the clip rectangle // Edge of the clip rectangle
reactphysics3d::Vector3D A = clipRectangle[i]; reactphysics3d::Vector3 A = clipRectangle[i];
reactphysics3d::Vector3D B = clipRectangle[ (i+1) % 4]; reactphysics3d::Vector3 B = clipRectangle[ (i+1) % 4];
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4]; reactphysics3d::Vector3 planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
// If the point P is inside the clip plane // If the point P is inside the clip plane
if (planeNormal.dot(P-A) >= 0.0 - epsilon) { if (planeNormal.dot(P-A) >= 0.0 - epsilon) {
@ -352,7 +352,7 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
} }
else { // P is inside and S is outside the clip plane else { // P is inside and S is outside the clip plane
// Compute the intersection point between the segment SP and the clip plane // Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); reactphysics3d::Vector3 intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputSegment.push_back(P); outputSegment.push_back(P);
outputSegment.push_back(intersectPoint); outputSegment.push_back(intersectPoint);
@ -360,7 +360,7 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
} }
else if (planeNormal.dot(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane else if (planeNormal.dot(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane
// Compute the intersection point between the segment SP and the clip plane // Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); reactphysics3d::Vector3 intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputSegment.push_back(S); outputSegment.push_back(S);
outputSegment.push_back(intersectPoint); outputSegment.push_back(intersectPoint);
@ -376,12 +376,12 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
// This method uses the Sutherland-Hodgman clipping algorithm to clip a subject polygon (given by the ordered 3D vertices in "subjectPolygon") using // This method uses the Sutherland-Hodgman clipping algorithm to clip a subject polygon (given by the ordered 3D vertices in "subjectPolygon") using
// 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 // 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. // 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::Vector3D> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& subjectPolygon, const std::vector<reactphysics3d::Vector3D>& clipRectangle) { inline std::vector<reactphysics3d::Vector3> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3>& subjectPolygon, const std::vector<reactphysics3d::Vector3>& clipRectangle) {
double const epsilon = 0.1; double const epsilon = 0.1;
assert(clipRectangle.size() == 4); assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> outputPolygon; std::vector<reactphysics3d::Vector3> outputPolygon;
std::vector<reactphysics3d::Vector3D> inputPolygon = subjectPolygon; std::vector<reactphysics3d::Vector3> inputPolygon = subjectPolygon;
// For each edge of the clip rectangle // For each edge of the clip rectangle
for (unsigned int i=0; i<4; ++i) { for (unsigned int i=0; i<4; ++i) {
@ -389,14 +389,14 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
// Each edge defines a clip plane. The clip plane is define by a point on this plane (a vertice of the current edge) and // Each edge defines a clip plane. The clip plane is define by a point on this plane (a vertice of the current edge) and
// a plane normal (because we are using a clip rectangle, the plane normal is the next edge of the clip rectangle). // a plane normal (because we are using a clip rectangle, the plane normal is the next edge of the clip rectangle).
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4]; reactphysics3d::Vector3 planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
reactphysics3d::Vector3D A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle" reactphysics3d::Vector3 A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle"
reactphysics3d::Vector3D B = clipRectangle[(i+1) % 4]; reactphysics3d::Vector3 B = clipRectangle[(i+1) % 4];
reactphysics3d::Vector3D S = inputPolygon[0]; reactphysics3d::Vector3 S = inputPolygon[0];
// For each vertex of the subject polygon // For each vertex of the subject polygon
for (unsigned int j=0; j<inputPolygon.size(); ++j) { for (unsigned int j=0; j<inputPolygon.size(); ++j) {
reactphysics3d::Vector3D P = inputPolygon[(j+1) % inputPolygon.size()]; reactphysics3d::Vector3 P = inputPolygon[(j+1) % inputPolygon.size()];
// If the point P is inside the clip plane // If the point P is inside the clip plane
double test = planeNormal.dot(P-A); double test = planeNormal.dot(P-A);
@ -407,7 +407,7 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
} }
else { // If the point S is outside the clip plane else { // If the point S is outside the clip plane
// Compute the intersection point between the segment SP and the clip plane // Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); reactphysics3d::Vector3 intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputPolygon.push_back(intersectPoint); outputPolygon.push_back(intersectPoint);
outputPolygon.push_back(P); outputPolygon.push_back(P);
@ -415,7 +415,7 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
} }
else if (planeNormal.dot(S-A) > 0.0) { else if (planeNormal.dot(S-A) > 0.0) {
// Compute the intersection point between the segment SP and the clip plane // Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A); reactphysics3d::Vector3 intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputPolygon.push_back(intersectPoint); outputPolygon.push_back(intersectPoint);
} }
@ -430,8 +430,8 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
// Compute the intersection point between a line and a plane in 3D space. There must be an intersection, therefore the // Compute the intersection point between a line and a plane in 3D space. There must be an intersection, therefore the
// the lineVector must not be orthogonal to the planeNormal. // the lineVector must not be orthogonal to the planeNormal.
inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector, inline reactphysics3d::Vector3 intersectLineWithPlane(const reactphysics3d::Vector3& linePoint, const reactphysics3d::Vector3& lineVector,
const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) { const reactphysics3d::Vector3& planePoint, const reactphysics3d::Vector3& planeNormal) {
assert(!approxEqual(lineVector.dot(planeNormal), 0.0)); 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 // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane

2
src/shapes/AABB.cpp
View File

@ -39,7 +39,7 @@ AABB::AABB() : bodyPointer(0) {
} }
// Constructor // Constructor
AABB::AABB(const Transform& transform, const Vector3D& extents) : bodyPointer(0) { AABB::AABB(const Transform& transform, const Vector3& extents) : bodyPointer(0) {
update(transform, extents); update(transform, extents);
} }

24
src/shapes/AABB.h
View File

@ -44,39 +44,39 @@ class Body;
*/ */
class AABB { class AABB {
private : private :
Vector3D minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis Vector3 minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis
Vector3D maxCoordinates; // Maximum 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 : public :
AABB(); // Constructor AABB(); // Constructor
AABB(const Transform& transform, const Vector3D& extents); // Constructor AABB(const Transform& transform, const Vector3& extents); // Constructor
virtual ~AABB(); // Destructor virtual ~AABB(); // Destructor
Vector3D getCenter() const; // Return the center point Vector3 getCenter() const; // Return the center point
const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB const Vector3& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3D& getMaxCoordinates() const; // Return the maximum 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 Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer 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 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 Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body 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 #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 #endif
}; };
// Return the center point of the AABB in world coordinates // Return the center point of the AABB in world coordinates
inline Vector3D AABB::getCenter() const { inline Vector3 AABB::getCenter() const {
return (minCoordinates + maxCoordinates) * 0.5; return (minCoordinates + maxCoordinates) * 0.5;
} }
// Return the minimum coordinates of the AABB // Return the minimum coordinates of the AABB
inline const Vector3D& AABB::getMinCoordinates() const { inline const Vector3& AABB::getMinCoordinates() const {
return minCoordinates; return minCoordinates;
} }
// Return the maximum coordinates of the AABB // Return the maximum coordinates of the AABB
inline const Vector3D& AABB::getMaxCoordinates() const { inline const Vector3& AABB::getMaxCoordinates() const {
return maxCoordinates; return maxCoordinates;
} }
@ -100,9 +100,9 @@ inline bool AABB::testCollision(const AABB& aabb) const {
} }
// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis // Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis
inline void AABB::update(const Transform& newTransform, const Vector3D& extents) { inline void AABB::update(const Transform& newTransform, const Vector3& extents) {
Matrix3x3 worldAxis = newTransform.getOrientation().getMatrix().getAbsoluteMatrix(); Matrix3x3 worldAxis = newTransform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(extents), Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
worldAxis.getColumn(1).dot(extents), worldAxis.getColumn(1).dot(extents),
worldAxis.getColumn(2).dot(extents)); worldAxis.getColumn(2).dot(extents));
minCoordinates = newTransform.getPosition() - worldExtents; minCoordinates = newTransform.getPosition() - worldExtents;

2
src/shapes/BoxShape.cpp
View File

@ -36,7 +36,7 @@ using namespace reactphysics3d;
using namespace std; using namespace std;
// Constructor // Constructor
BoxShape::BoxShape(const Vector3D& extent) : extent(extent) { BoxShape::BoxShape(const Vector3& extent) : extent(extent) {
} }

24
src/shapes/BoxShape.h
View File

@ -46,16 +46,16 @@ namespace reactphysics3d {
*/ */
class BoxShape : public Shape { class BoxShape : public Shape {
private : private :
Vector3D extent; // Extent sizes of the box Vector3 extent; // Extent sizes of the box
public : public :
BoxShape(const Vector3D& extent); // Constructor BoxShape(const Vector3& extent); // Constructor
virtual ~BoxShape(); // Destructor virtual ~BoxShape(); // Destructor
const Vector3D& getExtent() const; // Return the extents of the box const Vector3& getExtent() const; // Return the extents of the box
void setExtent(const Vector3D& extent); // Set the extents of the box void setExtent(const Vector3& extent); // Set the extents of the box
virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a local support point in a given direction virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the Box (only for testing purpose) virtual void draw() const; // Draw the Box (only for testing purpose)
@ -63,26 +63,26 @@ class BoxShape : public Shape {
}; };
// Return the extents of the box // Return the extents of the box
inline const Vector3D& BoxShape::getExtent() const { inline const Vector3& BoxShape::getExtent() const {
return extent; return extent;
} }
// Set the extents of the box // Set the extents of the box
inline void BoxShape::setExtent(const Vector3D& extent) { inline void BoxShape::setExtent(const Vector3& extent) {
this->extent = 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 shape (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box // This method is used to compute the AABB of the box
inline Vector3D BoxShape::getLocalExtents(double margin) const { inline Vector3 BoxShape::getLocalExtents(double margin) const {
return extent + Vector3D(margin, margin, margin); return extent + Vector3(margin, margin, margin);
} }
// Return a local support point in a given direction // Return a local support point in a given direction
inline Vector3D BoxShape::getLocalSupportPoint(const Vector3D& direction, double margin) const { inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0); assert(margin >= 0.0);
return Vector3D(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin, return Vector3(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin,
direction.getY() < 0.0 ? -extent.getY()-margin : extent.getY()+margin, direction.getY() < 0.0 ? -extent.getY()-margin : extent.getY()+margin,
direction.getZ() < 0.0 ? -extent.getZ()-margin : extent.getZ()+margin); direction.getZ() < 0.0 ? -extent.getZ()-margin : extent.getZ()+margin);
} }

14
src/shapes/ConeShape.cpp
View File

@ -48,30 +48,30 @@ ConeShape::~ConeShape() {
} }
// Return a local support point in a given direction // Return a local support point in a given direction
inline Vector3D ConeShape::getLocalSupportPoint(const Vector3D& direction, double margin) const { inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0); assert(margin >= 0.0);
const Vector3D& v = direction; const Vector3& v = direction;
double sinThetaTimesLengthV = sinTheta * v.length(); double sinThetaTimesLengthV = sinTheta * v.length();
Vector3D supportPoint; Vector3 supportPoint;
if (v.getY() >= sinThetaTimesLengthV) { if (v.getY() >= sinThetaTimesLengthV) {
supportPoint = Vector3D(0.0, halfHeight, 0.0); supportPoint = Vector3(0.0, halfHeight, 0.0);
} }
else { else {
double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ()); double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
if (projectedLength > MACHINE_EPSILON) { if (projectedLength > MACHINE_EPSILON) {
double d = radius / projectedLength; double d = radius / projectedLength;
supportPoint = Vector3D(v.getX() * d, -halfHeight, v.getZ() * d); supportPoint = Vector3(v.getX() * d, -halfHeight, v.getZ() * d);
} }
else { else {
supportPoint = Vector3D(radius, -halfHeight, 0.0); supportPoint = Vector3(radius, -halfHeight, 0.0);
} }
} }
// Add the margin to the support point // Add the margin to the support point
if (margin != 0.0) { if (margin != 0.0) {
Vector3D unitVec(0.0, -1.0, 0.0); Vector3 unitVec(0.0, -1.0, 0.0);
if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = v.getUnit(); unitVec = v.getUnit();
} }

11
src/shapes/ConeShape.h
View File

@ -29,9 +29,6 @@
#include "Shape.h" #include "Shape.h"
#include "../mathematics/mathematics.h" #include "../mathematics/mathematics.h"
// TODO : CHECK THAT THE AABB IS CORRECT
// TODO : TEST THIS SHAPE WITH GJK AND EPA ALGORITHMS
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -59,8 +56,8 @@ class ConeShape : public Shape {
void setRadius(double radius); // Set the radius void setRadius(double radius); // Set the radius
double getHeight() const; // Return the height double getHeight() const; // Return the height
void setHeight(double height); // Set the height void setHeight(double height); // Set the height
virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction
virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
@ -95,8 +92,8 @@ inline void ConeShape::setHeight(double height) {
} }
// Return the local extents in x,y and z direction // Return the local extents in x,y and z direction
inline Vector3D ConeShape::getLocalExtents(double margin) const { inline Vector3 ConeShape::getLocalExtents(double margin) const {
return Vector3D(radius + margin, halfHeight + margin, radius + margin); return Vector3(radius + margin, halfHeight + margin, radius + margin);
} }
}; // End of the ReactPhysics3D namespace }; // End of the ReactPhysics3D namespace

8
src/shapes/CylinderShape.cpp
View File

@ -42,12 +42,12 @@ CylinderShape::~CylinderShape() {
} }
// Return a local support point in a given direction // Return a local support point in a given direction
Vector3D CylinderShape::getLocalSupportPoint(const Vector3D& direction, double margin) const { Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0); assert(margin >= 0.0);
Vector3D supportPoint(0.0, 0.0, 0.0); Vector3 supportPoint(0.0, 0.0, 0.0);
double uDotv = direction.getY(); double uDotv = direction.getY();
Vector3D w(direction.getX(), 0.0, direction.getZ()); Vector3 w(direction.getX(), 0.0, direction.getZ());
double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ()); double lengthW = sqrt(direction.getX() * direction.getX() + direction.getZ() * direction.getZ());
if (lengthW != 0.0) { if (lengthW != 0.0) {
@ -62,7 +62,7 @@ Vector3D CylinderShape::getLocalSupportPoint(const Vector3D& direction, double m
// Add the margin to the support point // Add the margin to the support point
if (margin != 0.0) { if (margin != 0.0) {
Vector3D unitVec(0.0, 1.0, 0.0); Vector3 unitVec(0.0, 1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit(); unitVec = direction.getUnit();
} }

8
src/shapes/CylinderShape.h
View File

@ -56,8 +56,8 @@ class CylinderShape : public Shape {
void setRadius(double radius); // Set the radius void setRadius(double radius); // Set the radius
double getHeight() const; // Return the height double getHeight() const; // Return the height
void setHeight(double height); // Set the height void setHeight(double height); // Set the height
virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a support point in a given direction
virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
@ -86,8 +86,8 @@ inline void CylinderShape::setHeight(double height) {
} }
// Return the local extents in x,y and z direction // Return the local extents in x,y and z direction
inline Vector3D CylinderShape::getLocalExtents(double margin) const { inline Vector3 CylinderShape::getLocalExtents(double margin) const {
return Vector3D(radius + margin, halfHeight + margin, radius + margin); return Vector3(radius + margin, halfHeight + margin, radius + margin);
} }
}; // End of the ReactPhysics3D namespace }; // End of the ReactPhysics3D namespace

6
src/shapes/Shape.h
View File

@ -28,7 +28,7 @@
// Libraries // Libraries
#include <cassert> #include <cassert>
#include "../mathematics/Vector3D.h" #include "../mathematics/Vector3.h"
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -52,8 +52,8 @@ class Shape {
Body* getBodyPointer() const; // Return the body pointer Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* bodyPointer); // Set the body pointer void setBodyPointer(Body* bodyPointer); // Set the body pointer
virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a local support point in a given direction virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const=0; // Return a local support point in a given direction
virtual Vector3D getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction
}; };
// Return the body pointer // Return the body pointer

12
src/shapes/SphereShape.h
View File

@ -48,8 +48,8 @@ class SphereShape : public Shape {
double getRadius() const; // Return the radius of the sphere double getRadius() const; // Return the radius of the sphere
void setRadius(double radius); // Set the radius of the sphere void setRadius(double radius); // Set the radius of the sphere
virtual Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a local support point in a given direction virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
virtual Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the sphere (only for testing purpose) virtual void draw() const; // Draw the sphere (only for testing purpose)
@ -67,7 +67,7 @@ inline void SphereShape::setRadius(double radius) {
} }
// Return a local support point in a given direction // Return a local support point in a given direction
inline Vector3D SphereShape::getLocalSupportPoint(const Vector3D& direction, double margin) const { inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0); assert(margin >= 0.0);
double length = direction.length(); double length = direction.length();
@ -79,13 +79,13 @@ inline Vector3D SphereShape::getLocalSupportPoint(const Vector3D& direction, dou
// If the direction vector is the zero vector we return a point on the // If the direction vector is the zero vector we return a point on the
// boundary of the sphere // boundary of the sphere
return Vector3D(0, radius + margin, 0); 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 shape (half-width) in x,y and z local direction
// This method is used to compute the AABB of the box // This method is used to compute the AABB of the box
inline Vector3D SphereShape::getLocalExtents(double margin) const { inline Vector3 SphereShape::getLocalExtents(double margin) const {
return Vector3D(radius + margin, radius + margin, radius + margin); return Vector3(radius + margin, radius + margin, radius + margin);
} }
}; // End of the ReactPhysics3D namespace }; // End of the ReactPhysics3D namespace