git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@222 92aac97c-a6ce-11dd-a772-7fcde58d38e6

sources/reactphysics3d/body/OBB.cpp

@@ -19,9 +19,12 @@
 
 // Libraries
 #include "OBB.h"
-
+#include <vector>
 #include <GL/freeglut.h>        // TODO : Remove this in the final version
 #include <GL/gl.h>              // TODO : Remove this in the final version
+#include <cassert>
+#include <cstdlib>
+#include <iostream>             // TODO : Delete this
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -124,7 +127,56 @@ void OBB::draw() const {
         glVertex3f(s6.getX(), s6.getY(), s6.getZ());
 
         glVertex3f(center.getX(), center.getY(), center.getZ());
-        glVertex3f(center.getX() + 8.0 * axis[2].getX(), center.getY() + 8.0 * axis[2].getY(), center.getZ() + 8.0 * axis[2].getZ());
+        glVertex3f(center.getX() + 8.0 * axis[1].getX(), center.getY() + 8.0 * axis[1].getY(), center.getZ() + 8.0 * axis[1].getZ());
 
     glEnd();
 }
+
+// Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis.
+// The function returns the number of extreme vertices and a set that contains thoses vertices.
+int OBB::getExtremeVertices(const Vector3D projectionAxis, std::vector<Vector3D>& extremeVertices) const {
+    assert(extremeVertices.size() == 0);
+    assert(projectionAxis.length() != 0);
+
+    double maxProjectionLength = 0.0;     // Longest projection length of a vertex onto the projection axis
+
+    // Compute the vertices of the OBB
+    double e0 = extent[0];
+    double e1 = extent[1];
+    double e2 = extent[2];
+    Vector3D vertices[8];
+    vertices[0] = center + (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2);
+    vertices[1] = center + (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2);
+    vertices[2] = center - (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2);
+    vertices[3] = center - (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2);
+    vertices[4] = center + (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2);
+    vertices[5] = center + (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2);
+    vertices[6] = center - (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2);
+    vertices[7] = center - (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2);
+
+    for (int i=0; i<8; ++i) {
+        // Compute the projection length of the current vertex onto the projection axis
+        double projectionLength = projectionAxis.scalarProduct(vertices[i]-center) / projectionAxis.length();
+
+        //std::cout << "Projection length : " << projectionLength << std::endl;
+        //std::cout << "Max Projection length : " << maxProjectionLength << std::endl;
+
+        // If we found a bigger projection length
+        if (projectionLength > maxProjectionLength + EPSILON) {
+            maxProjectionLength = projectionLength;
+            extremeVertices.clear();
+            extremeVertices.push_back(vertices[i]);
+            //std::cout << "PRINT 1" << std::endl;
+        }
+        else if (equal(projectionLength, maxProjectionLength)) {
+            extremeVertices.push_back(vertices[i]);
+            //std::cout << "PRINT 2" << std::endl;
+        }
+    }
+
+    // An extreme should be a unique vertex, an edge or a face
+    assert(extremeVertices.size() == 1 || extremeVertices.size() == 2 || extremeVertices.size() == 4);
+
+    // Return the number of extreme vertices
+    return extremeVertices.size();
+}

sources/reactphysics3d/body/OBB.h

@@ -56,13 +56,11 @@ class OBB : public BoundingVolume {
         void setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument);                            // Set an axis
         double getExtent(unsigned int index) const throw(std::invalid_argument);                                        // Return an extent value
         void setExtent(unsigned int index, double extent) throw(std::invalid_argument);                                 // Set an extent value
-        virtual void updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion);    // Update the orientation of the OBB according to the orientation of the rigid body                                                                      // Update the oriented bounding box orientation according to a new orientation of the rigid body
+        virtual int getExtremeVertices(const Vector3D projectionAxis, std::vector<Vector3D>& extremeVertices) const;    // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis
+        virtual void updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion);                // Update the oriented bounding box orientation according to a new orientation of the rigid body
         virtual void draw() const;                                                                                      // Draw the OBB (only for testing purpose)
 };
 
