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 {
protected :
Vector3D linearVelocity; // Linear velocity of the body
Vector3D angularVelocity; // Angular velocity of the body
Vector3D externalForce; // Current external force on the body
Vector3D externalTorque; // Current external torque on the body
Vector3 linearVelocity; // Linear velocity of the body
Vector3 angularVelocity; // Angular velocity of the body
Vector3 externalForce; // Current external force on the body
Vector3 externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates)
double massInverse; // Inverse of the mass of the body
@ -57,15 +57,15 @@ class RigidBody : public Body {
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
Vector3D getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body
Vector3D getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity
Vector3 getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3& linearVelocity); // Set the linear velocity of the body
Vector3 getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3& angularVelocity); // Set the angular velocity
void setMassInverse(double massInverse); // Set the inverse of the mass
Vector3D getExternalForce() const; // Return the current external force of the body
void setExternalForce(const Vector3D& force); // Set the current external force on the body
Vector3D getExternalTorque() const; // Return the current external torque of the body
void setExternalTorque(const Vector3D& torque); // Set the current external torque of the body
Vector3 getExternalForce() const; // Return the current external force of the body
void setExternalForce(const Vector3& force); // Set the current external force on the body
Vector3 getExternalTorque() const; // Return the current external torque of the body
void setExternalTorque(const Vector3& torque); // Set the current external torque of the body
double getMassInverse() const; // Return the inverse of the mass of the body
Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates)
@ -78,16 +78,16 @@ class RigidBody : public Body {
};
// Return the linear velocity
inline Vector3D RigidBody::getLinearVelocity() const {
inline Vector3 RigidBody::getLinearVelocity() const {
return linearVelocity;
}
// Return the angular velocity of the body
inline Vector3D RigidBody::getAngularVelocity() const {
inline Vector3 RigidBody::getAngularVelocity() const {
return angularVelocity;
}
inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) {
inline void RigidBody::setAngularVelocity(const Vector3& angularVelocity) {
this->angularVelocity = angularVelocity;
}
@ -102,22 +102,22 @@ inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const {
}
// Return the external force on the body
inline Vector3D RigidBody::getExternalForce() const {
inline Vector3 RigidBody::getExternalForce() const {
return externalForce;
}
// Set the external force on the body
inline void RigidBody::setExternalForce(const Vector3D& force) {
inline void RigidBody::setExternalForce(const Vector3& force) {
this->externalForce = force;
}
// Return the current external torque on the body
inline Vector3D RigidBody::getExternalTorque() const {
inline Vector3 RigidBody::getExternalTorque() const {
return externalTorque;
}
// 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;
}
@ -155,7 +155,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
}
// 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 (isMotionEnabled) {
// 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
// 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) {
}
// Constructor for GJK
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2)
ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3& normal,
double penetrationDepth, const Vector3& point1, const Vector3& 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:
Body* const body1; // Pointer to the first body of the contact
Body* const body2; // Pointer to the second body of the contact
const Vector3D point1; // Contact point of body 1
const Vector3D point2; // Contact point of body 2
const Vector3D normal; // Normal vector the the collision contact
const Vector3 point1; // Contact point of body 1
const Vector3 point2; // Contact point of body 2
const Vector3 normal; // Normal vector the the collision 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
ContactInfo(Body* body1, Body* body2, const Vector3D& normal,
double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK
ContactInfo(Body* body1, Body* body2, const Vector3& normal,
double penetrationDepth, const Vector3& point1, const Vector3& point2); // Constructor for GJK
};
} // 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
// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
// the vertex that is wrong if the origin is not in the tetrahedron
int 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
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) {
return 4;
}
// 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) {
return 1;
}
// 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) {
return 2;
}
// 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) {
return 3;
}
@ -81,10 +81,10 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
const Shape* shape2, const Transform& transform2,
Vector3D& v, ContactInfo*& contactInfo) {
Vector3D 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
Vector3D points[MAX_SUPPORT_POINTS]; // Current points
Vector3& v, ContactInfo*& contactInfo) {
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
@ -125,7 +125,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// v1, v2 and v3.
// 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
int minAxis = d.getAbsoluteVector().getMinAxis();
@ -141,23 +141,23 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Matrix3x3 rotationMat = rotationQuat.getMatrix();
// Compute the vector v1, v2, v3
Vector3D v1 = d.cross(Vector3D(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3D v2 = rotationMat * v1;
Vector3D v3 = rotationMat * v2;
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationMat * v1;
Vector3 v3 = rotationMat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPoint(v1, OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN);
suppPointsB[2] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v1), OBJECT_MARGIN);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPoint(v2, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v2), OBJECT_MARGIN);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPoint(v3, OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN);
suppPointsB[4] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-v3), OBJECT_MARGIN);
points[4] = suppPointsA[4] - suppPointsB[4];
// 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.
// Compute the normal of the triangle
Vector3D v1 = points[1] - points[0];
Vector3D v2 = points[2] - points[0];
Vector3D n = v1.cross(v2);
Vector3 v1 = points[1] - points[0];
Vector3 v2 = points[2] - points[0];
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPoint(n, OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN);
suppPointsB[3] = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * (-n), OBJECT_MARGIN);
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);
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
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];
int indexNewVertex = nbVertices;
@ -368,9 +368,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Compute the contact info (in world-space)
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
Vector3D pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3D pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3D normal = v.getUnit();
Vector3 pA = transform1 * triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pB = transform1 * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
double penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
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,
uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap
int isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
const Vector3D& p3, const Vector3D& p4) const; // Decide if the origin is in the tetrahedron
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const; // Decide if the origin is in the tetrahedron
public:
EPAAlgorithm(); // Constructor
@ -84,7 +84,7 @@ class EPAAlgorithm {
bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1,
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

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

@ -59,7 +59,7 @@ uint EdgeEPA::getTargetVertexIndex() const {
}
// 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 (!ownerTriangle->getIsObsolete()) {
// 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
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
bool computeSilhouette(const Vector3D* vertices, uint index,
bool computeSilhouette(const Vector3* vertices, uint index,
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
bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
const Vector3D& p0 = vertices[indicesVertices[0]];
bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3& p0 = vertices[indicesVertices[0]];
Vector3D v1 = vertices[indicesVertices[1]] - p0;
Vector3D v2 = vertices[indicesVertices[2]] - p0;
Vector3 v1 = vertices[indicesVertices[1]] - p0;
Vector3 v2 = vertices[indicesVertices[2]] - p0;
double v1Dotv1 = v1.dot(v1);
double v1Dotv2 = v1.dot(v2);
double v2Dotv2 = v2.dot(v2);
@ -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,
// 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.
bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
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)
bool isObsolete; // True if the triangle face is visible from the new support point
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 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
@ -66,12 +66,12 @@ class TriangleEPA {
double getDistSquare() const; // Return the square distance of the closest point to origin
void setIsObsolete(bool isObsolete); // Set the isObsolete value
bool getIsObsolete() const; // Return true if the triangle face is obsolete
const 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 isVisibleFromVertex(const Vector3D* 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
Vector3D computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const; // Compute the point of an object closest to the origin
bool computeSilhouette(const Vector3D* vertices, uint index,
bool isVisibleFromVertex(const Vector3* vertices, uint index) const; // Return true if the triangle is visible from a given vertex
bool computeClosestPoint(const Vector3* vertices); // Compute the point v closest to the origin of this triangle
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const; // Compute the point of an object closest to the origin
bool computeSilhouette(const Vector3* vertices, uint index,
TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this triangle face
uint operator[](int i) const; // Access operator
@ -107,7 +107,7 @@ inline bool TriangleEPA::getIsObsolete() const {
}
// Return the point closest to the origin
inline const Vector3D& TriangleEPA::getClosestPoint() const {
inline const Vector3& TriangleEPA::getClosestPoint() const {
return closestPoint;
}
@ -117,14 +117,14 @@ inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3D* vertices, uint index) const {
Vector3D closestToVert = vertices[index] - closestPoint;
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
Vector3 closestToVert = vertices[index] - closestPoint;
return (closestPoint.dot(closestToVert) > 0.0);
}
// Compute the point of an object closest to the origin
inline Vector3D TriangleEPA::computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const {
const Vector3D& p0 = supportPointsOfObject[indicesVertices[0]];
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const {
const Vector3& p0 = supportPointsOfObject[indicesVertices[0]];
return p0 + 1.0/det * (lambda1 * (supportPointsOfObject[indicesVertices[1]] - 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
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
};
@ -85,7 +85,7 @@ inline TriangleEPA& TrianglesStore::last() {
}
// 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;
// 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);
Vector3D suppA; // Support point of object A
Vector3D suppB; // Support point of object B
Vector3D w; // Support point of Minkowski difference A-B
Vector3D pA; // Closest point of object A
Vector3D pB; // Closest point of object B
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
Vector3 w; // Support point of Minkowski difference A-B
Vector3 pA; // Closest point of object A
Vector3 pB; // Closest point of object B
double vDotw;
double prevDistSquare;
Body* const body1 = shape1->getBodyPointer();
@ -89,14 +89,14 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
// Get the last point V (last separating axis)
// TODO : Implement frame coherence. For each pair of body, store
// 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
double distSquare = DBL_MAX;
do {
// 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);
// 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);
// Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
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);
// Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
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);
// Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
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
prevDistSquare = distSquare;
distSquare = v.dot(v);
distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.dot(v);
distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
@ -199,7 +199,7 @@ bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform
pB = transform1 * (pB + (OBJECT_MARGIN / dist) * v);
// Compute the contact info
Vector3D normal = transform1.getOrientation().getMatrix() * v.getOpposite().getUnit();
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
double penetrationDepth = margin - dist;
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.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
const Shape* const shape2, const Transform& transform2,
ContactInfo*& contactInfo, Vector3D& v) {
ContactInfo*& contactInfo, Vector3& v) {
Simplex simplex;
Vector3D suppA;
Vector3D suppB;
Vector3D w;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
double vDotw;
double distSquare = DBL_MAX;
double prevDistSquare;
@ -239,7 +239,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const
do {
// 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);
// 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
prevDistSquare = distSquare;
distSquare = v.dot(v);
distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
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,
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 :
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
// suppPointB : support point of object B in a direction v
// 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());
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
bool Simplex::isPointInSimplex(const Vector3D& point) const {
bool Simplex::isPointInSimplex(const Vector3& point) const {
int i;
Bits bit;
@ -100,7 +100,7 @@ void Simplex::updateCache() {
// Compute the distance between two points in the possible simplex set
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
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
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;
int i;
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
// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B
// with lambda_i = deltaX_i / deltaX
void Simplex::computeClosestPointsOfAandB(Vector3D& pA, Vector3D& pB) const {
void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const {
double deltaX = 0.0;
pA.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 (overlap(bitsCurrentSimplex, bit)) {
deltaX += det[bitsCurrentSimplex][i];
pA = pA + det[bitsCurrentSimplex][i] * suppPointsA[i];
pB = pB + det[bitsCurrentSimplex][i] * suppPointsB[i];
pA += det[bitsCurrentSimplex][i] * suppPointsA[i];
pB += det[bitsCurrentSimplex][i] * suppPointsB[i];
}
}
assert(deltaX > 0.0);
double factor = 1.0 / deltaX;
pA = factor * pA;
pB = factor * pB;
pA *= factor;
pB *= factor;
}
// Compute the closest point "v" to the origin of the current simplex
// This method executes the Jonhnson's algorithm for computing the point
// "v" of simplex that is closest to the origin. The method returns true
// if a closest point has been found.
bool Simplex::computeClosestPoint(Vector3D& v) {
bool Simplex::computeClosestPoint(Vector3& v) {
Bits subset;
// For each possible simplex set
@ -325,13 +325,13 @@ bool Simplex::computeClosestPoint(Vector3D& v) {
}
// Backup the closest point
void Simplex::backupClosestPointInSimplex(Vector3D& v) {
void Simplex::backupClosestPointInSimplex(Vector3& v) {
double minDistSquare = DBL_MAX;
Bits bit;
for (bit = allBits; bit != 0x0; bit--) {
if (isSubset(bit, allBits) && isProperSubset(bit)) {
Vector3D u = computeClosestPointForSubset(bit);
Vector3 u = computeClosestPointForSubset(bit);
double distSquare = u.dot(u);
if (distSquare < minDistSquare) {
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
// represented by the bits "subset"
Vector3D Simplex::computeClosestPointForSubset(Bits subset) {
Vector3D v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
Vector3 Simplex::computeClosestPointForSubset(Bits subset) {
Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i])
maxLengthSquare = 0.0;
double deltaX = 0.0; // deltaX = sum of all det[subset][i]
int i;
@ -363,7 +363,7 @@ Vector3D Simplex::computeClosestPointForSubset(Bits subset) {
}
// 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 {
private:
Vector3D points[4]; // Current points
Vector3 points[4]; // Current points
double pointsLengthSquare[4]; // pointsLengthSquare[i] = (points[i].length)^2
double maxLengthSquare; // Maximum length of pointsLengthSquare[i]
Vector3D suppPointsA[4]; // Support points of object A in local coordinates
Vector3D suppPointsB[4]; // Support points of object B in local coordinates
Vector3D diffLength[4][4]; // diff[i][j] contains points[i] - points[j]
Vector3 suppPointsA[4]; // Support points of object A in local coordinates
Vector3 suppPointsB[4]; // Support points of object B in local coordinates
Vector3 diffLength[4][4]; // diff[i][j] contains points[i] - points[j]
double det[16][4]; // Cached determinant values
double normSquare[4][4]; // norm[i][j] = (diff[i][j].length())^2
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
void updateCache(); // Update the cached values used during the GJK algorithm
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:
Simplex(); // Constructor
@ -75,14 +75,14 @@ class Simplex {
bool isFull() const; // Return true if the simplex contains 4 points
bool isEmpty() const; // Return true if the simple is empty
unsigned int getSimplex(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
void addPoint(const Vector3D& point, const Vector3D& suppPointA, const Vector3D& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3D& point) const; // Return true if the point is in the simplex
void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); // Addd a point to the simplex
bool isPointInSimplex(const Vector3& point) const; // Return true if the point is in the simplex
bool isAffinelyDependent() const; // Return true if the set is affinely dependent
void backupClosestPointInSimplex(Vector3D& point); // Backup the closest point
void computeClosestPointsOfAandB(Vector3D& pA, Vector3D& 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
void backupClosestPointInSimplex(Vector3& point); // Backup the closest point
void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; // Compute the closest points of object A and B
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"

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
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
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
int i;
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
for (it2 = it + 1; it2 != sortedAABBs.end(); ++it2) {
// 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;
}

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
// axis.
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

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 min2; // Minimm 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
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
@ -109,7 +109,7 @@ bool SATAlgorithm::computeCollisionTest(const BoxShape* obb1, const Transform& t
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 = 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
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,
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
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 :
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
// 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) {
return axis;
}
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(body2);
Vector3D body1Position = body1->getTransform().getPosition();
Vector3D body2Position = body2->getTransform().getPosition();
Vector3 body1Position = body1->getTransform().getPosition();
Vector3 body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index
Vector3D r1 = pointOnBody1 - body1Position;
Vector3D r2 = pointOnBody2 - body2Position;
Vector3D r1CrossN = r1.cross(normal);
Vector3D r2CrossN = r2.cross(normal);
Vector3 r1 = pointOnBody1 - body1Position;
Vector3 r2 = pointOnBody2 - body2Position;
Vector3 r1CrossN = r1.cross(normal);
Vector3 r2CrossN = r2.cross(normal);
// Compute the jacobian matrix for the body 1 for the contact constraint
J_sp[currentIndex][0].setValue(0, -normal.getX());
@ -84,10 +84,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
currentIndex++;
// Compute the jacobian matrix for the body 1 for the first friction constraint
Vector3D r1CrossU1 = r1.cross(frictionVectors[0]);
Vector3D r2CrossU1 = r2.cross(frictionVectors[0]);
Vector3D r1CrossU2 = r1.cross(frictionVectors[1]);
Vector3D r2CrossU2 = r2.cross(frictionVectors[1]);
Vector3 r1CrossU1 = r1.cross(frictionVectors[0]);
Vector3 r2CrossU1 = r2.cross(frictionVectors[0]);
Vector3 r1CrossU2 = r1.cross(frictionVectors[1]);
Vector3 r2CrossU2 = r2.cross(frictionVectors[1]);
J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
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());
// Compute the error value for the contact constraint
Vector3D velocity1 = rigidBody1->getLinearVelocity();
Vector3D velocity2 = rigidBody2->getLinearVelocity();
Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3 velocity2 = rigidBody2->getLinearVelocity();
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
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 {
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 Vector3D pointOnBody1; // Contact point on body 1
const Vector3D pointOnBody2; // Contact point on body 2
std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
const Vector3 pointOnBody1; // Contact point on body 1
const Vector3 pointOnBody2; // Contact point on body 2
std::vector<Vector3> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
double mu_mc_g;
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
virtual ~Contact(); // Destructor
Vector3D getNormal() const; // Return the normal vector of the contact
Vector3D getPointOnBody1() const; // Return the contact point on body 1
Vector3D getPointOnBody2() const; // Return the contact point on body 2
Vector3 getNormal() const; // Return the normal vector of the contact
Vector3 getPointOnBody1() const; // Return the contact point on body 1
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 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
@ -84,7 +84,7 @@ inline void Contact::computeFrictionVectors() {
frictionVectors.clear();
// Compute the first orthogonal vector
Vector3D vector1 = normal.getOneOrthogonalVector();
Vector3 vector1 = normal.getOneOrthogonalVector();
frictionVectors.push_back(vector1);
// Compute the second orthogonal vector using the cross product
@ -92,17 +92,17 @@ inline void Contact::computeFrictionVectors() {
}
// Return the normal vector of the contact
inline Vector3D Contact::getNormal() const {
inline Vector3 Contact::getNormal() const {
return normal;
}
// Return the contact point on body 1
inline Vector3D Contact::getPointOnBody1() const {
inline Vector3 Contact::getPointOnBody1() const {
return pointOnBody1;
}
// Return the contact point on body 2
inline Vector3D Contact::getPointOnBody2() const {
inline Vector3 Contact::getPointOnBody2() const {
return pointOnBody2;
}

32
src/engine/ConstraintSolver.cpp
View File

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

12
src/engine/ConstraintSolver.h
View File

@ -98,8 +98,8 @@ class ConstraintSolver {
virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3D 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 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
void cleanup();
};
@ -112,18 +112,18 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
}
// 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));
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
inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(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

4
src/engine/ContactCache.cpp
View File

@ -71,7 +71,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
posZ = contact->getPointOnBody1().getZ();
// 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 (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE ||
@ -88,7 +88,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
posZ = contact->getPointOnBody2().getZ();
// 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 (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;
// 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) {
}

4
src/engine/ContactCachingInfo.h
View File

@ -43,10 +43,10 @@ struct ContactCachingInfo {
public:
Body* body1; // Body pointer of the first 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
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

99
src/engine/PhysicsEngine.cpp
View File

@ -30,19 +30,10 @@ using namespace reactphysics3d;
using namespace std;
// 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) {
// Check if the pointer to the world is not NULL
if (world == 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");
}
assert(world);
assert(timeStep > 0.0);
}
// Destructor
@ -51,52 +42,46 @@ PhysicsEngine::~PhysicsEngine() {
}
// Update the physics simulation
void PhysicsEngine::update() throw (logic_error) {
void PhysicsEngine::update() {
bool existCollision = false;
// Check that the timer is running
if (timer.getIsRunning()) {
assert(timer.getIsRunning());
// Compute the time since the last update() call and update the timer
timer.update();
// Compute the time since the last update() call and update the timer
timer.update();
// Apply the gravity force to all bodies
applyGravity();
// Apply the gravity force to all bodies
applyGravity();
// While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) {
existCollision = false;
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
// While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) {
existCollision = false;
// Solve constraints
constraintSolver.solve(timer.getTimeStep());
}
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
// Update the timer
timer.nextStep();
// 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();
// Solve constraints
constraintSolver.solve(timer.getTimeStep());
}
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
}
else { // Call to update() but the timer is not running
// Throw an exception
throw logic_error("Error : The PhysicsEngine::start() method have to be called before calling PhysicsEngine::update()");
// Update the timer
timer.nextStep();
// 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
setInterpolationFactorToAllBodies();
}
// 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
void PhysicsEngine::updateAllBodiesMotion() {
double dt = timer.getTimeStep();
Vector3D newLinearVelocity;
Vector3D newAngularVelocity;
Vector3 newLinearVelocity;
Vector3 newAngularVelocity;
// For each body of thephysics world
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
// external forces and torques.
newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Add the velocity V1 to the new velocity
newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity();
newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity();
newLinearVelocity += rigidBody->getLinearVelocity();
newAngularVelocity += rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
@ -152,7 +137,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
// Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body
void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) {
void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3& newLinVelocity, const Vector3& newAngVelocity) {
double dt = timer.getTimeStep();
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
@ -166,10 +151,10 @@ void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3
rigidBody->setAngularVelocity(newAngVelocity);
// 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();
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;
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);

12
src/engine/PhysicsEngine.h
View File

@ -50,17 +50,17 @@ class PhysicsEngine {
ConstraintSolver constraintSolver; // Constraint solver
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 applyGravity(); // Apply the gravity force to all bodies
public :
PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor
~PhysicsEngine(); // Destructor
PhysicsEngine(PhysicsWorld* world, double timeStep); // Constructor
~PhysicsEngine(); // Destructor
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update() throw (std::logic_error); // Update the physics simulation
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update(); // Update the physics simulation
};
// --- Inline functions --- //

2
src/engine/PhysicsWorld.cpp
View File

@ -31,7 +31,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
PhysicsWorld::PhysicsWorld(const Vector3D& gravity)
PhysicsWorld::PhysicsWorld(const Vector3& gravity)
: 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*> removedBodies; // Removed bodies since last update
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
public :
PhysicsWorld(const Vector3D& gravity); // Constructor
PhysicsWorld(const Vector3& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor
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 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
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
void addConstraint(Constraint* constraint); // Add a constraint
@ -129,7 +129,7 @@ inline void PhysicsWorld::clearAddedAndRemovedBodies() {
}
// Return the gravity vector of the world
inline Vector3D PhysicsWorld::getGravity() const {
inline Vector3 PhysicsWorld::getGravity() const {
return gravity;
}

13
src/mathematics/Matrix3x3.cpp
View File

@ -70,18 +70,5 @@ Matrix3x3 Matrix3x3::getInverse() const {
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
#include <cassert>
#include "Vector3D.h"
#include "Vector3.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -54,7 +54,7 @@ class Matrix3x3 {
void setValue(int i, int j, double value); // Set a value in the matrix
void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Set all the values in the matrix
Vector3D getColumn(int i) const; // Return a column
Vector3 getColumn(int i) const; // Return a column
Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix
@ -64,13 +64,19 @@ class Matrix3x3 {
static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- //
Matrix3x3 operator+(const Matrix3x3& matrix2) const; // Overloaded operator for addition
Matrix3x3 operator-(const Matrix3x3& matrix2) const ; // Overloaded operator for substraction
Matrix3x3 operator*(double nb) const; // Overloaded operator for multiplication with a number
Matrix3x3 operator*(const Matrix3x3& matrix2) const; // Overloaded operator for multiplication with a matrix
Vector3D operator*(const Vector3D& vector3d) const; // Overloaded operator for multiplication with a vector
Matrix3x3& operator=(const Matrix3x3& matrix2); // Overloaded operator for assignment
bool operator==(const Matrix3x3& matrix2) const; // Overloaded operator for equality condition
friend Matrix3x3 operator+(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for addition
friend Matrix3x3 operator-(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for substraction
friend Matrix3x3 operator-(const Matrix3x3& matrix); // Overloaded operator for the negative of the matrix
friend Matrix3x3 operator*(double nb, const Matrix3x3& matrix); // Overloaded operator for multiplication with a number
friend Matrix3x3 operator*(const Matrix3x3& matrix, double nb); // Overloaded operator for multiplication with a matrix
friend Matrix3x3 operator*(const Matrix3x3& matrix1, const Matrix3x3& matrix2); // Overloaded operator for matrix multiplication
friend Vector3 operator*(const Matrix3x3& matrix, const Vector3& vector); // Overloaded operator for multiplication with a vector
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
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[1][0] = b1; array[1][1] = b2; array[1][2] = b3;
array[2][0] = c1; array[2][1] = c2; array[2][2] = c3;
}
// Return a column
inline Vector3D Matrix3x3::getColumn(int i) const {
inline Vector3 Matrix3x3::getColumn(int i) const {
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
@ -121,27 +127,6 @@ inline double Matrix3x3::getTrace() const {
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
inline void Matrix3x3::setToIdentity() {
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]));
}
// 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
inline Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const {
// Return the sum matrix
return Matrix3x3(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]);
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[0][1], matrix1.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],
matrix1.array[2][0] + matrix2.array[2][0], matrix1.array[2][1] + matrix2.array[2][1], matrix1.array[2][2] + matrix2.array[2][2]);
}
// Overloaded operator for substraction
inline Matrix3x3 Matrix3x3::operator-(const Matrix3x3& matrix2) const {
// Return the substraction matrix
return Matrix3x3(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]);
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[0][1], matrix1.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],
matrix1.array[2][0] - matrix2.array[2][0], matrix1.array[2][1] - matrix2.array[2][1], matrix1.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
inline Matrix3x3 Matrix3x3::operator*(double nb) const {
// Return multiplied matrix
return Matrix3x3(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);
inline Matrix3x3 operator*(double nb, const Matrix3x3& matrix) {
return Matrix3x3(matrix.array[0][0] * nb, matrix.array[0][1] * nb, matrix.array[0][2] * nb,
matrix.array[1][0] * nb, matrix.array[1][1] * nb, matrix.array[1][2] * nb,
matrix.array[2][0] * nb, matrix.array[2][1] * nb, matrix.array[2][2] * nb);
}
// Overloaded operator for multiplication with a matrix
inline Matrix3x3 operator*(const Matrix3x3& matrix, double nb) {
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

8
src/mathematics/Quaternion.cpp
View File

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

12
src/mathematics/Quaternion.h
View File

@ -27,7 +27,7 @@
// Libraries
#include <cmath>
#include "Vector3D.h"
#include "Vector3.h"
#include "Matrix3x3.h"
#include "exceptions.h"
@ -50,7 +50,7 @@ class Quaternion {
public :
Quaternion(); // Constructor
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 Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor
@ -62,7 +62,7 @@ class Quaternion {
void setY(double y); // Set the value y
void setZ(double z); // Set the value z
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
Quaternion getUnit() const; // Return the unit 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
static Quaternion identity(); // Return the identity quaternion
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,
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
inline Vector3D Quaternion::vectorV() const {
inline Vector3 Quaternion::vectorV() const {
// Return the vector v
return Vector3D(x, y, z);
return Vector3(x, y, z);
}
// Return the length of the quaternion (inline)

6
src/mathematics/Transform.cpp
View File

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

26
src/mathematics/Transform.h
View File

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

2
src/mathematics/Vector.cpp
View File

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

4
src/mathematics/Vector.h
View File

@ -26,7 +26,7 @@
#define VECTOR_H
// Libraries
#include "Vector3D.h"
#include "Vector3.h"
#include "../constants.h"
#include "mathematics_functions.h"
#include "exceptions.h"
@ -52,7 +52,7 @@ class Vector {
Vector(); // Constructor without argument
Vector(int n) throw(std::invalid_argument); // 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
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

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 "Quaternion.h"
#include "Vector.h"
#include "Vector3D.h"
#include "Vector3.h"
#include "Transform.h"
#include "../constants.h"
#include "exceptions.h"
@ -47,7 +47,7 @@ namespace reactphysics3d {
// Rotate a vector according to a rotation quaternion.
// 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
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
// P1 = point1 + alpha * d1
// P2 = point2 + beta * d2
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2,
const reactphysics3d::Vector3D& d2, double* alpha, double* beta) {
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3& point1, const reactphysics3d::Vector3& d1, const reactphysics3d::Vector3& point2,
const reactphysics3d::Vector3& d2, double* alpha, double* beta) {
reactphysics3d::Vector3D r = point1 - point2;
reactphysics3d::Vector3 r = point1 - point2;
double a = d1.dot(d1);
double b = d1.dot(d2);
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
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"
reactphysics3d::Vector3D d = segPointB - segPointA;
reactphysics3d::Vector3D dP = P - segPointA;
reactphysics3d::Vector3 d = segPointB - segPointA;
reactphysics3d::Vector3 dP = P - segPointA;
if (!d.isParallelWith(dP)) {
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.
// 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,
const reactphysics3d::Vector3D& p2, const reactphysics3d::Vector3D& d2) {
inline reactphysics3d::Vector3 computeLinesIntersection(const reactphysics3d::Vector3& p1, const reactphysics3d::Vector3& d1,
const reactphysics3d::Vector3& p2, const reactphysics3d::Vector3& d2) {
// Computes the two closest points on the lines
double alpha, beta;
closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = p1 + alpha * d1;
reactphysics3d::Vector3D point2 = p2 + beta * d2;
reactphysics3d::Vector3 point1 = p1 + alpha * d1;
reactphysics3d::Vector3 point2 = p2 + beta * d2;
// The two points must be very close
//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.
// This method returns the intersection point.
inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB) {
inline reactphysics3d::Vector3 computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3& seg1PointA, const reactphysics3d::Vector3& seg1PointB,
const reactphysics3d::Vector3& seg2PointA, const reactphysics3d::Vector3& seg2PointB) {
// Determine the lines of both segments
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
reactphysics3d::Vector3 d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3 d2 = seg2PointB - seg2PointA;
// The segments must not be parallel
assert(!d1.isParallelWith(d2));
@ -140,15 +140,15 @@ inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const rea
// Compute the closet points between the two lines
double alpha, beta;
closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = seg1PointA + alpha * d1;
reactphysics3d::Vector3D point2 = seg2PointA + beta * d2;
reactphysics3d::Vector3 point1 = seg1PointA + alpha * d1;
reactphysics3d::Vector3 point2 = seg2PointA + beta * d2;
// The closest points have to be on the segments, otherwise there is no intersection between the segments
assert(isPointOnSegment(seg1PointA, seg1PointB, point1));
assert(isPointOnSegment(seg2PointA, seg2PointB, point2));
// 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);
// 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.
// 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) {
std::vector<reactphysics3d::Vector3D> result;
inline std::vector<reactphysics3d::Vector3> movePoints(const std::vector<reactphysics3d::Vector3>& points, const reactphysics3d::Vector3& vector) {
std::vector<reactphysics3d::Vector3> result;
// For each point of the set
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
// 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,
const reactphysics3d::Vector3D& normal) {
inline std::vector<reactphysics3d::Vector3> projectPointsOntoPlane(const std::vector<reactphysics3d::Vector3>& points, const reactphysics3d::Vector3& A,
const reactphysics3d::Vector3& normal) {
assert(normal.length() != 0.0);
std::vector<Vector3D> projectedPoints;
reactphysics3d::Vector3D n = normal.getUnit();
std::vector<Vector3> projectedPoints;
reactphysics3d::Vector3 n = normal.getUnit();
// For each point of the set
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")
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);
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")
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);
}
// 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 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);
double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
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 distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
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
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"
// 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.
inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB,
reactphysics3d::Vector3D& resultPointA, reactphysics3d::Vector3D& resultPointB) {
inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3& seg1PointA, const reactphysics3d::Vector3& seg1PointB,
const reactphysics3d::Vector3& seg2PointA, const reactphysics3d::Vector3& seg2PointB,
reactphysics3d::Vector3& resultPointA, reactphysics3d::Vector3& resultPointB) {
// Compute the segment vectors
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
reactphysics3d::Vector3 d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3 d2 = seg2PointB - seg2PointA;
// The two segments should be parallel
assert(d1.isParallelWith(d2));
@ -290,8 +290,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D&
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
reactphysics3d::Vector3D point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1);
reactphysics3d::Vector3D v = seg2PointA - point;
reactphysics3d::Vector3 point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1);
reactphysics3d::Vector3 v = seg2PointA - point;
// Return the segment intersection according to the configuration of two projection intervals
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.
// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle".
// This method returns the clipped segment.
inline std::vector<reactphysics3d::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;
assert(segment.size() == 2);
assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> inputSegment = segment;
std::vector<reactphysics3d::Vector3D> outputSegment;
std::vector<reactphysics3d::Vector3> inputSegment = segment;
std::vector<reactphysics3d::Vector3> outputSegment;
// For each edge of the clip rectangle
for (unsigned int i=0; i<4; ++i) {
@ -335,13 +335,13 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
// Current clipped segment
//assert(inputSegment.size() == 2);
reactphysics3d::Vector3D S = inputSegment[0];
reactphysics3d::Vector3D P = inputSegment[1];
reactphysics3d::Vector3 S = inputSegment[0];
reactphysics3d::Vector3 P = inputSegment[1];
// Edge of the clip rectangle
reactphysics3d::Vector3D A = clipRectangle[i];
reactphysics3d::Vector3D B = clipRectangle[ (i+1) % 4];
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
reactphysics3d::Vector3 A = clipRectangle[i];
reactphysics3d::Vector3 B = 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 (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
// 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(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
// 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(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
// a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that
// they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon.
inline std::vector<reactphysics3d::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;
assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> outputPolygon;
std::vector<reactphysics3d::Vector3D> inputPolygon = subjectPolygon;
std::vector<reactphysics3d::Vector3> outputPolygon;
std::vector<reactphysics3d::Vector3> inputPolygon = subjectPolygon;
// For each edge of the clip rectangle
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
// 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::Vector3D A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle"
reactphysics3d::Vector3D B = clipRectangle[(i+1) % 4];
reactphysics3d::Vector3D S = inputPolygon[0];
reactphysics3d::Vector3 planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
reactphysics3d::Vector3 A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle"
reactphysics3d::Vector3 B = clipRectangle[(i+1) % 4];
reactphysics3d::Vector3 S = inputPolygon[0];
// For each vertex of the subject polygon
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
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
// 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(P);
@ -415,7 +415,7 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
}
else if (planeNormal.dot(S-A) > 0.0) {
// 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);
}
@ -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
// the lineVector must not be orthogonal to the planeNormal.
inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector,
const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) {
inline reactphysics3d::Vector3 intersectLineWithPlane(const reactphysics3d::Vector3& linePoint, const reactphysics3d::Vector3& lineVector,
const reactphysics3d::Vector3& planePoint, const reactphysics3d::Vector3& planeNormal) {
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

2
src/shapes/AABB.cpp
View File

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

24
src/shapes/AABB.h
View File

@ -44,39 +44,39 @@ class Body;
*/
class AABB {
private :
Vector3D 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 minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis
Vector3 maxCoordinates; // Maximum world coordinates of the AABB on the x,y and z axis
Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable)
public :
AABB(); // Constructor
AABB(const Transform& transform, const Vector3D& extents); // Constructor
AABB(const Transform& transform, const Vector3& extents); // Constructor
virtual ~AABB(); // Destructor
Vector3D getCenter() const; // Return the center point
const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3D& getMaxCoordinates() const; // Return the maximum coordinates of the AABB
Vector3 getCenter() const; // Return the center point
const Vector3& getMinCoordinates() const; // Return the minimum coordinates of the AABB
const Vector3& getMaxCoordinates() const; // Return the maximum coordinates of the AABB
Body* getBodyPointer() const; // Return a pointer to the owner body
void setBodyPointer(Body* bodyPointer); // Set the body pointer
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
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
virtual void draw() const; // Draw the AABB (only for testing purpose)
#endif
};
// 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 the minimum coordinates of the AABB
inline const Vector3D& AABB::getMinCoordinates() const {
inline const Vector3& AABB::getMinCoordinates() const {
return minCoordinates;
}
// Return the maximum coordinates of the AABB
inline const Vector3D& AABB::getMaxCoordinates() const {
inline const Vector3& AABB::getMaxCoordinates() const {
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
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();
Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(extents),
Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
worldAxis.getColumn(1).dot(extents),
worldAxis.getColumn(2).dot(extents));
minCoordinates = newTransform.getPosition() - worldExtents;

2
src/shapes/BoxShape.cpp
View File

@ -36,7 +36,7 @@ using namespace reactphysics3d;
using namespace std;
// 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 {
private :
Vector3D extent; // Extent sizes of the box
Vector3 extent; // Extent sizes of the box
public :
BoxShape(const Vector3D& extent); // Constructor
BoxShape(const Vector3& extent); // Constructor
virtual ~BoxShape(); // Destructor
const Vector3D& getExtent() const; // Return the extents of the box
void setExtent(const Vector3D& 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 Vector3D getLocalSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a local support point in a given direction
const Vector3& getExtent() const; // Return the extents of the box
void setExtent(const Vector3& extent); // Set the extents of the box
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
#ifdef VISUAL_DEBUG
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
inline const Vector3D& BoxShape::getExtent() const {
inline const Vector3& BoxShape::getExtent() const {
return extent;
}
// Set the extents of the box
inline void BoxShape::setExtent(const Vector3D& extent) {
inline void BoxShape::setExtent(const Vector3& extent) {
this->extent = extent;
}
// 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
inline Vector3D BoxShape::getLocalExtents(double margin) const {
return extent + Vector3D(margin, margin, margin);
inline Vector3 BoxShape::getLocalExtents(double margin) const {
return extent + Vector3(margin, margin, margin);
}
// 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);
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.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
inline Vector3D ConeShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0);
const Vector3D& v = direction;
const Vector3& v = direction;
double sinThetaTimesLengthV = sinTheta * v.length();
Vector3D supportPoint;
Vector3 supportPoint;
if (v.getY() >= sinThetaTimesLengthV) {
supportPoint = Vector3D(0.0, halfHeight, 0.0);
supportPoint = Vector3(0.0, halfHeight, 0.0);
}
else {
double projectedLength = sqrt(v.getX() * v.getX() + v.getZ() * v.getZ());
if (projectedLength > MACHINE_EPSILON) {
double d = radius / projectedLength;
supportPoint = Vector3D(v.getX() * d, -halfHeight, v.getZ() * d);
supportPoint = Vector3(v.getX() * d, -halfHeight, v.getZ() * d);
}
else {
supportPoint = Vector3D(radius, -halfHeight, 0.0);
supportPoint = Vector3(radius, -halfHeight, 0.0);
}
}
// Add the margin to the support point
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) {
unitVec = v.getUnit();
}

11
src/shapes/ConeShape.h
View File

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

6
src/shapes/Shape.h
View File

@ -28,7 +28,7 @@
// Libraries
#include <cassert>
#include "../mathematics/Vector3D.h"
#include "../mathematics/Vector3.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -52,8 +52,8 @@ class Shape {
Body* getBodyPointer() const; // Return 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 Vector3D getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const=0; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const=0; // Return the local extents in x,y and z direction
};
// 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
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 Vector3D getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
virtual Vector3 getLocalSupportPoint(const Vector3& direction, double margin=0.0) const; // Return a local support point in a given direction
virtual Vector3 getLocalExtents(double margin=0.0) const; // Return the local extents in x,y and z direction
#ifdef VISUAL_DEBUG
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
inline Vector3D SphereShape::getLocalSupportPoint(const Vector3D& direction, double margin) const {
inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, double margin) const {
assert(margin >= 0.0);
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
// 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
// This method is used to compute the AABB of the box
inline Vector3D SphereShape::getLocalExtents(double margin) const {
return Vector3D(radius + margin, radius + margin, radius + margin);
inline Vector3 SphereShape::getLocalExtents(double margin) const {
return Vector3(radius + margin, radius + margin, radius + margin);
}
}; // End of the ReactPhysics3D namespace