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

sources/reactphysics3d/mathematics/Matrix3x3.cpp

@@ -37,6 +37,8 @@ Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, doub
 }
 
 // Copy-constructor
+// TODO : Test if this copy-constructor is correct (check if the the copy matrix use
+//        the same memory place for its array)
 Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) {
     // Copy the values in the matrix
     setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2],

sources/reactphysics3d/mathematics/Vector.h

@@ -52,6 +52,7 @@ class Vector {
         double scalarProduct(const Vector& vector) const throw(MathematicsException);   // Scalar product of two vectors
         Vector crossProduct(const Vector& vector) const throw(MathematicsException);    // Cross product of two vectors (in 3D only)
         void fillInSubVector(unsigned int index, const Vector& subVector);              // Replace a part of the current vector with another sub-vector
+        bool isUnit() const;                                                            // Return true if the vector is unit and false otherwise
 
         // --- Overloaded operators --- //
         Vector operator+(const Vector& vector) const throw(MathematicsException);   // Overloaded operator for addition
@@ -119,6 +120,11 @@ inline void Vector::fillInSubVector(unsigned int rowIndex, const Vector& subVect
     }
 }
 
+// Return true if the vector is unit and false otherwise
+inline bool Vector::isUnit() const {
+    return (1.0 - EPSILON <= this->length() && this->length() <= 1.0 + EPSILON );
+}
+
 // Overloaded operator for multiplication between a number and a Vector (inline)
 inline Vector operator*(double number, const Vector& vector) {
     // Compute and return the result

sources/reactphysics3d/mathematics/Vector3D.cpp

@@ -20,6 +20,8 @@
 // Libraries
 #include "Vector3D.h"
 #include <iostream>
+#include <cassert>
+#include <vector>
 
 // Namespaces
 using namespace reactphysics3d;
@@ -63,6 +65,34 @@ Vector3D Vector3D::getUnit() const throw(MathematicsException) {
     }
 }
 
+// Return two unit orthogonal vectors of the current vector
+// TODO : Test this method
+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

sources/reactphysics3d/mathematics/Vector3D.h

@@ -54,7 +54,10 @@ class Vector3D {
         void setAllValues(double x, double y, double z);        // Set all the values of the vector
         double length() const;                                  // Return the lenght 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 scalarProduct(const Vector3D& vector) const;     // Scalar product of two vectors
         Vector3D crossProduct(const Vector3D& vector) const;    // Cross product of two vectors
         bool isParallelWith(const Vector3D& vector) const;      // Return true if two vectors are parallel
@@ -67,7 +70,6 @@ class Vector3D {
         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;
@@ -134,6 +136,16 @@ inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
     return (approxEqual(abs(this->scalarProduct(vector)), length() * vector.length()));
 }
 
+// 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

sources/reactphysics3d/mathematics/mathematics_functions.h

@@ -28,7 +28,7 @@ namespace reactphysics3d {
 
 // ---------- Mathematics functions ---------- //
 
-// function to test if two numbers are (almost) equal
+// function to test if two real numbers are (almost) equal
 // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
 inline bool approxEqual(double a, double b) {
     double difference = a - b;