-// TODO : Don't forget that we need to code a way that a change in the orientation of a rigid body imply
-//        a change in the orientation (center and axis) of the corresponding OBB.
-
 // Return the center point of the OBB
 inline Vector3D OBB::getCenter() const {
     return center;

sources/reactphysics3d/body/RigidBody.cpp

@@ -25,19 +25,18 @@
 
  // Constructor
  RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensor, const OBB& obb)
-           : Body(mass), inertiaTensor(inertiaTensor), obb(obb),
+           : Body(mass), inertiaTensor(inertiaTensor), currentBodyState(position, orientation, inertiaTensor.getInverse(), Kilogram(1.0/mass.getValue())),
-             currentBodyState(position, orientation, inertiaTensor.getInverse(),Kilogram(1.0/mass.getValue())),
+             previousBodyState(position, orientation, inertiaTensor.getInverse(), Kilogram(1.0/mass.getValue())), obb(obb) {
-             previousBodyState(position, orientation, inertiaTensor.getInverse(), Kilogram(1.0/mass.getValue())) {
 
         isMotionEnabled = true;
         isCollisionEnabled = true;
         interpolationFactor = 0.0;
 
-        // Set the body pointer to the OBB
-        this->obb.setBodyPointer(this);
-
         // Update the orientation of the OBB according to the orientation of the rigid body
         this->update();
+
+        // Set the body pointer to the OBB
+        this->obb.setBodyPointer(this);
  }
 
  // Copy-constructor
@@ -48,12 +47,12 @@
     interpolationFactor = rigidBody.interpolationFactor;
  }
 
- // Destructor
+// Destructor
- RigidBody::~RigidBody() {
+RigidBody::~RigidBody() {
 
- };
+};
 
- // Compute the linear interpolation state between the previous body state and the current body state
+// Compute the linear interpolation state between the previous body state and the current body state
 // This is used to avoid visual stuttering when the display and physics framerates are out of synchronization
 BodyState RigidBody::getInterpolatedState() const {
 

sources/reactphysics3d/body/RigidBody.h

@@ -56,10 +56,12 @@ class RigidBody : public Body {
         Matrix3x3 getInertiaTensor() const;                             // Return the inertia tensor of the body
         void setInertiaTensor(const Matrix3x3& inertiaTensor);          // Set the inertia tensor of the body
         BodyState& getCurrentBodyState();                               // Return a reference to the current state of the body
+        BodyState& getPreviousBodyState();                              // TODO : DELETE THIS
         void setInterpolationFactor(double factor);                     // Set the interpolation factor of the body
         BodyState getInterpolatedState() const;                         // Compute and return the interpolated state
         bool getIsMotionEnabled() const;                                // Return if the rigid body can move
         void setIsMotionEnabled(bool isMotionEnabled);                 // Set the value to true if the body can move
+        void setLinearVelocity(const Vector3D& linearVelocity);         // Set the linear velocity of the rigid body
         void updatePreviousBodyState();                                 // Update the previous body state of the body
         OBB getOBB() const;                                             // Return the oriented bounding box of the rigid body
         void update();                                                  // Update the rigid body in order to reflect a change in the body state
@@ -83,6 +85,12 @@ inline BodyState& RigidBody::getCurrentBodyState() {
     return currentBodyState;
 }
 
+// TODO : DELETE THIS
+inline BodyState& RigidBody::getPreviousBodyState() {
+    return previousBodyState;
+}
+
+
 // Set the interpolation factor of the body
 inline void RigidBody::setInterpolationFactor(double factor) {
     assert(factor >= 0 && factor <= 1);
@@ -101,6 +109,15 @@ inline void RigidBody::setIsMotionEnabled(bool isMotionEnabled) {
     this->isMotionEnabled = isMotionEnabled;
 }
 
+// Set the linear velocity of the rigid body
+inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) {
+    // Update the linear velocity of the current body state
+    currentBodyState.setLinearVelocity(linearVelocity);
+
+    // Update the linear momentum of the current body state
+    currentBodyState.setLinearMomentum(linearVelocity * (1.0/currentBodyState.getMassInverse().getValue()));
+}
+
 // Update the previous body state of the body
 inline void RigidBody::updatePreviousBodyState() {
     // The current body state becomes the previous body state