diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index ec87faf5..9cf944f0 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -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 diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp index 2d3a46db..d5964e31 100644 --- a/src/collision/ContactInfo.cpp +++ b/src/collision/ContactInfo.cpp @@ -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) { } diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h index 53735b8f..e0771d09 100644 --- a/src/collision/ContactInfo.h +++ b/src/collision/ContactInfo.h @@ -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 diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index fd4ebac3..1a55671f 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -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); diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h index 1650f0c7..d8ed53a4 100644 --- a/src/collision/EPA/EPAAlgorithm.h +++ b/src/collision/EPA/EPAAlgorithm.h @@ -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 diff --git a/src/collision/EPA/EdgeEPA.cpp b/src/collision/EPA/EdgeEPA.cpp index 2ad83c1c..b6e60910 100644 --- a/src/collision/EPA/EdgeEPA.cpp +++ b/src/collision/EPA/EdgeEPA.cpp @@ -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 diff --git a/src/collision/EPA/EdgeEPA.h b/src/collision/EPA/EdgeEPA.h index 5914ba14..0d5dc7d0 100644 --- a/src/collision/EPA/EdgeEPA.h +++ b/src/collision/EPA/EdgeEPA.h @@ -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 }; diff --git a/src/collision/EPA/TriangleEPA.cpp b/src/collision/EPA/TriangleEPA.cpp index 03f4ca2d..b851dd52 100644 --- a/src/collision/EPA/TriangleEPA.cpp +++ b/src/collision/EPA/TriangleEPA.cpp @@ -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(); diff --git a/src/collision/EPA/TriangleEPA.h b/src/collision/EPA/TriangleEPA.h index 3cea9e45..e8250064 100644 --- a/src/collision/EPA/TriangleEPA.h +++ b/src/collision/EPA/TriangleEPA.h @@ -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)); } diff --git a/src/collision/EPA/TrianglesStore.h b/src/collision/EPA/TrianglesStore.h index 0cdd7294..a8d0f72b 100644 --- a/src/collision/EPA/TrianglesStore.h +++ b/src/collision/EPA/TrianglesStore.h @@ -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 diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp index ec455042..be34b2c0 100644 --- a/src/collision/GJK/GJKAlgorithm.cpp +++ b/src/collision/GJK/GJKAlgorithm.cpp @@ -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; diff --git a/src/collision/GJK/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h index bdb326de..82035e97 100644 --- a/src/collision/GJK/GJKAlgorithm.h +++ b/src/collision/GJK/GJKAlgorithm.h @@ -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 diff --git a/src/collision/GJK/Simplex.cpp b/src/collision/GJK/Simplex.cpp index db6971b0..65ed5ff0 100644 --- a/src/collision/GJK/Simplex.cpp +++ b/src/collision/GJK/Simplex.cpp @@ -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]; } } diff --git a/src/collision/GJK/Simplex.h b/src/collision/GJK/Simplex.h index 4807cb72..1b668c0f 100644 --- a/src/collision/GJK/Simplex.h +++ b/src/collision/GJK/Simplex.h @@ -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" diff --git a/src/collision/SAPAlgorithm.cpp b/src/collision/SAPAlgorithm.cpp index f2f008cc..4408c735 100644 --- a/src/collision/SAPAlgorithm.cpp +++ b/src/collision/SAPAlgorithm.cpp @@ -84,7 +84,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector 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::iterator it; // Iterator on the sortedAABBs set vector::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 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; } diff --git a/src/collision/SAPAlgorithm.h b/src/collision/SAPAlgorithm.h index 107064d2..462b9f51 100644 --- a/src/collision/SAPAlgorithm.h +++ b/src/collision/SAPAlgorithm.h @@ -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 diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp index 136cd230..ccd5cc48 100644 --- a/src/collision/SATAlgorithm.cpp +++ b/src/collision/SATAlgorithm.cpp @@ -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) { diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h index dfc37e00..ef8a6532 100644 --- a/src/collision/SATAlgorithm.h +++ b/src/collision/SATAlgorithm.h @@ -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; } } diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index b76ed659..fc2442ee 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -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; diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index f6d8af26..7acef065 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -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 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 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; } diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 40cfca40..53d3a40c 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -241,25 +241,29 @@ void ConstraintSolver::fillInMatrices() { // TODO : Use polymorphism and remove this downcasting rigidBody = dynamic_cast(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 points; + vector points; vector lambdas; points.push_back(contact->getPointOnBody1()); points.push_back(contact->getPointOnBody2()); diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 4867f961..46c0f5c0 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -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 diff --git a/src/engine/ContactCache.cpp b/src/engine/ContactCache.cpp index 9c739358..31c35851 100644 --- a/src/engine/ContactCache.cpp +++ b/src/engine/ContactCache.cpp @@ -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 || diff --git a/src/engine/ContactCachingInfo.cpp b/src/engine/ContactCachingInfo.cpp index b3b65090..11f7353a 100644 --- a/src/engine/ContactCachingInfo.cpp +++ b/src/engine/ContactCachingInfo.cpp @@ -29,7 +29,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector& positions, const vector& lambdas) +ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector& positions, const vector& lambdas) : body1(body1), body2(body2), positions(positions), lambdas(lambdas) { } diff --git a/src/engine/ContactCachingInfo.h b/src/engine/ContactCachingInfo.h index 29eb0c56..019080a6 100644 --- a/src/engine/ContactCachingInfo.h +++ b/src/engine/ContactCachingInfo.h @@ -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 positions; // Positions of the contact points + std::vector positions; // Positions of the contact points std::vector lambdas; // Last lambdas value for the constraint - ContactCachingInfo(Body* body1, Body* body2, const std::vector& positions, const std::vector& lambdas); // Constructor + ContactCachingInfo(Body* body1, Body* body2, const std::vector& positions, const std::vector& lambdas); // Constructor }; } // End of the ReactPhysics3D namespace diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp index 79952a8e..7744a9b8 100644 --- a/src/engine/PhysicsEngine.cpp +++ b/src/engine/PhysicsEngine.cpp @@ -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::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(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); diff --git a/src/engine/PhysicsEngine.h b/src/engine/PhysicsEngine.h index 5335134d..00690209 100644 --- a/src/engine/PhysicsEngine.h +++ b/src/engine/PhysicsEngine.h @@ -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 --- // diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index 54782415..c6fc619b 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -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) { } diff --git a/src/engine/PhysicsWorld.h b/src/engine/PhysicsWorld.h index b46de269..33f4e283 100644 --- a/src/engine/PhysicsWorld.h +++ b/src/engine/PhysicsWorld.h @@ -49,17 +49,17 @@ class PhysicsWorld { std::vector addedBodies; // Added bodies since last update std::vector removedBodies; // Removed bodies since last update std::vector 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; } diff --git a/src/mathematics/Matrix3x3.cpp b/src/mathematics/Matrix3x3.cpp index 7453d9da..6b73aafc 100644 --- a/src/mathematics/Matrix3x3.cpp +++ b/src/mathematics/Matrix3x3.cpp @@ -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; -} - diff --git a/src/mathematics/Matrix3x3.h b/src/mathematics/Matrix3x3.h index 865bf882..6126b7f5 100644 --- a/src/mathematics/Matrix3x3.h +++ b/src/mathematics/Matrix3x3.h @@ -28,7 +28,7 @@ // Libraries #include -#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 diff --git a/src/mathematics/Quaternion.cpp b/src/mathematics/Quaternion.cpp index fa4b532a..1f6a27d8 100755 --- a/src/mathematics/Quaternion.cpp +++ b/src/mathematics/Quaternion.cpp @@ -24,7 +24,7 @@ // Libraries #include "Quaternion.h" -#include "Vector3D.h" +#include "Vector3.h" #include // 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(); diff --git a/src/mathematics/Quaternion.h b/src/mathematics/Quaternion.h index 6e92be05..201db7fc 100644 --- a/src/mathematics/Quaternion.h +++ b/src/mathematics/Quaternion.h @@ -27,7 +27,7 @@ // Libraries #include -#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) diff --git a/src/mathematics/Transform.cpp b/src/mathematics/Transform.cpp index 3805461c..79f2d804 100644 --- a/src/mathematics/Transform.cpp +++ b/src/mathematics/Transform.cpp @@ -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; } diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index cc92f190..f22cbee3 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -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; } diff --git a/src/mathematics/Vector.cpp b/src/mathematics/Vector.cpp index 76c23b79..7326bd88 100644 --- a/src/mathematics/Vector.cpp +++ b/src/mathematics/Vector.cpp @@ -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]; diff --git a/src/mathematics/Vector.h b/src/mathematics/Vector.h index 7eb53aec..beacbfbe 100644 --- a/src/mathematics/Vector.h +++ b/src/mathematics/Vector.h @@ -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 diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp new file mode 100644 index 00000000..a4cfc52b --- /dev/null +++ b/src/mathematics/Vector3.cpp @@ -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 +#include +#include + +// 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; +} diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h new file mode 100644 index 00000000..1fe138e0 --- /dev/null +++ b/src/mathematics/Vector3.h @@ -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 +#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 diff --git a/src/mathematics/Vector3D.cpp b/src/mathematics/Vector3D.cpp deleted file mode 100644 index 40de9453..00000000 --- a/src/mathematics/Vector3D.cpp +++ /dev/null @@ -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 -#include -#include - -// 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; -} diff --git a/src/mathematics/Vector3D.h b/src/mathematics/Vector3D.h deleted file mode 100644 index 96b4be2a..00000000 --- a/src/mathematics/Vector3D.h +++ /dev/null @@ -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 -#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 diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index f13ef901..4e183f82 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -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 movePoints(const std::vector& points, const reactphysics3d::Vector3D& vector) { - std::vector result; +inline std::vector movePoints(const std::vector& points, const reactphysics3d::Vector3& vector) { + std::vector result; // For each point of the set for (unsigned int i=0; i movePoints(const std::vector projectPointsOntoPlane(const std::vector& points, const reactphysics3d::Vector3D& A, - const reactphysics3d::Vector3D& normal) { +inline std::vector projectPointsOntoPlane(const std::vector& points, const reactphysics3d::Vector3& A, + const reactphysics3d::Vector3& normal) { assert(normal.length() != 0.0); - std::vector projectedPoints; - reactphysics3d::Vector3D n = normal.getUnit(); + std::vector projectedPoints; + reactphysics3d::Vector3 n = normal.getUnit(); // For each point of the set for (unsigned int i=0; i 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 rectangle) { +inline reactphysics3d::Vector3 computeNearestPointOnRectangle(const reactphysics3d::Vector3& P, const std::vector 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 clipSegmentWithRectangleInPlane(const std::vector& segment, const std::vector clipRectangle) { +inline std::vector clipSegmentWithRectangleInPlane(const std::vector& segment, const std::vector clipRectangle) { double const epsilon = 0.01; assert(segment.size() == 2); assert(clipRectangle.size() == 4); - std::vector inputSegment = segment; - std::vector outputSegment; + std::vector inputSegment = segment; + std::vector outputSegment; // For each edge of the clip rectangle for (unsigned int i=0; i<4; ++i) { @@ -335,13 +335,13 @@ inline std::vector 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 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 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 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 clipPolygonWithRectangleInPlane(const std::vector& subjectPolygon, const std::vector& clipRectangle) { +inline std::vector clipPolygonWithRectangleInPlane(const std::vector& subjectPolygon, const std::vector& clipRectangle) { double const epsilon = 0.1; assert(clipRectangle.size() == 4); - std::vector outputPolygon; - std::vector inputPolygon = subjectPolygon; + std::vector outputPolygon; + std::vector inputPolygon = subjectPolygon; // For each edge of the clip rectangle for (unsigned int i=0; i<4; ++i) { @@ -389,14 +389,14 @@ inline std::vector 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 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 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 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 diff --git a/src/shapes/AABB.cpp b/src/shapes/AABB.cpp index 70ab1c07..e5375654 100644 --- a/src/shapes/AABB.cpp +++ b/src/shapes/AABB.cpp @@ -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); } diff --git a/src/shapes/AABB.h b/src/shapes/AABB.h index df4f9b89..b845b546 100644 --- a/src/shapes/AABB.h +++ b/src/shapes/AABB.h @@ -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; diff --git a/src/shapes/BoxShape.cpp b/src/shapes/BoxShape.cpp index 2d695d3b..c43d6510 100644 --- a/src/shapes/BoxShape.cpp +++ b/src/shapes/BoxShape.cpp @@ -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) { } diff --git a/src/shapes/BoxShape.h b/src/shapes/BoxShape.h index 8ac194f4..97be5ca9 100644 --- a/src/shapes/BoxShape.h +++ b/src/shapes/BoxShape.h @@ -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); } diff --git a/src/shapes/ConeShape.cpp b/src/shapes/ConeShape.cpp index c0861ba7..de556ba4 100644 --- a/src/shapes/ConeShape.cpp +++ b/src/shapes/ConeShape.cpp @@ -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(); } diff --git a/src/shapes/ConeShape.h b/src/shapes/ConeShape.h index 3fd58cfc..f9eca349 100644 --- a/src/shapes/ConeShape.h +++ b/src/shapes/ConeShape.h @@ -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 diff --git a/src/shapes/CylinderShape.cpp b/src/shapes/CylinderShape.cpp index b78984c8..9a39027b 100644 --- a/src/shapes/CylinderShape.cpp +++ b/src/shapes/CylinderShape.cpp @@ -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(); } diff --git a/src/shapes/CylinderShape.h b/src/shapes/CylinderShape.h index 8b1180fb..de67bc35 100644 --- a/src/shapes/CylinderShape.h +++ b/src/shapes/CylinderShape.h @@ -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 diff --git a/src/shapes/Shape.h b/src/shapes/Shape.h index e6fafb8a..cac4ce49 100644 --- a/src/shapes/Shape.h +++ b/src/shapes/Shape.h @@ -28,7 +28,7 @@ // Libraries #include -#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 diff --git a/src/shapes/SphereShape.h b/src/shapes/SphereShape.h index 6dc876ca..d57d8739 100644 --- a/src/shapes/SphereShape.h +++ b/src/shapes/SphereShape.h @@ -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