Optimizations in the constraint solver
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@405 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
9fd3d8b598
commit
ddf602c125
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(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++;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<nbConstraints; i++) {
|
||||
bodyMapping[i] = new Body*[2];
|
||||
J_sp[i] = new Matrix[2];
|
||||
J_sp[i] = new Matrix1x6[2];
|
||||
}
|
||||
|
||||
errorValues.changeSize(nbConstraints);
|
||||
|
|
@ -233,8 +233,8 @@ void ConstraintSolver::fillInMatrices() {
|
|||
Body* body;
|
||||
Vector v(6);
|
||||
Vector f(6);
|
||||
Matrix identity = Matrix::identity(3);
|
||||
Matrix mInv(6,6);
|
||||
//Matrix identity = Matrix::identity(3);
|
||||
//Matrix mInv(6,6);
|
||||
uint b=0;
|
||||
for (set<Body*>::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<nbConstraints; c++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
B_sp[0][c].changeSize(6,1);
|
||||
B_sp[1][c].changeSize(6,1);
|
||||
//B_sp[0][c].changeSize(6,1);
|
||||
//B_sp[1][c].changeSize(6,1);
|
||||
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
|
||||
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
|
||||
}
|
||||
|
|
@ -319,8 +332,8 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
|
|||
for (uint i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)).getVector() * dt;
|
||||
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)).getVector() * dt;
|
||||
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (Matrix(B_sp[0][i]) * lambda.getValue(i)).getVector() * dt;
|
||||
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (Matrix(B_sp[1][i]) * lambda.getValue(i)).getVector() * dt; // TODO : Remove conversion here
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -73,10 +73,10 @@ class ConstraintSolver {
|
|||
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
|
||||
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
|
||||
// J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
|
||||
Matrix** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
|
||||
Matrix1x6** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
|
||||
// The dimension of this array is nbConstraints times 2. Each cell will contain
|
||||
// a 1x6 matrix
|
||||
Matrix** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
||||
Vector6D** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
||||
// The dimension of this array is 2 times nbConstraints. Each cell will contain
|
||||
// a 6x1 matrix
|
||||
Vector b; // Vector "b" of the LCP problem
|
||||
|
|
@ -85,7 +85,7 @@ class ConstraintSolver {
|
|||
Vector errorValues; // Error vector of all constraints
|
||||
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
|
||||
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
|
||||
Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
|
||||
Matrix6x6* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
|
||||
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
||||
Vector* V1; // Array that contains for each body the Vector that contains linear and angular velocities
|
||||
// Each cell contains a 6x1 vector with linear and angular velocities
|
||||
|
|
|
|||
|
|
@ -112,6 +112,59 @@ Matrix::Matrix(const Vector& vector) {
|
|||
}
|
||||
}
|
||||
|
||||
// TODO : Delete this
|
||||
Matrix::Matrix(const Matrix6x6& matrix) : nbRow(6), nbColumn(6) {
|
||||
// Create the two dimensional dynamic array
|
||||
array = new double*[nbRow];
|
||||
|
||||
assert(array != 0); // Array pointer musn't be null
|
||||
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
array[i] = new double[nbColumn];
|
||||
}
|
||||
|
||||
// Copy the matrix
|
||||
for (int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
setValue(i,j, matrix.getValue(i,j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO : Delete this
|
||||
Matrix::Matrix(const Matrix1x6& matrix) : nbRow(1), nbColumn(6) {
|
||||
// Create the two dimensional dynamic array
|
||||
array = new double*[nbRow];
|
||||
|
||||
assert(array != 0); // Array pointer musn't be null
|
||||
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
array[i] = new double[nbColumn];
|
||||
}
|
||||
|
||||
// Copy the matrix
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
setValue(0,j, matrix.getValue(j));
|
||||
}
|
||||
}
|
||||
|
||||
// TODO : Delete this
|
||||
Matrix::Matrix(const Vector6D& vector) :nbRow(6), nbColumn(1) {
|
||||
// Create the two dimensional dynamic array
|
||||
array = new double*[nbRow];
|
||||
|
||||
assert(array != 0); // Array pointer musn't be null
|
||||
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
array[i] = new double[nbColumn];
|
||||
}
|
||||
|
||||
// Copy the matrix
|
||||
for(int j=0; j<nbRow; ++j) {
|
||||
setValue(j,0, vector.getValue(j));
|
||||
}
|
||||
}
|
||||
|
||||
// Destructor of the class Matrix
|
||||
Matrix::~Matrix() {
|
||||
// Destruction of the dynamic array
|
||||
|
|
|
|||
|
|
@ -27,8 +27,11 @@
|
|||
|
||||
// Libraries
|
||||
#include "exceptions.h"
|
||||
#include "Matrix1x6.h"
|
||||
#include "Matrix6x6.h"
|
||||
#include "Matrix3x3.h"
|
||||
#include "Vector.h"
|
||||
#include "Vector6D.h"
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
52
src/mathematics/Matrix1x6.cpp
Normal file
52
src/mathematics/Matrix1x6.cpp
Normal file
|
|
@ -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() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
138
src/mathematics/Matrix1x6.h
Normal file
138
src/mathematics/Matrix1x6.h
Normal file
|
|
@ -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 <cassert>
|
||||
|
||||
#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
|
||||
46
src/mathematics/Matrix6x6.cpp
Normal file
46
src/mathematics/Matrix6x6.cpp
Normal file
|
|
@ -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() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
197
src/mathematics/Matrix6x6.h
Normal file
197
src/mathematics/Matrix6x6.h
Normal file
|
|
@ -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 <cassert>
|
||||
#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
|
||||
54
src/mathematics/Vector6D.cpp
Normal file
54
src/mathematics/Vector6D.cpp
Normal file
|
|
@ -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 <iostream>
|
||||
#include <cassert>
|
||||
#include <vector>
|
||||
|
||||
// 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() {
|
||||
|
||||
}
|
||||
112
src/mathematics/Vector6D.h
Normal file
112
src/mathematics/Vector6D.h
Normal file
|
|
@ -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 <cmath>
|
||||
#include <cassert>
|
||||
#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
|
||||
|
|
@ -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<Body*, uint> 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<nbBodies; i++) {
|
||||
a[i].changeSize(6);
|
||||
}
|
||||
Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
||||
//for (i=0; i<nbBodies; i++) {
|
||||
// a[i].changeSize(6);
|
||||
//}
|
||||
|
||||
// Compute the vector a
|
||||
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
|
||||
|
||||
// For each constraint
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]).getValue(0,0);
|
||||
d[i] = (Matrix(J_sp[i][0]) * B_sp[0][i] + Matrix(J_sp[i][1]) * B_sp[1][i]).getValue(0,0); // TODO : Remove conversion here
|
||||
}
|
||||
|
||||
for(iter=0; iter<maxIterations; iter++) {
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]).getValue(0,0) - (J_sp[i][1] * a[indexBody2]).getValue(0,0)) / d[i];
|
||||
deltaLambda = (b.getValue(i) - (Matrix(J_sp[i][0]) * a[indexBody1]).getValue(0,0) - (Matrix(J_sp[i][1]) * a[indexBody2]).getValue(0,0)) / d[i]; // TODO : Remove conversion here
|
||||
lambdaTemp = lambda.getValue(i);
|
||||
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
|
||||
deltaLambda = lambda.getValue(i) - lambdaTemp;
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda);
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -81,8 +81,8 @@ void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstra
|
|||
// Compute the vector a used in the solve() method
|
||||
// Note that a = B * lambda
|
||||
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
|
||||
Vector* const a, uint nbBodies) const {
|
||||
Vector6D** B_sp, std::map<Body*, uint> 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<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * lambda.getValue(i));
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * lambda.getValue(i));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,13 +37,13 @@ class LCPProjectedGaussSeidel : public LCPSolver {
|
|||
protected:
|
||||
|
||||
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
|
||||
Vector* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method
|
||||
Vector6D** B_sp, std::map<Body*, uint> 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<Body*, uint> 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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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<Body*, uint> 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user