From ddf602c125b2040b8ffa577dd8ca24fe43c0bd32 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Tue, 14 Sep 2010 19:30:24 +0000 Subject: [PATCH] Optimizations in the constraint solver git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@405 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/constraint/Constraint.h | 2 +- src/constraint/Contact.cpp | 86 ++++---- src/constraint/Contact.h | 2 +- src/engine/ConstraintSolver.cpp | 55 +++-- src/engine/ConstraintSolver.h | 6 +- src/mathematics/Matrix.cpp | 53 +++++ src/mathematics/Matrix.h | 6 + src/mathematics/Matrix1x6.cpp | 52 +++++ src/mathematics/Matrix1x6.h | 138 ++++++++++++ src/mathematics/Matrix6x6.cpp | 46 ++++ src/mathematics/Matrix6x6.h | 197 ++++++++++++++++++ src/mathematics/Vector6D.cpp | 54 +++++ src/mathematics/Vector6D.h | 112 ++++++++++ .../lcp/LCPProjectedGaussSeidel.cpp | 26 +-- src/mathematics/lcp/LCPProjectedGaussSeidel.h | 6 +- src/mathematics/lcp/LCPSolver.h | 2 +- 16 files changed, 757 insertions(+), 86 deletions(-) create mode 100644 src/mathematics/Matrix1x6.cpp create mode 100644 src/mathematics/Matrix1x6.h create mode 100644 src/mathematics/Matrix6x6.cpp create mode 100644 src/mathematics/Matrix6x6.h create mode 100644 src/mathematics/Vector6D.cpp create mode 100644 src/mathematics/Vector6D.h diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index bba2033d..0aa5b623 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -53,7 +53,7 @@ class Constraint { Body* const getBody1() const; // Return the reference to the body 1 Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 - virtual void computeJacobian(int noConstraint, Matrix**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints + virtual void computeJacobian(int noConstraint, Matrix1x6**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const=0; // Compute the lowerbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index 30ff9176..55d073f3 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -51,7 +51,7 @@ Contact::~Contact() { // fill in this matrix with all the jacobian matrix of the mathematical constraint // of the contact. The argument "noConstraint", is the row were the method have // to start to fill in the J_sp matrix. -void Contact::computeJacobian(int noConstraint, Matrix**& J_sp) const { +void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { RigidBody* rigidBody1 = dynamic_cast(body1); RigidBody* rigidBody2 = dynamic_cast(body2); Vector3D r1; @@ -78,22 +78,22 @@ void Contact::computeJacobian(int noConstraint, Matrix**& J_sp) const { r2CrossN = r2.crossProduct(normal); // Compute the jacobian matrix for the body 1 for the contact constraint - J_sp[currentIndex][0].changeSize(1, 6); - J_sp[currentIndex][0].setValue(0, 0, -normal.getX()); - J_sp[currentIndex][0].setValue(0, 1, -normal.getY()); - J_sp[currentIndex][0].setValue(0, 2, -normal.getZ()); - J_sp[currentIndex][0].setValue(0, 3, -r1CrossN.getX()); - J_sp[currentIndex][0].setValue(0, 4, -r1CrossN.getY()); - J_sp[currentIndex][0].setValue(0, 5, -r1CrossN.getZ()); + //J_sp[currentIndex][0].changeSize(1, 6); + J_sp[currentIndex][0].setValue(0, -normal.getX()); + J_sp[currentIndex][0].setValue(1, -normal.getY()); + J_sp[currentIndex][0].setValue(2, -normal.getZ()); + J_sp[currentIndex][0].setValue(3, -r1CrossN.getX()); + J_sp[currentIndex][0].setValue(4, -r1CrossN.getY()); + J_sp[currentIndex][0].setValue(5, -r1CrossN.getZ()); // Compute the jacobian matrix for the body 2 for the contact constraint - J_sp[currentIndex][1].changeSize(1, 6); - J_sp[currentIndex][1].setValue(0, 0, normal.getX()); - J_sp[currentIndex][1].setValue(0, 1, normal.getY()); - J_sp[currentIndex][1].setValue(0, 2, normal.getZ()); - J_sp[currentIndex][1].setValue(0, 3, r2CrossN.getX()); - J_sp[currentIndex][1].setValue(0, 4, r2CrossN.getY()); - J_sp[currentIndex][1].setValue(0, 5, r2CrossN.getZ()); + //J_sp[currentIndex][1].changeSize(1, 6); + J_sp[currentIndex][1].setValue(0, normal.getX()); + J_sp[currentIndex][1].setValue(1, normal.getY()); + J_sp[currentIndex][1].setValue(2, normal.getZ()); + J_sp[currentIndex][1].setValue(3, r2CrossN.getX()); + J_sp[currentIndex][1].setValue(4, r2CrossN.getY()); + J_sp[currentIndex][1].setValue(5, r2CrossN.getZ()); currentIndex++; @@ -102,42 +102,42 @@ void Contact::computeJacobian(int noConstraint, Matrix**& J_sp) const { r2CrossU1 = r2.crossProduct(frictionVectors[0]); r1CrossU2 = r1.crossProduct(frictionVectors[1]); r2CrossU2 = r2.crossProduct(frictionVectors[1]); - J_sp[currentIndex][0].changeSize(1, 6); - J_sp[currentIndex][0].setValue(0, 0, -frictionVectors[0].getX()); - J_sp[currentIndex][0].setValue(0, 1, -frictionVectors[0].getY()); - J_sp[currentIndex][0].setValue(0, 2, -frictionVectors[0].getZ()); - J_sp[currentIndex][0].setValue(0, 3, -r1CrossU1.getX()); - J_sp[currentIndex][0].setValue(0, 4, -r1CrossU1.getY()); - J_sp[currentIndex][0].setValue(0, 5, -r1CrossU1.getZ()); + //J_sp[currentIndex][0].changeSize(1, 6); + 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()); + J_sp[currentIndex][0].setValue(3, -r1CrossU1.getX()); + J_sp[currentIndex][0].setValue(4, -r1CrossU1.getY()); + J_sp[currentIndex][0].setValue(5, -r1CrossU1.getZ()); // Compute the jacobian matrix for the body 2 for the first friction constraint - J_sp[currentIndex][1].changeSize(1, 6); - J_sp[currentIndex][1].setValue(0, 0, frictionVectors[0].getX()); - J_sp[currentIndex][1].setValue(0, 1, frictionVectors[0].getY()); - J_sp[currentIndex][1].setValue(0, 2, frictionVectors[0].getZ()); - J_sp[currentIndex][1].setValue(0, 3, r2CrossU1.getX()); - J_sp[currentIndex][1].setValue(0, 4, r2CrossU1.getY()); - J_sp[currentIndex][1].setValue(0, 5, r2CrossU1.getZ()); + //J_sp[currentIndex][1].changeSize(1, 6); + J_sp[currentIndex][1].setValue(0, frictionVectors[0].getX()); + J_sp[currentIndex][1].setValue(1, frictionVectors[0].getY()); + J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ()); + J_sp[currentIndex][1].setValue(3, r2CrossU1.getX()); + J_sp[currentIndex][1].setValue(4, r2CrossU1.getY()); + J_sp[currentIndex][1].setValue(5, r2CrossU1.getZ()); currentIndex++; // Compute the jacobian matrix for the body 1 for the second friction constraint - J_sp[currentIndex][0].changeSize(1, 6); - J_sp[currentIndex][0].setValue(0, 0, -frictionVectors[1].getX()); - J_sp[currentIndex][0].setValue(0, 1, -frictionVectors[1].getY()); - J_sp[currentIndex][0].setValue(0, 2, -frictionVectors[1].getZ()); - J_sp[currentIndex][0].setValue(0, 3, -r1CrossU2.getX()); - J_sp[currentIndex][0].setValue(0, 4, -r1CrossU2.getY()); - J_sp[currentIndex][0].setValue(0, 5, -r1CrossU2.getZ()); - J_sp[currentIndex][1].changeSize(1, 6); + //J_sp[currentIndex][0].changeSize(1, 6); + J_sp[currentIndex][0].setValue(0, -frictionVectors[1].getX()); + J_sp[currentIndex][0].setValue(1, -frictionVectors[1].getY()); + J_sp[currentIndex][0].setValue(2, -frictionVectors[1].getZ()); + J_sp[currentIndex][0].setValue(3, -r1CrossU2.getX()); + J_sp[currentIndex][0].setValue(4, -r1CrossU2.getY()); + J_sp[currentIndex][0].setValue(5, -r1CrossU2.getZ()); + //J_sp[currentIndex][1].changeSize(1, 6); // Compute the jacobian matrix for the body 2 for the second friction constraint - J_sp[currentIndex][1].setValue(0, 0, frictionVectors[1].getX()); - J_sp[currentIndex][1].setValue(0, 1, frictionVectors[1].getY()); - J_sp[currentIndex][1].setValue(0, 2, frictionVectors[1].getZ()); - J_sp[currentIndex][1].setValue(0, 3, r2CrossU2.getX()); - J_sp[currentIndex][1].setValue(0, 4, r2CrossU2.getY()); - J_sp[currentIndex][1].setValue(0, 5, r2CrossU2.getZ()); + J_sp[currentIndex][1].setValue(0, frictionVectors[1].getX()); + J_sp[currentIndex][1].setValue(1, frictionVectors[1].getY()); + J_sp[currentIndex][1].setValue(2, frictionVectors[1].getZ()); + J_sp[currentIndex][1].setValue(3, r2CrossU2.getX()); + J_sp[currentIndex][1].setValue(4, r2CrossU2.getY()); + J_sp[currentIndex][1].setValue(5, r2CrossU2.getZ()); currentIndex++; } diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 01d1d440..569e9a55 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -69,7 +69,7 @@ class Contact : public Constraint { Vector3D getNormal() const; // Return the normal vector of the contact Vector3D getPoint(int index) const; // Return a contact point int getNbPoints() const; // Return the number of contact points - virtual void computeJacobian(int noConstraint, Matrix**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints + 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 virtual void computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 0800a7d0..92d51c02 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -113,7 +113,7 @@ void ConstraintSolver::allocate() { freeMemory(true); bodiesCapacity = nbBodies; - Minv_sp = new Matrix[nbBodies]; + Minv_sp = new Matrix6x6[nbBodies]; V1 = new Vector[nbBodies]; Vconstraint = new Vector[nbBodies]; Fext = new Vector[nbBodies]; @@ -128,13 +128,13 @@ void ConstraintSolver::allocate() { constraintsCapacity = nbConstraints; bodyMapping = new Body**[nbConstraints]; - J_sp = new Matrix*[nbConstraints]; - B_sp = new Matrix*[2]; - B_sp[0] = new Matrix[nbConstraints]; - B_sp[1] = new Matrix[nbConstraints]; + J_sp = new Matrix1x6*[nbConstraints]; + B_sp = new Vector6D*[2]; + B_sp[0] = new Vector6D[nbConstraints]; + B_sp[1] = new Vector6D[nbConstraints]; for (int i=0; i::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) { body = *it; @@ -261,13 +261,26 @@ void ConstraintSolver::fillInMatrices() { Fext[bodyNumber] = f; // Compute the inverse sparse mass matrix - mInv.initWithValue(0.0); + Minv_sp[bodyNumber].initWithValue(0.0); + const Matrix3x3& tensorInv = rigidBody->getInertiaTensorInverseWorld(); if (rigidBody->getIsMotionEnabled()) { - mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity); - mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); + Minv_sp[bodyNumber].setValue(0, 0, rigidBody->getMassInverse()); + Minv_sp[bodyNumber].setValue(1, 1, rigidBody->getMassInverse()); + Minv_sp[bodyNumber].setValue(2, 2, rigidBody->getMassInverse()); + Minv_sp[bodyNumber].setValue(3, 3, tensorInv.getValue(0, 0)); + Minv_sp[bodyNumber].setValue(3, 4, tensorInv.getValue(0, 1)); + Minv_sp[bodyNumber].setValue(3, 5, tensorInv.getValue(0, 2)); + Minv_sp[bodyNumber].setValue(4, 3, tensorInv.getValue(1, 0)); + Minv_sp[bodyNumber].setValue(4, 4, tensorInv.getValue(1, 1)); + Minv_sp[bodyNumber].setValue(4, 5, tensorInv.getValue(1, 2)); + Minv_sp[bodyNumber].setValue(5, 3, tensorInv.getValue(2, 0)); + Minv_sp[bodyNumber].setValue(5, 4, tensorInv.getValue(2, 1)); + Minv_sp[bodyNumber].setValue(5, 5, tensorInv.getValue(2, 2)); + //mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity); + //mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld()); } - Minv_sp[bodyNumber].changeSize(6, 6); - Minv_sp[bodyNumber] = mInv; + //Minv_sp[bodyNumber].changeSize(6, 6); + //Minv_sp[bodyNumber] = mInv; } } @@ -282,12 +295,12 @@ void ConstraintSolver::computeVectorB(double dt) { // Substract 1.0/dt*J*V to the vector b indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; - b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]).getValue(0,0) * oneOverDT); - b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]).getValue(0,0) * oneOverDT); + b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][0]) * V1[indexBody1]).getValue(0,0) * oneOverDT); // TODO : Remove conversion here + b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][1]) * V1[indexBody2]).getValue(0,0) * oneOverDT); // Substract J*M^-1*F_ext to the vector b - b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1] - + (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]).getValue(0,0)); + b.setValue(c, b.getValue(c) - ((Matrix(J_sp[c][0]) * Matrix(Minv_sp[indexBody1])) * Fext[indexBody1] + + (Matrix(J_sp[c][1]) * Matrix(Minv_sp[indexBody2]))*Fext[indexBody2]).getValue(0,0)); // TODO : Delete conversion here } } @@ -299,8 +312,8 @@ void ConstraintSolver::computeMatrixB_sp() { for (uint c = 0; c #include @@ -54,6 +57,9 @@ class Matrix { Matrix(const Matrix& matrix); // Copy constructor of the class Matrix Matrix(const Matrix3x3& matrix); // Conversion from Matrix3x3 Matrix(const Vector& vector); // Conversion from Vector to Matrix + Matrix(const Matrix6x6& matrix); + Matrix(const Matrix1x6& matrix); + Matrix(const Vector6D& vector); virtual ~Matrix(); // Destructor of the class Matrix double getValue(int i, int j) const throw(std::invalid_argument); // Return a value in the matrix void setValue(int i, int j, double value) throw(std::invalid_argument); // Set a value in the matrix diff --git a/src/mathematics/Matrix1x6.cpp b/src/mathematics/Matrix1x6.cpp new file mode 100644 index 00000000..06d48933 --- /dev/null +++ b/src/mathematics/Matrix1x6.cpp @@ -0,0 +1,52 @@ +/******************************************************************************** +* 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 "Matrix1x6.h" + +// Namespaces +using namespace reactphysics3d; + +// Constructor +Matrix1x6::Matrix1x6() { + +} + +// Constructor +Matrix1x6::Matrix1x6(double value) { + initWithValue(value); +} + +// Constructor +Matrix1x6::Matrix1x6(double v1, double v2, double v3, double v4, double v5, double v6) { + array[0] = v1; array[1] = v2; array[2] = v3; array[3] = v4; array[4] = v5; array[5] = v6; +} + +// Destructor +Matrix1x6::~Matrix1x6() { + +} + + + diff --git a/src/mathematics/Matrix1x6.h b/src/mathematics/Matrix1x6.h new file mode 100644 index 00000000..db4cb593 --- /dev/null +++ b/src/mathematics/Matrix1x6.h @@ -0,0 +1,138 @@ +/******************************************************************************** +* 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 MATRIX1X6_H +#define MATRIX1X6_H + +// Libraries +#include + +#include "Matrix6x6.h" +#include "Vector6D.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Matrix1x6 : + This class represents a 1x6 matrix. + ------------------------------------------------------------------- +*/ +class Matrix1x6 { + private : + double array[6]; // Array with the values of the matrix + + public : + Matrix1x6(); // Constructor + Matrix1x6(double value); // Constructor + Matrix1x6(double v1, double v2, double v3, double v4, + double v5, double v6); // Constructor + virtual ~Matrix1x6(); // Destructor + + double getValue(int i) const; // Get a value in the matrix + void setValue(int i, double value); // Set a value in the matrix + void initWithValue(double value); // Init all the matrix value with the same value + Vector6D getTranspose() const; // Return the transpose matrix + + // --- Overloaded operators --- // + Matrix1x6 operator+(const Matrix1x6& matrix2) const; // Overloaded operator for addition + Matrix1x6 operator-(const Matrix1x6& matrix2) const ; // Overloaded operator for substraction + Matrix1x6 operator*(double nb) const; // Overloaded operator for multiplication with a number + Matrix1x6 operator*(const Matrix6x6& matrix) const; // Overloaded operator for multiplication with a Matrix6x6 + double operator*(const Vector6D& vector6d) const; // Overloaded operator for multiplication with a Vector6D +}; + + +// Get a value in the matrix +inline double Matrix1x6::getValue(int i) const { + assert(i>=0 && i<6); + return array[i]; +} + +// Set a value in the matrix +inline void Matrix1x6::setValue(int i, double value) { + assert(i>=0 && i<6); + array[i] = value; +} + +// Init all the matrix value with the same value +inline void Matrix1x6::initWithValue(double value) { + array[0] = value; array[1] = value; array[2] = value; + array[3] = value; array[4] = value; array[5] = value; +} + +// Return the transpose matrix +inline Vector6D Matrix1x6::getTranspose() const { + return Vector6D(array[0], array[1], array[2], array[3], array[4], array[5]); +} + +// Overloaded operator for multiplication between a number and a Matrix6x6 +inline Matrix1x6 operator*(double value, const Matrix1x6& matrix) { + return matrix * value; +} + +// Overloaded operator for multiplication with a Vector6D +inline double Matrix1x6::operator*(const Vector6D& vector) const { + return (array[0] * vector.getValue(0) + array[1] * vector.getValue(1) + array[2] * vector.getValue(2) + + array[3] * vector.getValue(3) + array[4] * vector.getValue(4) + array[5] * vector.getValue(5)); +} + +// Overloaded operator for substraction +inline Matrix1x6 Matrix1x6::operator-(const Matrix1x6& matrix) const { + return (array[0] - matrix.array[0], array[1] - matrix.array[1], array[2] - matrix.array[2], + array[3] - matrix.array[3], array[4] - matrix.array[4], array[5] - matrix.array[5]); +} + +// Overloaded operator for addition +inline Matrix1x6 Matrix1x6::operator+(const Matrix1x6& matrix) const { + return (array[0] + matrix.array[0], array[1] + matrix.array[1], array[2] + matrix.array[2], + array[3] + matrix.array[3], array[4] + matrix.array[4], array[5] + matrix.array[5]); +} + +// Overloaded operator for multiplication with a number +inline Matrix1x6 Matrix1x6::operator*(double value) const { + return (array[0] * value, array[1] * value, array[2] * value, + array[3] * value, array[4] * value, array[5] * value); +} + +// Overloaded operator for multiplication with a 6x6 matrix +inline Matrix1x6 Matrix1x6::operator*(const Matrix6x6& m) const { + return Matrix1x6(array[0] * m.getValue(0,0) + array[1] * m.getValue(1,0) + array[2] * m.getValue(2,0) + + array[3] * m.getValue(3,0) + array[4] * m.getValue(4,0) + array[5] * m.getValue(5,0), + array[0] * m.getValue(0,1) + array[1] * m.getValue(1,1) + array[2] * m.getValue(2,1) + + array[3] * m.getValue(3,1) + array[4] * m.getValue(4,1) + array[5] * m.getValue(5,1), + array[0] * m.getValue(0,2) + array[1] * m.getValue(1,2) + array[2] * m.getValue(2,2) + + array[3] * m.getValue(3,2) + array[4] * m.getValue(4,2) + array[5] * m.getValue(5,2), + array[0] * m.getValue(0,3) + array[1] * m.getValue(1,3) + array[2] * m.getValue(2,3) + + array[3] * m.getValue(3,3) + array[4] * m.getValue(4,3) + array[5] * m.getValue(5,3), + array[0] * m.getValue(0,4) + array[1] * m.getValue(1,4) + array[2] * m.getValue(2,4) + + array[3] * m.getValue(3,4) + array[4] * m.getValue(4,4) + array[5] * m.getValue(5,4), + array[0] * m.getValue(0,5) + array[1] * m.getValue(1,5) + array[2] * m.getValue(2,5) + + array[3] * m.getValue(3,5) + array[4] * m.getValue(4,5) + array[5] * m.getValue(5,5)); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/mathematics/Matrix6x6.cpp b/src/mathematics/Matrix6x6.cpp new file mode 100644 index 00000000..8c84c69f --- /dev/null +++ b/src/mathematics/Matrix6x6.cpp @@ -0,0 +1,46 @@ +/******************************************************************************** +* 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 "Matrix6x6.h" + +// Namespaces +using namespace reactphysics3d; + +// Constructor +Matrix6x6::Matrix6x6() { + +} + +// Constructor +Matrix6x6::Matrix6x6(double value) { + initWithValue(value); +} + +// Destructor +Matrix6x6::~Matrix6x6() { + +} + + diff --git a/src/mathematics/Matrix6x6.h b/src/mathematics/Matrix6x6.h new file mode 100644 index 00000000..ee9a84e4 --- /dev/null +++ b/src/mathematics/Matrix6x6.h @@ -0,0 +1,197 @@ +/******************************************************************************** +* 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 MATRIX6X6_H +#define MATRIX6X6_H + +// Libraries +#include +#include "Vector6D.h" + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Matrix6x6 : + This class represents a 6x6 matrix. + ------------------------------------------------------------------- +*/ +class Matrix6x6 { + private : + double array[6][6]; // Array with the values of the matrix + + public : + Matrix6x6(); // Constructor + Matrix6x6(double value); // Constructor + virtual ~Matrix6x6(); // Destructor + + double getValue(int i, int j) const; // Get a value in the matrix + void setValue(int i, int j, double value); // Set a value in the matrix + void initWithValue(double value); // Init all the values with a unique value + Matrix6x6 getTranspose() const; // Return the transpose matrix + static Matrix6x6 identity(); // Return the identity matrix + + // --- Overloaded operators --- // + Matrix6x6 operator+(const Matrix6x6& matrix2) const; // Overloaded operator for addition + Matrix6x6 operator-(const Matrix6x6& matrix2) const; // Overloaded operator for substraction + Matrix6x6 operator*(double nb) const; // Overloaded operator for multiplication with a number + Vector6D operator*(const Vector6D& vector6d) const; // Overloaded operator for multiplication with a Vector6D +}; + + +// Get a value in the matrix +inline double Matrix6x6::getValue(int i, int j) const { + assert(i>=0 && i<6 && j>=0 && j<6); + return array[i][j]; +} + +// Set a value in the matrix +inline void Matrix6x6::setValue(int i, int j, double value) { + array[i][j] = value; +} + +// Init all the values with a unique value +inline void Matrix6x6::initWithValue(double value) { + array[0][0] = value; array[0][1] = value; array[0][2] = value; + array[0][3] = value; array[0][4] = value; array[0][5] = value; + array[1][0] = value; array[1][1] = value; array[1][2] = value; + array[1][3] = value; array[1][4] = value; array[1][5] = value; + array[2][0] = value; array[2][1] = value; array[2][2] = value; + array[2][3] = value; array[2][4] = value; array[2][5] = value; + array[3][0] = value; array[3][1] = value; array[3][2] = value; + array[3][3] = value; array[3][4] = value; array[3][5] = value; + array[4][0] = value; array[4][1] = value; array[4][2] = value; + array[4][3] = value; array[4][4] = value; array[4][5] = value; + array[5][0] = value; array[5][1] = value; array[5][2] = value; + array[5][3] = value; array[5][4] = value; array[5][5] = value; +} + +// Return the transpose matrix +inline Matrix6x6 Matrix6x6::getTranspose() const { + Matrix6x6 transpose; + + transpose.array[0][0] = array[0][0]; transpose.array[0][1] = array[1][0]; transpose.array[0][2] = array[2][0]; + transpose.array[0][3] = array[3][0]; transpose.array[0][4] = array[4][0]; transpose.array[0][5] = array[5][0]; + transpose.array[1][0] = array[0][1]; transpose.array[1][1] = array[1][1]; transpose.array[1][2] = array[2][1]; + transpose.array[1][3] = array[3][1]; transpose.array[1][4] = array[4][1]; transpose.array[1][5] = array[5][1]; + transpose.array[2][0] = array[0][2]; transpose.array[2][1] = array[1][2]; transpose.array[2][2] = array[2][2]; + transpose.array[2][3] = array[3][2]; transpose.array[2][4] = array[4][2]; transpose.array[2][5] = array[5][2]; + transpose.array[3][0] = array[0][3]; transpose.array[3][1] = array[1][3]; transpose.array[3][2] = array[2][3]; + transpose.array[3][3] = array[3][3]; transpose.array[3][4] = array[4][3]; transpose.array[3][5] = array[5][3]; + transpose.array[4][0] = array[0][4]; transpose.array[4][1] = array[1][4]; transpose.array[4][2] = array[2][4]; + transpose.array[4][3] = array[3][4]; transpose.array[4][4] = array[4][4]; transpose.array[4][5] = array[5][4]; + transpose.array[5][0] = array[0][5]; transpose.array[5][1] = array[1][5]; transpose.array[5][2] = array[2][5]; + transpose.array[5][3] = array[3][5]; transpose.array[5][4] = array[4][5]; transpose.array[5][5] = array[5][5]; + return transpose; +} + +// Return the identity matrix +inline Matrix6x6 Matrix6x6::identity() { + Matrix6x6 identity(0.0); + identity.array[0][0] = 1.0; identity.array[1][1] = 1.0; identity.array[2][2] = 1.0; + identity.array[3][3] = 1.0; identity.array[4][4] = 1.0; identity.array[5][5] = 1.0; + return identity; +} + +// Overloaded operator for multiplication between a number and a Matrix6x6 +inline Matrix6x6 operator*(double value, const Matrix6x6& matrix) { + return matrix * value; +} + +// Overloaded operator for multiplication with a Vector6D +inline Vector6D Matrix6x6::operator*(const Vector6D& vector6d) const { + Vector6D result; + result.setValue(0, array[0][0] * vector6d.getValue(0) + array[0][1] * vector6d.getValue(1) + array[0][2] * vector6d.getValue(2) + + array[0][3] * vector6d.getValue(3) + array[0][4] * vector6d.getValue(4) + array[0][5] * vector6d.getValue(5)); + result.setValue(1, array[1][0] * vector6d.getValue(0) + array[1][1] * vector6d.getValue(1) + array[1][2] * vector6d.getValue(2) + + array[1][3] * vector6d.getValue(3) + array[1][4] * vector6d.getValue(4) + array[1][5] * vector6d.getValue(5)); + result.setValue(2, array[2][0] * vector6d.getValue(0) + array[2][1] * vector6d.getValue(1) + array[2][2] * vector6d.getValue(2) + + array[2][3] * vector6d.getValue(3) + array[2][4] * vector6d.getValue(4) + array[2][5] * vector6d.getValue(5)); + result.setValue(3, array[3][0] * vector6d.getValue(0) + array[3][1] * vector6d.getValue(1) + array[3][2] * vector6d.getValue(2) + + array[3][3] * vector6d.getValue(3) + array[3][4] * vector6d.getValue(4) + array[3][5] * vector6d.getValue(5)); + result.setValue(4, array[4][0] * vector6d.getValue(0) + array[4][1] * vector6d.getValue(1) + array[4][2] * vector6d.getValue(2) + + array[4][3] * vector6d.getValue(3) + array[4][4] * vector6d.getValue(4) + array[4][5] * vector6d.getValue(5)); + result.setValue(5, array[5][0] * vector6d.getValue(0) + array[5][1] * vector6d.getValue(1) + array[5][2] * vector6d.getValue(2) + + array[5][3] * vector6d.getValue(3) + array[5][4] * vector6d.getValue(4) + array[5][5] * vector6d.getValue(5)); + return result; +} + +// Overloaded operator for substraction +inline Matrix6x6 Matrix6x6::operator-(const Matrix6x6& matrix) const { + Matrix6x6 result; + result.array[0][0] = array[0][0] - matrix.array[0][0]; result.array[0][1] = array[0][1] - matrix.array[0][1]; result.array[0][2] = array[0][2] - matrix.array[0][2]; + result.array[0][3] = array[0][3] - matrix.array[0][3]; result.array[0][4] = array[0][4] - matrix.array[0][4]; result.array[0][5] = array[0][5] - matrix.array[0][5]; + result.array[1][0] = array[1][0] - matrix.array[1][0]; result.array[1][1] = array[1][1] - matrix.array[1][1]; result.array[1][2] = array[1][2] - matrix.array[1][2]; + result.array[1][3] = array[1][3] - matrix.array[1][3]; result.array[1][4] = array[1][4] - matrix.array[1][4]; result.array[1][5] = array[1][5] - matrix.array[1][5]; + result.array[2][0] = array[2][0] - matrix.array[2][0]; result.array[2][1] = array[2][1] - matrix.array[2][1]; result.array[2][2] = array[2][2] - matrix.array[2][2]; + result.array[2][3] = array[2][3] - matrix.array[2][3]; result.array[2][4] = array[2][4] - matrix.array[2][4]; result.array[2][5] = array[2][5] - matrix.array[2][5]; + result.array[3][0] = array[3][0] - matrix.array[3][0]; result.array[3][1] = array[3][1] - matrix.array[3][1]; result.array[3][2] = array[3][2] - matrix.array[3][2]; + result.array[3][3] = array[3][3] - matrix.array[3][3]; result.array[3][4] = array[3][4] - matrix.array[3][4]; result.array[3][5] = array[3][5] - matrix.array[3][5]; + result.array[4][0] = array[4][0] - matrix.array[4][0]; result.array[4][1] = array[4][1] - matrix.array[4][1]; result.array[4][2] = array[4][2] - matrix.array[4][2]; + result.array[4][3] = array[4][3] - matrix.array[4][3]; result.array[4][4] = array[4][4] - matrix.array[4][4]; result.array[4][5] = array[4][5] - matrix.array[4][5]; + result.array[5][0] = array[5][0] - matrix.array[5][0]; result.array[5][1] = array[5][1] - matrix.array[5][1]; result.array[5][2] = array[5][2] - matrix.array[5][2]; + result.array[5][3] = array[5][3] - matrix.array[5][3]; result.array[5][4] = array[5][4] - matrix.array[5][4]; result.array[5][5] = array[5][5] - matrix.array[5][5]; + return result; +} + +// Overloaded operator for addition +inline Matrix6x6 Matrix6x6::operator+(const Matrix6x6& matrix) const { + Matrix6x6 result; + result.array[0][0] = array[0][0] + matrix.array[0][0]; result.array[0][1] = array[0][1] + matrix.array[0][1]; result.array[0][2] = array[0][2] + matrix.array[0][2]; + result.array[0][3] = array[0][3] + matrix.array[0][3]; result.array[0][4] = array[0][4] + matrix.array[0][4]; result.array[0][5] = array[0][5] + matrix.array[0][5]; + result.array[1][0] = array[1][0] + matrix.array[1][0]; result.array[1][1] = array[1][1] + matrix.array[1][1]; result.array[1][2] = array[1][2] + matrix.array[1][2]; + result.array[1][3] = array[1][3] + matrix.array[1][3]; result.array[1][4] = array[1][4] + matrix.array[1][4]; result.array[1][5] = array[1][5] + matrix.array[1][5]; + result.array[2][0] = array[2][0] + matrix.array[2][0]; result.array[2][1] = array[2][1] + matrix.array[2][1]; result.array[2][2] = array[2][2] + matrix.array[2][2]; + result.array[2][3] = array[2][3] + matrix.array[2][3]; result.array[2][4] = array[2][4] + matrix.array[2][4]; result.array[2][5] = array[2][5] + matrix.array[2][5]; + result.array[3][0] = array[3][0] + matrix.array[3][0]; result.array[3][1] = array[3][1] + matrix.array[3][1]; result.array[3][2] = array[3][2] + matrix.array[3][2]; + result.array[3][3] = array[3][3] + matrix.array[3][3]; result.array[3][4] = array[3][4] + matrix.array[3][4]; result.array[3][5] = array[3][5] + matrix.array[3][5]; + result.array[4][0] = array[4][0] + matrix.array[4][0]; result.array[4][1] = array[4][1] + matrix.array[4][1]; result.array[4][2] = array[4][2] + matrix.array[4][2]; + result.array[4][3] = array[4][3] + matrix.array[4][3]; result.array[4][4] = array[4][4] + matrix.array[4][4]; result.array[4][5] = array[4][5] + matrix.array[4][5]; + result.array[5][0] = array[5][0] + matrix.array[5][0]; result.array[5][1] = array[5][1] + matrix.array[5][1]; result.array[5][2] = array[5][2] + matrix.array[5][2]; + result.array[5][3] = array[5][3] + matrix.array[5][3]; result.array[5][4] = array[5][4] + matrix.array[5][4]; result.array[5][5] = array[5][5] + matrix.array[5][5]; + return result; +} + +// Overloaded operator for multiplication with a number +inline Matrix6x6 Matrix6x6::operator*(double value) const { + Matrix6x6 result; + result.array[0][0] = value * array[0][0]; result.array[0][1] = value * array[0][1]; result.array[0][2] = value * array[0][2]; + result.array[0][3] = value * array[0][3]; result.array[0][4] = value * array[0][4]; result.array[0][5] = value * array[0][5]; + result.array[1][0] = value * array[1][0]; result.array[1][1] = value * array[1][1]; result.array[1][2] = value * array[1][2]; + result.array[1][3] = value * array[1][3]; result.array[1][4] = value * array[1][4]; result.array[1][5] = value * array[1][5]; + result.array[2][0] = value * array[2][0]; result.array[2][1] = value * array[2][1]; result.array[2][2] = value * array[2][2]; + result.array[2][3] = value * array[2][3]; result.array[2][4] = value * array[2][4]; result.array[2][5] = value * array[2][5]; + result.array[3][0] = value * array[3][0]; result.array[3][1] = value * array[3][1]; result.array[3][2] = value * array[3][2]; + result.array[3][3] = value * array[3][3]; result.array[3][4] = value * array[3][4]; result.array[3][5] = value * array[3][5]; + result.array[4][0] = value * array[4][0]; result.array[4][1] = value * array[4][1]; result.array[4][2] = value * array[4][2]; + result.array[4][3] = value * array[4][3]; result.array[4][4] = value * array[4][4]; result.array[4][5] = value * array[4][5]; + result.array[5][0] = value * array[5][0]; result.array[5][1] = value * array[5][1]; result.array[5][2] = value * array[5][2]; + result.array[5][3] = value * array[5][3]; result.array[5][4] = value * array[5][4]; result.array[5][5] = value * array[5][5]; + return result; +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/mathematics/Vector6D.cpp b/src/mathematics/Vector6D.cpp new file mode 100644 index 00000000..811f665a --- /dev/null +++ b/src/mathematics/Vector6D.cpp @@ -0,0 +1,54 @@ +/******************************************************************************** +* 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 "Vector6D.h" +#include +#include +#include + +// Namespaces +using namespace reactphysics3d; + +// Constructor +Vector6D::Vector6D() { + +} + +// Constructor +Vector6D::Vector6D(double value) { + array[0] = value; array[1] = value; array[2] = value; + array[3] = value; array[4] = value; array[5] = value; +} + +// Constructor +Vector6D::Vector6D(double v1, double v2, double v3, double v4, double v5, double v6) { + array[0] = v1; array[1] = v2; array[2] = v3; + array[3] = v4; array[4] = v5; array[5] = v6; +} + +// Destructor +Vector6D::~Vector6D() { + +} \ No newline at end of file diff --git a/src/mathematics/Vector6D.h b/src/mathematics/Vector6D.h new file mode 100644 index 00000000..a641aab6 --- /dev/null +++ b/src/mathematics/Vector6D.h @@ -0,0 +1,112 @@ +/******************************************************************************** +* 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 VECTOR6D_H +#define VECTOR6D_H + +// Libraries +#include +#include +#include "mathematics_functions.h" + + +// ReactPhysics3D namespace +namespace reactphysics3d { + +/* ------------------------------------------------------------------- + Class Vector6D : + This class represents 6 dimensionnal vector in space. + ------------------------------------------------------------------- +*/ +class Vector6D { + private : + double array[6]; + + public : + Vector6D(); // Constructor + Vector6D(double value); // Constructor + Vector6D(double v1, double v2, double v3, + double v4, double v5, double v6); // Constructor + virtual ~Vector6D(); // Destructor + double getValue(int index) const; // Get a component of the vector + void setValue(int index, double value); // Set a component of the vector + void setAllValues(double v1, double v2, double v3, + double v4, double v5, double v6); // Set all the values of the vector + void initWithValue(double value); // Init all the values with the same value + + // --- Overloaded operators --- // + Vector6D operator+(const Vector6D& vector) const; // Overloaded operator for addition + Vector6D operator-(const Vector6D& vector) const ; // Overloaded operator for substraction + Vector6D operator*(double number) const; // Overloaded operator for multiplication with a number +}; + + +// Get a component of the vector +inline double Vector6D::getValue(int index) const { + assert(index >= 0 && index < 6); + return array[index]; +} + +// Set a component of the vector +inline void Vector6D::setValue(int index, double value) { + assert(index >= 0 && index < 6); + array[index] = value; +} + +// Set all the values of the vector (inline) +inline void Vector6D::setAllValues(double v1, double v2, double v3, double v4, double v5, double v6) { + array[0] = v1; array[1] = v2; array[2] = v3; array[3] = v4; array[4] = v5; array[5] = v6; +} + +// Init the all the values with the same value +inline void Vector6D::initWithValue(double value) { + array[0] = value; array[1] = value; array[2] = value; array[3] = value; array[4] = value; array[5] = value; +} + +// Overloaded operator for multiplication between a number and a Vector3D (inline) +inline Vector6D operator*(double number, const Vector6D& vector) { + return vector * number; +} + +// Overloaded operator for addition +inline Vector6D Vector6D::operator+(const Vector6D& vector) const { + return Vector6D(array[0] + vector.array[0], array[1] + vector.array[1], array[2] + vector.array[2], + array[3] + vector.array[3], array[4] + vector.array[4], array[5] + vector.array[5]); +} + +// Overloaded operator for substraction +inline Vector6D Vector6D::operator-(const Vector6D& vector) const { + return Vector6D(array[0] - vector.array[0], array[1] - vector.array[1], array[2] - vector.array[2], + array[3] - vector.array[3], array[4] - vector.array[4], array[5] - vector.array[5]); +} + +// Overloaded operator for multiplication with a number +inline Vector6D Vector6D::operator*(double value) const { + return Vector6D(array[0] * value, array[1] * value, array[2] * value, + array[3] * value, array[4] * value, array[5] * value); +} + +} // End of the ReactPhysics3D namespace + +#endif diff --git a/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp b/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp index d16bef68..bba31a17 100644 --- a/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp +++ b/src/mathematics/lcp/LCPProjectedGaussSeidel.cpp @@ -36,7 +36,7 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() { // Solve a LCP problem using the Projected-Gauss-Seidel algorithm // This method outputs the result in the lambda vector -void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, +void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const { @@ -47,29 +47,29 @@ void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstra double deltaLambda; double lambdaTemp; uint i, iter; - Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1 - for (i=0; i bodyNumberMapping, - Vector* const a, uint nbBodies) const { + Vector6D** B_sp, std::map bodyNumberMapping, + Vector6D* const a, uint nbBodies) const { uint i; uint indexBody1, indexBody2; @@ -94,8 +94,8 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr for(i=0; i bodyNumberMapping, - Vector* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method + Vector6D** B_sp, std::map bodyNumberMapping, + Vector6D* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method public: LCPProjectedGaussSeidel(uint maxIterations); // Constructor virtual ~LCPProjectedGaussSeidel(); // Destructor - virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, + virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda }; diff --git a/src/mathematics/lcp/LCPSolver.h b/src/mathematics/lcp/LCPSolver.h index 6d375da9..90daaeaf 100644 --- a/src/mathematics/lcp/LCPSolver.h +++ b/src/mathematics/lcp/LCPSolver.h @@ -60,7 +60,7 @@ class LCPSolver { public: LCPSolver(uint maxIterations); // Constructor virtual ~LCPSolver(); // Destructor - virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints, + virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, uint nbBodies, Body*** const bodyMapping, std::map bodyNumberMapping, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector