From 53ef470c32ce4ca93cddf6ef862296fc7d244cf8 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Sun, 28 Mar 2010 21:55:15 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@298 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../reactphysics3d/mathematics/Matrix3x3.cpp | 2 ++ sources/reactphysics3d/mathematics/Vector.h | 6 ++++ .../reactphysics3d/mathematics/Vector3D.cpp | 30 +++++++++++++++++++ sources/reactphysics3d/mathematics/Vector3D.h | 14 ++++++++- .../mathematics/mathematics_functions.h | 2 +- 5 files changed, 52 insertions(+), 2 deletions(-) diff --git a/sources/reactphysics3d/mathematics/Matrix3x3.cpp b/sources/reactphysics3d/mathematics/Matrix3x3.cpp index decabaca..bda11b1d 100644 --- a/sources/reactphysics3d/mathematics/Matrix3x3.cpp +++ b/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], diff --git a/sources/reactphysics3d/mathematics/Vector.h b/sources/reactphysics3d/mathematics/Vector.h index 1c510e76..dada8095 100644 --- a/sources/reactphysics3d/mathematics/Vector.h +++ b/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 diff --git a/sources/reactphysics3d/mathematics/Vector3D.cpp b/sources/reactphysics3d/mathematics/Vector3D.cpp index 3c91725a..43e4c04a 100644 --- a/sources/reactphysics3d/mathematics/Vector3D.cpp +++ b/sources/reactphysics3d/mathematics/Vector3D.cpp @@ -20,6 +20,8 @@ // Libraries #include "Vector3D.h" #include +#include +#include // 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 diff --git a/sources/reactphysics3d/mathematics/Vector3D.h b/sources/reactphysics3d/mathematics/Vector3D.h index c617ff89..691ab8d2 100644 --- a/sources/reactphysics3d/mathematics/Vector3D.h +++ b/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 diff --git a/sources/reactphysics3d/mathematics/mathematics_functions.h b/sources/reactphysics3d/mathematics/mathematics_functions.h index 91511ad8..e2f0693d 100644 --- a/sources/reactphysics3d/mathematics/mathematics_functions.h +++ b/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;