db5ff8ec4a
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@392 92aac97c-a6ce-11dd-a772-7fcde58d38e6
107 lines
5.0 KiB
C++
107 lines
5.0 KiB
C++
/********************************************************************************
|
|
* 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;
|
|
|
|
// 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(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
|
|
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
|
|
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
|
|
|
|
lambda = lambdaInit;
|
|
|
|
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;
|
|
Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
|
for (i=0; i<nbBodies; i++) {
|
|
a[i].changeSize(6);
|
|
}
|
|
|
|
// Compute the vector a
|
|
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
|
|
|
|
// For each constraint
|
|
for (i=0; i<nbConstraints; i++) {
|
|
d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]).getValue(0,0);
|
|
}
|
|
|
|
for(iter=0; iter<maxIterations; iter++) {
|
|
for (i=0; i<nbConstraints; i++) {
|
|
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
|
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
|
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]).getValue(0,0) - (J_sp[i][1] * a[indexBody2]).getValue(0,0)) / d[i];
|
|
lambdaTemp = lambda.getValue(i);
|
|
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
|
|
deltaLambda = lambda.getValue(i) - lambdaTemp;
|
|
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
|
|
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
|
|
}
|
|
}
|
|
|
|
// Clean
|
|
delete[] d;
|
|
delete[] a;
|
|
}
|
|
|
|
// Compute the vector a used in the solve() method
|
|
// Note that a = B * lambda
|
|
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
|
Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
|
|
Vector* const a, uint nbBodies) const {
|
|
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]];
|
|
a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
|
|
a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
|
|
}
|
|
|
|
}
|