Remove files that are not used anymore
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@450 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
5467e09020
commit
ab6865b3a1
|
|
@ -1,549 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 <cassert>
|
||||
#include "Matrix.h"
|
||||
|
||||
// Namespaces
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor without argument
|
||||
Matrix::Matrix()
|
||||
:nbRow(0), nbColumn(0) {
|
||||
array = 0;
|
||||
}
|
||||
|
||||
// Constructor of the class Matrix
|
||||
Matrix::Matrix(int nbRow, int nbColumn) throw(std::invalid_argument)
|
||||
:nbRow(nbRow),nbColumn(nbColumn) {
|
||||
// Check the arguments
|
||||
if (nbRow>0 && nbColumn>0) {
|
||||
|
||||
// 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];
|
||||
assert(array[i] != 0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Throw an exception
|
||||
throw std::invalid_argument("Exception : The size of the matrix has to be positive !");
|
||||
}
|
||||
}
|
||||
|
||||
// Copy-constructor of the class Matrix
|
||||
Matrix::Matrix(const Matrix& matrix)
|
||||
:nbRow(matrix.nbRow), nbColumn(matrix.nbColumn) {
|
||||
|
||||
// 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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Conversion from Matrix3x3
|
||||
Matrix::Matrix(const Matrix3x3& matrix)
|
||||
:nbRow(3), nbColumn(3) {
|
||||
|
||||
// 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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Conversion from Vector to Matrix
|
||||
Matrix::Matrix(const Vector& vector) {
|
||||
// Create the two dimensional dynamic array
|
||||
array = new double*[vector.getNbComponent()];
|
||||
assert(array != 0);
|
||||
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
array[i] = new double[1];
|
||||
array[i][0] = vector.getValue(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Destructor of the class Matrix
|
||||
Matrix::~Matrix() {
|
||||
// Destruction of the dynamic array
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
delete array[i];
|
||||
}
|
||||
delete this->array;
|
||||
}
|
||||
|
||||
void Matrix::changeSize(uint newNbRows, uint newNbColumns) {
|
||||
if (array) {
|
||||
// Destruction of the dynamic array
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
delete array[i];
|
||||
}
|
||||
delete this->array;
|
||||
}
|
||||
|
||||
// Create the two dimensional dynamic array
|
||||
array = new double*[newNbRows];
|
||||
|
||||
assert(array != 0); // Array pointer musn't be null
|
||||
|
||||
for(int i=0; i<newNbRows; ++i) {
|
||||
array[i] = new double[newNbColumns];
|
||||
}
|
||||
|
||||
nbColumn = newNbColumns;
|
||||
nbRow = newNbRows;
|
||||
}
|
||||
|
||||
|
||||
// Function that return the cofactor matrix by removing row i and column j
|
||||
Matrix Matrix::getCofactor(int i, int j) const throw(std::invalid_argument) {
|
||||
// If i and j are in the matrix
|
||||
if (0<= i && i < nbRow && 0<= j && j<nbColumn) {
|
||||
// Create the cofactor matrix
|
||||
Matrix cofactor(nbRow-1,nbColumn-1);
|
||||
|
||||
int u=0; // Row coordinate in the cofactor matrix
|
||||
int v=0; // Column coordinate in the cofactor matrix
|
||||
|
||||
// For every element in the matrix
|
||||
for (int s=0; s<nbColumn; ++s) {
|
||||
for(int r=0; r<nbRow; ++r) {
|
||||
// If the element is not in row i or in column j
|
||||
if (r!=i && s!=j) {
|
||||
// Add the element in the cofactor matrix
|
||||
cofactor.setValue(u,v, getValue(r,s));
|
||||
++u;
|
||||
if (u==cofactor.nbRow) {
|
||||
u = 0;
|
||||
++v;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Return the cofactor matrix
|
||||
return cofactor;
|
||||
}
|
||||
else {
|
||||
// We Throw an out_of_range exception
|
||||
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
|
||||
}
|
||||
}
|
||||
|
||||
// This function return the transposed matrix
|
||||
Matrix Matrix::getTranspose() const {
|
||||
// Create the new matrix
|
||||
Matrix transposedMatrix(nbColumn, nbRow);
|
||||
|
||||
// Transposition of the matrix
|
||||
for (int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
transposedMatrix.setValue(j,i, array[i][j]);
|
||||
}
|
||||
}
|
||||
|
||||
// Return the transposed matrix
|
||||
return transposedMatrix;
|
||||
}
|
||||
|
||||
|
||||
// Function that return the inverse of the matrix if there exists
|
||||
Matrix Matrix::getInverse() const throw(MathematicsException) {
|
||||
// Check if the matrix is a square-matrix
|
||||
if (nbRow==nbColumn) {
|
||||
// Compute the determinant of the matrix
|
||||
double determinant = getDeterminant();
|
||||
|
||||
// Check if the matrix is invertible
|
||||
if (determinant != 0.0) {
|
||||
// Create a temp matrix
|
||||
Matrix tempMatrix(nbRow, nbColumn);
|
||||
|
||||
double k=1.0;
|
||||
|
||||
// Compute the inverse matrix
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
if ( (i+j) % 2 == 0) {
|
||||
k=1.0;
|
||||
}
|
||||
else {
|
||||
k=-1.0;
|
||||
}
|
||||
|
||||
tempMatrix.setValue(i,j, k * getCofactor(i,j).getDeterminant());
|
||||
}
|
||||
}
|
||||
|
||||
// Create the inverse matrix
|
||||
Matrix inverseMatrix = tempMatrix.getTranspose() * (1.0 / determinant);
|
||||
|
||||
// Return the inverse matrix
|
||||
return inverseMatrix;
|
||||
}
|
||||
else {
|
||||
// We throw a MathematicsException
|
||||
throw MathematicsException("MathematicsException : Inverse of the matrix can't be computed because the determinant is zero !");
|
||||
}
|
||||
}
|
||||
else {
|
||||
// We throw an Matrix Exception
|
||||
throw MathematicsException("MathematicsException : Inverse can't be computed for a non-square matrix !");
|
||||
}
|
||||
}
|
||||
|
||||
// Function that return the determinant of the matrix
|
||||
double Matrix::getDeterminant() const throw(MathematicsException) {
|
||||
// If the matrix is a square matrix
|
||||
if (nbRow == nbColumn) {
|
||||
if(nbRow == 1) {
|
||||
return getValue(0,0);
|
||||
}
|
||||
else if (nbRow == 2) {
|
||||
return (getValue(0,0) * getValue(1,1) - getValue(1,0) * getValue(0,1));
|
||||
}
|
||||
else {
|
||||
double determinant = 0.0;
|
||||
double k=1.0;
|
||||
|
||||
// For every element in the first row
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
determinant = determinant + k * getValue(0,j) * getCofactor(0,j).getDeterminant();
|
||||
|
||||
if (k==1.0) {
|
||||
k=-1.0;
|
||||
}
|
||||
else {
|
||||
k=1.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Return the determinant value
|
||||
return determinant;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Throw a MathematicsException
|
||||
throw MathematicsException("MathematicsException : The determinant of a non-square matrix isn't computable !");
|
||||
}
|
||||
}
|
||||
|
||||
// Return the trace of the matrix
|
||||
double Matrix::getTrace() const throw(MathematicsException) {
|
||||
|
||||
// Check if the matrix is a square-matrix
|
||||
if (nbRow == nbColumn) {
|
||||
double sum = 0.0;
|
||||
|
||||
// Compute the trace of the matrix
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
sum = sum + array[i][i];
|
||||
}
|
||||
|
||||
// Return the trace
|
||||
return sum;
|
||||
}
|
||||
else {
|
||||
// We throw an exception because the matrix is non-square
|
||||
throw MathematicsException("MathematicsException : Impossible to compute the trace for a non-square matrix");
|
||||
}
|
||||
}
|
||||
|
||||
// Return a sub matrix of size of the current matrix
|
||||
Matrix Matrix::getSubMatrix(unsigned int i, unsigned int j,
|
||||
unsigned int sizeRows, unsigned int sizeColumns) const throw(std::invalid_argument) {
|
||||
// Check the arguments
|
||||
if (i<0 || j<0 || i+sizeRows > nbRow || j+sizeColumns > nbColumn) {
|
||||
// Throw an exception
|
||||
throw std::invalid_argument("Error : The arguments are out of matrix bounds");
|
||||
}
|
||||
|
||||
// Compute the sub-matrix
|
||||
Matrix resultMatrix(sizeRows, sizeColumns);
|
||||
for (unsigned int k=0; k<sizeRows; k++) {
|
||||
for (unsigned int l=0; l<sizeColumns; l++) {
|
||||
resultMatrix.array[k][l] = array[i+k][j+l];
|
||||
}
|
||||
}
|
||||
|
||||
// Return the sub-matrix
|
||||
return resultMatrix;
|
||||
}
|
||||
|
||||
// Static function that return a identity matrix of size nxn
|
||||
Matrix Matrix::identity(int dimension) throw(std::invalid_argument) {
|
||||
// Argument verification
|
||||
if (dimension > 0) {
|
||||
// Create a new matrix
|
||||
Matrix identityMatrix(dimension,dimension);
|
||||
|
||||
// Fill in the identity matrix
|
||||
for(int i=0; i<dimension; ++i) {
|
||||
for(int j=0; j<dimension; ++j) {
|
||||
if (i==j) {
|
||||
identityMatrix.setValue(i, j, 1.0);
|
||||
}
|
||||
else {
|
||||
identityMatrix.setValue(i, j, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Return the identity matrix
|
||||
return identityMatrix;
|
||||
}
|
||||
else {
|
||||
// Throw an exception
|
||||
throw std::invalid_argument("Exception : The argument of identity() has to be positive !");
|
||||
}
|
||||
}
|
||||
|
||||
// Fill in a sub-matrix of the current matrix with another matrix.
|
||||
// This method replaces the sub-matrix with the upper-left index (i,j) in the current matrix by another
|
||||
// "subMatrix" matrix.
|
||||
void Matrix::fillInSubMatrix(unsigned int rowIndex, unsigned int colIndex, const Matrix& subMatrix) {
|
||||
assert(nbRow-rowIndex >= subMatrix.nbColumn);
|
||||
assert(nbColumn-colIndex >= subMatrix.nbColumn);
|
||||
|
||||
// Fill in the sub-matrix
|
||||
for (unsigned int i=0; i<subMatrix.nbRow; ++i) {
|
||||
for (unsigned int j=0; j<subMatrix.nbColumn; ++j) {
|
||||
array[rowIndex + i][colIndex + j] = subMatrix.array[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize all the matrix with the given value
|
||||
void Matrix::initWithValue(double value) {
|
||||
for (unsigned int i=0; i<nbRow; ++i) {
|
||||
for(unsigned int j=0; j<nbColumn; ++j) {
|
||||
array[i][j] = value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Vector Matrix::getVector() const {
|
||||
assert(nbColumn == 1);
|
||||
Vector vec(nbRow);
|
||||
for (int i=0; i<nbRow; i++) {
|
||||
vec.setValue(i, array[i][0]);
|
||||
}
|
||||
return vec;
|
||||
}
|
||||
|
||||
// Definition of the operator + for the sum of two matrices with references
|
||||
Matrix Matrix::operator+(const Matrix& matrix2) const throw(MathematicsException) {
|
||||
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
|
||||
// Create a new matrix
|
||||
Matrix sumMatrix(nbRow,nbColumn);
|
||||
|
||||
// Sum the two matrices
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
sumMatrix.setValue(i, j, this->getValue(i,j) + matrix2.getValue(i,j));
|
||||
}
|
||||
}
|
||||
|
||||
// Return the sum matrix
|
||||
return sumMatrix;
|
||||
}
|
||||
else {
|
||||
// We throw an MathematicsException
|
||||
throw MathematicsException("MathematicsException : Addition of the matrices isn't possible beacause the size of the matrices aren't the same");
|
||||
}
|
||||
}
|
||||
|
||||
// Definition of the operator - for the substraction of two matrices with references
|
||||
Matrix Matrix::operator-(const Matrix& matrix2) const throw(MathematicsException) {
|
||||
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
|
||||
// Create a new matrix
|
||||
Matrix sumMatrix(nbRow, nbColumn);
|
||||
|
||||
// Substract the two matrices
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<this->nbColumn; ++j) {
|
||||
sumMatrix.setValue(i, j, this->getValue(i,j) - matrix2.getValue(i,j));
|
||||
}
|
||||
}
|
||||
|
||||
// Return the sum matrix
|
||||
return sumMatrix;
|
||||
}
|
||||
else {
|
||||
// We throw a MathematicsException
|
||||
throw MathematicsException("MathematicsException : Substraction of the matrices isn't possible beacause the size of the matrices aren't the same");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator * for the multiplication of the matrix with a number
|
||||
Matrix Matrix::operator*(double nb) const {
|
||||
// Creation of the result matrix
|
||||
Matrix result(nbRow, nbColumn);
|
||||
|
||||
// Multiplication of the matrix with the number
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
result.setValue(i,j, getValue(i,j) * nb);
|
||||
}
|
||||
}
|
||||
|
||||
// Return the result matrix
|
||||
return result;
|
||||
}
|
||||
|
||||
// Overloaded operator for multiplication with a matrix
|
||||
Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException) {
|
||||
// Check the sizes of the matrices
|
||||
if (nbColumn == matrix2.nbRow) {
|
||||
// Compute the result of the multiplication
|
||||
Matrix result(nbRow, matrix2.nbColumn);
|
||||
double sum;
|
||||
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<matrix2.nbColumn; ++j) {
|
||||
sum = 0.0;
|
||||
for(int k=0; k<nbColumn; ++k) {
|
||||
sum = sum + array[i][k] * matrix2.array[k][j];
|
||||
}
|
||||
result.array[i][j] = sum;
|
||||
}
|
||||
}
|
||||
|
||||
// Return the result matrix
|
||||
return result;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the multiplication is impossible
|
||||
throw MathematicsException("MathematicsException : The sizes of the matrices aren't compatible for the multiplication");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for multiplication with a vector
|
||||
Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
|
||||
|
||||
// Check the sizes of the matrices
|
||||
if (nbColumn == vector.getNbComponent()) {
|
||||
Matrix result(nbRow, 1);
|
||||
for (int i=0; i<nbRow; i++) {
|
||||
double sum = 0.0;
|
||||
for (int j=0; j<nbColumn; j++) {
|
||||
sum += array[i][j] * vector.getValue(j);
|
||||
}
|
||||
result.array[i][0] = sum;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the multiplication is impossible
|
||||
throw MathematicsException("MathematicsException : The sizes of the matrix and the vector aren't compatible for the multiplication");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator = for the assignment
|
||||
Matrix& Matrix::operator=(const Matrix& matrix2) throw(MathematicsException) {
|
||||
|
||||
// Check for self-assignement
|
||||
if(this == &matrix2) {
|
||||
return *this;
|
||||
}
|
||||
|
||||
// Check the size of the matrix
|
||||
if (nbRow==matrix2.nbRow && nbColumn==matrix2.nbColumn) {
|
||||
// Check for self-assignment
|
||||
if (this != &matrix2) {
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
this->setValue(i,j, matrix2.getValue(i,j));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Return a reference to the matrix
|
||||
return *this;
|
||||
}
|
||||
else {
|
||||
// Throw a MathematicsException
|
||||
throw MathematicsException("MathematicsException : Assignment impossible because the size of the matrices aren't the same !");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for equality condition
|
||||
bool Matrix::operator==(const Matrix& matrix2) const throw(MathematicsException) {
|
||||
// Check if the matrices dimensions are compatible
|
||||
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
|
||||
for (int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
if (array[i][j] != matrix2.array[i][j]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the matrices dimensions aren't the same
|
||||
throw MathematicsException("MathematicsException : Impossible to check if the matrices are equal because they don't have the same dimension");
|
||||
}
|
||||
}
|
||||
|
||||
// TO DELETE, THIS IS JUST FOR TESTING MATRICES
|
||||
void Matrix::display() const {
|
||||
for(int i=0; i<nbRow; ++i) {
|
||||
for(int j=0; j<nbColumn; ++j) {
|
||||
std::cout << array[i][j] << " ";
|
||||
}
|
||||
|
||||
std::cout << std::endl;
|
||||
}
|
||||
}
|
||||
|
|
@ -1,132 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 MATRIX_H
|
||||
#define MATRIX_H
|
||||
|
||||
// Libraries
|
||||
#include "exceptions.h"
|
||||
#include "Matrix1x6.h"
|
||||
#include "Matrix6x6.h"
|
||||
#include "Matrix3x3.h"
|
||||
#include "Vector.h"
|
||||
#include "Vector6D.h"
|
||||
#include <stdexcept>
|
||||
#include <iostream>
|
||||
|
||||
// TODO : Replace the "int" by "unsigned int"
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class Matrix :
|
||||
This class represents a matrix.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class Matrix {
|
||||
private :
|
||||
unsigned int nbRow; // Number of row in the matrix
|
||||
unsigned int nbColumn; // Number of colum in the matrix
|
||||
double** array; // Dynamic array that contains the values of the matrix
|
||||
|
||||
public :
|
||||
Matrix(); // Constructor without argument
|
||||
Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the 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
|
||||
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
|
||||
int getNbRow() const; // Return the number of row of the matrix
|
||||
int getNbColumn() const; // Return the number of column of the matrix
|
||||
Matrix getCofactor(int i, int j) const throw(std::invalid_argument); // Return the cofactor matrix by removing row i and column j
|
||||
Matrix getTranspose() const; // Return the transposed matrixs
|
||||
Matrix getInverse() const throw(MathematicsException); // Return the inverse of the matrix if there exists
|
||||
double getDeterminant() const throw(MathematicsException); // Return the determinant of the matrix
|
||||
double getTrace() const throw(MathematicsException); // Return the trace of a square matrix
|
||||
Matrix getSubMatrix(unsigned int i, unsigned int j,
|
||||
unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument); // Return a sub matrix of size of the current matrix
|
||||
Vector getVector() const;
|
||||
static Matrix identity(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension
|
||||
void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix); // Fill in a sub-matrix of the current matrix with another matrix
|
||||
void initWithValue(double value); // Initialize all the matrix with the given value
|
||||
void display() const; // TO DELETE
|
||||
void changeSize(uint nbRows, uint nbColumns);
|
||||
|
||||
// --- Overloaded operators --- //
|
||||
Matrix operator+(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for addition
|
||||
Matrix operator-(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for substraction
|
||||
Matrix operator*(double nb) const; // Overloaded operator for multiplication with a number
|
||||
Matrix operator*(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for multiplication with a matrix
|
||||
Matrix operator*(const Vector& vector) const throw(MathematicsException); // Overloaded operator for multiplication with a vector
|
||||
Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment
|
||||
bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition
|
||||
};
|
||||
|
||||
// Function to get a value in the matrix (inline)
|
||||
inline double Matrix::getValue(int i, int j) const throw(std::invalid_argument) {
|
||||
if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) {
|
||||
// get the value in the matrix
|
||||
return array[i][j];
|
||||
}
|
||||
else {
|
||||
// We Throw an out_of_range exception
|
||||
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
|
||||
}
|
||||
}
|
||||
|
||||
// Function to set a value in the matrix (inline)
|
||||
inline void Matrix::setValue(int i, int j, double value) throw(std::invalid_argument) {
|
||||
if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) {
|
||||
// Set the value in the matrix
|
||||
this->array[i][j] = value;
|
||||
}
|
||||
else {
|
||||
// We Throw an out_of_range exception
|
||||
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
|
||||
}
|
||||
}
|
||||
|
||||
// Function that return the number of row of the matrix (inline)
|
||||
inline int Matrix::getNbRow() const {
|
||||
return nbRow;
|
||||
}
|
||||
|
||||
// Function that return the number of colum of the matrix (inline)
|
||||
inline int Matrix::getNbColumn() const {
|
||||
return nbColumn;
|
||||
}
|
||||
|
||||
// Overloaded operator for multiplication between a number and a Matrix (inline)
|
||||
inline Matrix operator*(double number, const Matrix& matrix) {
|
||||
|
||||
// Return the result matrix
|
||||
return matrix * number;
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
@ -1,52 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,138 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1,197 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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
|
||||
|
|
@ -1,266 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 "Vector.h"
|
||||
|
||||
// Namespaces
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor without argument
|
||||
Vector::Vector()
|
||||
:nbComponent(0) {
|
||||
tab = 0;
|
||||
}
|
||||
|
||||
// Constructor of the class Vector
|
||||
Vector::Vector(int n) throw(std::invalid_argument) {
|
||||
// Check the argument
|
||||
if (n > 0) {
|
||||
// Create the array that contains the values of the vector
|
||||
nbComponent = n;
|
||||
tab = new double[nbComponent];
|
||||
|
||||
// TODO : Remove this initialization
|
||||
// Fill the array with zero's value
|
||||
for(int i=0; i<nbComponent; ++i) {
|
||||
tab[i] = 0.0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Throw an exception because of the wrong argument
|
||||
throw std::invalid_argument("Exception : The size of the vector has to be positive !");
|
||||
}
|
||||
}
|
||||
|
||||
// Copy-constructor of the class Vector
|
||||
Vector::Vector(const Vector& vector) {
|
||||
nbComponent = vector.nbComponent;
|
||||
tab = new double[nbComponent];
|
||||
|
||||
// Fill the array with the value of the vector
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
tab[i] = vector.tab[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Conversion from Vector3D to Vector
|
||||
Vector::Vector(const Vector3& vector3d) {
|
||||
nbComponent = 3;
|
||||
tab = new double[3];
|
||||
|
||||
tab[0] = vector3d.getX();
|
||||
tab[1] = vector3d.getY();
|
||||
tab[2] = vector3d.getZ();
|
||||
}
|
||||
|
||||
// Destructor of the class Vector
|
||||
Vector::~Vector() {
|
||||
// Erase the array with the values of the vector
|
||||
delete [] tab;
|
||||
}
|
||||
|
||||
// Return the corresponding unit vector
|
||||
Vector Vector::getUnit() const throw(MathematicsException) {
|
||||
double lengthVector = length();
|
||||
|
||||
// Check if the length of the vector is equal to zero
|
||||
if (lengthVector!= 0) {
|
||||
double lengthInv = 1.0 / lengthVector;
|
||||
Vector unitVector(nbComponent);
|
||||
|
||||
// Compute the unit vector
|
||||
for(int i=0; i<nbComponent; ++i) {
|
||||
unitVector.setValue(i, getValue(i) * lengthInv);
|
||||
}
|
||||
|
||||
// Return the unit vector
|
||||
return unitVector;
|
||||
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the length of the vector is zero
|
||||
throw MathematicsException("MathematicsException : Impossible to compute the unit vector because the length of the vector is zero");
|
||||
}
|
||||
}
|
||||
|
||||
void Vector::setVector(const Vector& vector) {
|
||||
assert(nbComponent == vector.nbComponent);
|
||||
|
||||
for (int i=0; i<nbComponent; i++) {
|
||||
tab[i] = vector.tab[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Method to compute the scalar product of two vectors
|
||||
double Vector::scalarProduct(const Vector& vector) const throw(MathematicsException) {
|
||||
// Check the sizes of the two vectors
|
||||
if (nbComponent == vector.nbComponent) {
|
||||
double result = 0.0;
|
||||
|
||||
// Compute the scalar product
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
result = result + vector.tab[i] * tab[i];
|
||||
}
|
||||
|
||||
// Return the result of the scalar product
|
||||
return result;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the two vectors haven't the same size
|
||||
throw MathematicsException("MathematicsException : Impossible to compute the scalar product because the vectors haven't the same size");
|
||||
}
|
||||
}
|
||||
|
||||
// Method to compute the cross product of two vectors
|
||||
Vector Vector::crossProduct(const Vector& vector) const throw(MathematicsException) {
|
||||
// Check if the vectors have 3 components
|
||||
if (nbComponent == 3 && vector.nbComponent == 3) {
|
||||
Vector result(3);
|
||||
|
||||
// Compute the cross product
|
||||
result.tab[0] = tab[1] * vector.tab[2] - tab[2] * vector.tab[1];
|
||||
result.tab[1] = tab[2] * vector.tab[0] - tab[0] * vector.tab[2];
|
||||
result.tab[2] = tab[0] * vector.tab[1] - tab[1] * vector.tab[0];
|
||||
|
||||
// Return the result of the cross product
|
||||
return result;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the vectors haven't three components
|
||||
throw MathematicsException("MathematicsException : Impossible to compute the cross product because the vectors haven't 3 components");
|
||||
}
|
||||
}
|
||||
|
||||
void Vector::changeSize(uint newSize) {
|
||||
if (tab) {
|
||||
delete[] tab;
|
||||
}
|
||||
|
||||
nbComponent = newSize;
|
||||
tab = new double[nbComponent];
|
||||
|
||||
// Fill the array with the value of the vector
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
tab[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for addition
|
||||
Vector Vector::operator+(const Vector& vector) const throw(MathematicsException) {
|
||||
// Check the size of the two vectors
|
||||
if (nbComponent == vector.nbComponent) {
|
||||
Vector sum(nbComponent);
|
||||
|
||||
// Compute the sum of the two vectors
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
sum.setValue(i, vector.tab[i] + tab[i]);
|
||||
}
|
||||
|
||||
// Return the sum vector
|
||||
return sum;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the sizes of the two vectors aren't the same
|
||||
throw MathematicsException("MathematicsException : Impossible two sum the two vectors because the sizes aren't the same !");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for substraction
|
||||
Vector Vector::operator-(const Vector& vector) const throw(MathematicsException) {
|
||||
// Check the size of the two vectors
|
||||
if (nbComponent == vector.nbComponent) {
|
||||
Vector substraction(nbComponent);
|
||||
|
||||
// Compute the subraction of the two vectors
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
substraction.setValue(i, tab[i] - vector.tab[i]);
|
||||
}
|
||||
|
||||
// Return the subraction vector
|
||||
return substraction;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the sizes of the two vectors aren't the same
|
||||
throw MathematicsException("MathematicsException : Impossible two substract the two vectors because the sizes aren't the same !");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for multiplication with a number
|
||||
Vector Vector::operator*(double number) const {
|
||||
Vector result(nbComponent);
|
||||
|
||||
// Compute the multiplication
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
result.setValue(i, number * tab[i]);
|
||||
}
|
||||
|
||||
// Return the result vector
|
||||
return result;
|
||||
}
|
||||
|
||||
// Overloaded operator for assigment to a Vector
|
||||
Vector& Vector::operator=(const Vector& vector) throw(MathematicsException) {
|
||||
|
||||
// Check for self-assignment
|
||||
if (this == &vector) {
|
||||
return *this;
|
||||
}
|
||||
|
||||
// Check the size of the vectors
|
||||
if (nbComponent == vector.nbComponent) {
|
||||
// Check for self-assignment
|
||||
if (this != &vector) {
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
tab[i] = vector.tab[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Return a reference to the vector
|
||||
return *this;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the sizes of the vectors aren't the same
|
||||
throw MathematicsException("MathematicsException : The assigment to a Vector is impossible because the size of the vectors aren't the same");
|
||||
}
|
||||
}
|
||||
|
||||
// Overloaded operator for the equality condition
|
||||
bool Vector::operator==(const Vector& vector) const throw(MathematicsException) {
|
||||
// Check if the sizes of the vectors are compatible
|
||||
if (nbComponent == vector.nbComponent) {
|
||||
for (int i=0; i<nbComponent; ++i) {
|
||||
if (tab[i] != vector.tab[i]) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because the sizes of the vectors aren't the same
|
||||
throw MathematicsException("MathematicsException : Impossible to check if the vectors are equal because they don't have the same size");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1,174 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 VECTOR_H
|
||||
#define VECTOR_H
|
||||
|
||||
// Libraries
|
||||
#include "Vector3.h"
|
||||
#include "../constants.h"
|
||||
#include "mathematics_functions.h"
|
||||
#include "exceptions.h"
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class Vector :
|
||||
This class represents a Vector.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class Vector {
|
||||
private :
|
||||
double* tab; // Array of the vector's components
|
||||
uint nbComponent; // number of components in the vector
|
||||
|
||||
public :
|
||||
Vector(); // Constructor without argument
|
||||
Vector(int n) throw(std::invalid_argument); // Constructor of the class Vector
|
||||
Vector(const Vector& vector); // Copy-constructor of the class Vector
|
||||
Vector(const Vector3& vector3d); // Conversion from Vector3D to Vector
|
||||
virtual ~Vector(); // Destructor of the class Vector
|
||||
double getValue(int n) const throw(std::invalid_argument); // Get a component of the vector
|
||||
void setValue(int n, double value) throw(std::invalid_argument); // Set the value of a component of the vector
|
||||
int getNbComponent() const; // Get the number of components in the vector
|
||||
void initWithValue(double value); // Init all the elements with a given value
|
||||
double length() const; // Get the length of the vector
|
||||
Vector getUnit() const throw(MathematicsException); // Return the corresponding unit vector
|
||||
double scalarProduct(const Vector& vector) const throw(MathematicsException); // Scalar product of two vectors
|
||||
Vector crossProduct(const Vector& vector) const throw(MathematicsException); // Cross product of two vectors (in 3D only)
|
||||
void fillInSubVector(uint index, const Vector& subVector); // Replace a part of the current vector with another sub-vector
|
||||
Vector getSubVector(uint index, uint nbElements) const throw(std::invalid_argument); // Return a sub-vector of the current vector
|
||||
void setVector(const Vector& vector);
|
||||
bool isUnit() const; // Return true if the vector is unit and false otherwise
|
||||
void changeSize(uint newSize);
|
||||
|
||||
// --- Overloaded operators --- //
|
||||
Vector operator+(const Vector& vector) const throw(MathematicsException); // Overloaded operator for addition
|
||||
Vector operator-(const Vector& vector) const throw(MathematicsException); // Overloaded operator for substraction
|
||||
Vector operator*(double number) const; // Overloaded operator for multiplication with a number
|
||||
Vector& operator=(const Vector& vector) throw(MathematicsException); // Overloaded operator for the assignement to a Vector
|
||||
bool operator==(const Vector& vector) const throw(MathematicsException); // Overloaded operator for the equality condition
|
||||
};
|
||||
|
||||
|
||||
// ------ Definition of inlines functions ------ //
|
||||
|
||||
// Method to get the value of a component of the vector (inline)
|
||||
inline double Vector::getValue(int n) const throw(std::invalid_argument) {
|
||||
// Check the argument
|
||||
if (n>=0 && n<nbComponent) {
|
||||
// Return the value of the component
|
||||
return tab[n];
|
||||
}
|
||||
else {
|
||||
// Throw an exception because of the wrong argument
|
||||
throw std::invalid_argument("The argument is outside the bounds of the Vector");
|
||||
}
|
||||
}
|
||||
|
||||
// Method to set the value of a component of the vector
|
||||
inline void Vector::setValue(int n, double value) throw(std::invalid_argument) {
|
||||
// Check the argument
|
||||
if (n >= 0 && n<nbComponent) {
|
||||
// Set the value
|
||||
tab[n] = value;
|
||||
}
|
||||
else {
|
||||
// Throw an exception because of the wrong argument
|
||||
throw std::invalid_argument("Exception : The argument is outside the bounds of the Vector");
|
||||
}
|
||||
}
|
||||
|
||||
// Method to get the number of components in the vector (inline)
|
||||
inline int Vector::getNbComponent() const {
|
||||
// Return the number of components in the vector
|
||||
return nbComponent;
|
||||
}
|
||||
|
||||
// Method to get the length of the vector
|
||||
inline double Vector::length() const {
|
||||
// Compute the length of the vector
|
||||
double sum = 0.0;
|
||||
for(int i=0; i<nbComponent; ++i) {
|
||||
sum = sum + tab[i] * tab[i];
|
||||
}
|
||||
|
||||
// Return the length of the vector
|
||||
return sqrt(sum);
|
||||
}
|
||||
|
||||
// Init all the elements with a given value
|
||||
inline void Vector::initWithValue(double value) {
|
||||
for (uint i=0; i<nbComponent; i++) {
|
||||
tab[i] = value;
|
||||
}
|
||||
}
|
||||
|
||||
// Replace a part of the current vector with another sub-vector.
|
||||
// The argument "rowIndex" is the row index where the subVector starts.
|
||||
inline void Vector::fillInSubVector(uint rowIndex, const Vector& subVector) {
|
||||
assert(nbComponent-rowIndex >= subVector.nbComponent);
|
||||
|
||||
// For each value of the sub-vector
|
||||
for (uint i=0; i<subVector.nbComponent; ++i) {
|
||||
tab[rowIndex + i] = subVector.getValue(i);
|
||||
}
|
||||
}
|
||||
|
||||
// Return a sub-vector of the current vector
|
||||
inline Vector Vector::getSubVector(uint index, uint nbElements) const throw(std::invalid_argument) {
|
||||
// Check if the arguments are valid
|
||||
if (index < 0 || index+nbElements > nbComponent) {
|
||||
throw std::invalid_argument("Error : arguments are out of bounds");
|
||||
}
|
||||
|
||||
// Compute the sub-vector
|
||||
Vector subVector(nbElements);
|
||||
for (uint i=0, j=index; i<nbElements; i++, j++) {
|
||||
subVector.tab[i] = tab[j];
|
||||
}
|
||||
|
||||
// Return the sub-vector
|
||||
return subVector;
|
||||
}
|
||||
|
||||
// Return true if the vector is unit and false otherwise
|
||||
inline bool Vector::isUnit() const {
|
||||
return approxEqual(1.0, length());
|
||||
}
|
||||
|
||||
// Overloaded operator for multiplication between a number and a Vector (inline)
|
||||
inline Vector operator*(double number, const Vector& vector) {
|
||||
// Compute and return the result
|
||||
return vector * number;
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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() {
|
||||
|
||||
}
|
||||
|
|
@ -1,112 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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
|
||||
|
|
@ -1,125 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 "LCPProjectedGaussSeidel.h"
|
||||
#include <cmath>
|
||||
|
||||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// Constructor
|
||||
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations)
|
||||
:LCPSolver(maxIterations) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
|
||||
|
||||
}
|
||||
|
||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
||||
// This method outputs the result in the lambda vector
|
||||
void LCPProjectedGaussSeidel::solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], map<Body*, uint> bodyNumberMapping,
|
||||
double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const {
|
||||
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
lambda[i] = lambdaInit[i];
|
||||
}
|
||||
|
||||
|
||||
double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
|
||||
uint indexBody1, indexBody2;
|
||||
double deltaLambda;
|
||||
double lambdaTemp;
|
||||
uint i, iter;
|
||||
Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
||||
|
||||
|
||||
// Compute the vector a
|
||||
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
|
||||
|
||||
|
||||
// For each constraint
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
uint indexConstraintArray = 6 * i;
|
||||
d[i] = 0.0;
|
||||
for (uint j=0; j<6; j++) {
|
||||
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for(iter=0; iter<maxIterations; iter++) {
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
uint indexConstraintArray = 6 * i;
|
||||
deltaLambda = b[i];
|
||||
for (uint j=0; j<6; j++) {
|
||||
deltaLambda -= (J_sp[i][j] * a[indexBody1].getValue(j) + J_sp[i][6 + j] * a[indexBody2].getValue(j));
|
||||
}
|
||||
deltaLambda /= d[i];
|
||||
lambdaTemp = lambda[i];
|
||||
lambda[i] = std::max(lowLimits[i], std::min(lambda[i] + deltaLambda, highLimits[i]));
|
||||
deltaLambda = lambda[i] - lambdaTemp;
|
||||
for (uint j=0; j<6; j++) {
|
||||
a[indexBody1].setValue(j, a[indexBody1].getValue(j) + (B_sp[0][indexConstraintArray + j] * deltaLambda));
|
||||
a[indexBody2].setValue(j, a[indexBody2].getValue(j) + (B_sp[1][indexConstraintArray + j] * deltaLambda));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Clean
|
||||
delete[] d;
|
||||
delete[] a;
|
||||
}
|
||||
|
||||
// Compute the vector a used in the solve() method
|
||||
// Note that a = B * lambda
|
||||
void LCPProjectedGaussSeidel::computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
|
||||
double B_sp[2][6*NB_MAX_CONSTRAINTS], map<Body*, uint> bodyNumberMapping,
|
||||
Vector6D* const a, uint nbBodies) const {
|
||||
uint i;
|
||||
uint indexBody1, indexBody2;
|
||||
|
||||
// Init the vector a with zero values
|
||||
for (i=0; i<nbBodies; i++) {
|
||||
a[i].initWithValue(0.0);
|
||||
}
|
||||
|
||||
|
||||
for(i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
uint indexConstraintArray = 6 * i;
|
||||
for (uint j=0; j<6; j++) {
|
||||
a[indexBody1].setValue(j, a[indexBody1].getValue(j) + (B_sp[0][indexConstraintArray + j] * lambda[i]));
|
||||
a[indexBody2].setValue(j, a[indexBody2].getValue(j) + (B_sp[1][indexConstraintArray + j] * lambda[i]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1,58 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 LCP_PROJECTED_GAUSS_SEIDEL_H
|
||||
#define LCP_PROJECTED_GAUSS_SEIDEL_H
|
||||
|
||||
// Libraries
|
||||
#include "LCPSolver.h"
|
||||
#include <map>
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class LCPProjectedGaussSeidel :
|
||||
This class implements the Projected-Gauss-Seidel (PGS)
|
||||
algorithm in order to solve a LCP problem. This class inherits
|
||||
from the LCPSolver class.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class LCPProjectedGaussSeidel : public LCPSolver {
|
||||
protected:
|
||||
|
||||
void computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
|
||||
double B_sp[2][6*NB_MAX_CONSTRAINTS], 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(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
|
||||
double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda
|
||||
};
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
@ -1,39 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 "LCPSolver.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
LCPSolver::LCPSolver(uint maxIterations)
|
||||
: maxIterations(maxIterations) {
|
||||
|
||||
}
|
||||
|
||||
// Destructor
|
||||
LCPSolver::~LCPSolver() {
|
||||
|
||||
}
|
||||
|
|
@ -1,90 +0,0 @@
|
|||
/********************************************************************************
|
||||
* 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 LCP_SOLVER_H
|
||||
#define LCP_SOLVER_H
|
||||
|
||||
// Libraries
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include "../Vector.h"
|
||||
#include "../Matrix.h"
|
||||
#include "../../body/Body.h"
|
||||
#include "../../constants.h"
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class LCPSolver :
|
||||
This abstract class represents an algorithm to solve a Linear
|
||||
Complementary Problem (LCP). Given a matrix "A=J*B", a vector "b",
|
||||
a vector "lowLimit" of lower limits and a vector "highLimits" of
|
||||
upper limits. The goal is to find a vector "lambda" such that:
|
||||
|
||||
w = Ax - b
|
||||
lowLimits <= lambda <= highLimits
|
||||
|
||||
and one of the thre following conditions holds :
|
||||
|
||||
lambda_i = lowLimits_i, w_i >= 0
|
||||
lambda_i = highLimits_i, w_i >= 0
|
||||
lowLimits_i < lambda_i < highLimits_i, w_i = 0
|
||||
|
||||
Note that the matrix A is givent by the two matrices J and B with A=J*B.
|
||||
But only their sparse representations "J_sp" and "B_sp" are passed in
|
||||
arguments to solve() to be more efficient.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class LCPSolver {
|
||||
protected:
|
||||
uint maxIterations; // Maximum number of iterations
|
||||
double lambdaInit[NB_MAX_CONSTRAINTS]; // Initial value for lambda at the beginning of the algorithm
|
||||
|
||||
public:
|
||||
LCPSolver(uint maxIterations); // Constructor
|
||||
virtual ~LCPSolver(); // Destructor
|
||||
virtual void solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
|
||||
double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const=0; // Solve a LCP problem
|
||||
void setLambdaInit(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints); // Set the initial lambda vector
|
||||
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
|
||||
};
|
||||
|
||||
// Set the initial lambda vector
|
||||
inline void LCPSolver::setLambdaInit(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints) {
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
this->lambdaInit[i] = lambdaInit[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Set the maximum number of iterations
|
||||
inline void LCPSolver::setMaxIterations(uint maxIterations) {
|
||||
assert(maxIterations > 0);
|
||||
this->maxIterations = maxIterations;
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
@ -26,10 +26,8 @@
|
|||
#define MATHEMATICS_H
|
||||
|
||||
// Libraries
|
||||
#include "Matrix.h"
|
||||
#include "Matrix3x3.h"
|
||||
#include "Quaternion.h"
|
||||
#include "Vector.h"
|
||||
#include "Vector3.h"
|
||||
#include "Transform.h"
|
||||
#include "../constants.h"
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user