Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

merged most of the changes from the branch into trunk, except for COLLADA, libxml and glut glitches.

Browse Source 
Still need to verify to make sure no unwanted renaming is introduced.
This commit is contained in:
ejcoumans
2006-09-27 20:43:51 +00:00
parent d1e9a885f3
commit eb23bb5c0c
263 changed files with 7528 additions and 6714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
View File

@@ -18,36 +18,36 @@ subject to the following restrictions:
#include "btSolve2LinearConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/SimdVector3.h"
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
void Solve2LinearConstraint::resolveUnilateralPairConstraint(
RigidBody* body1,
RigidBody* body2,
void btSolve2LinearConstraint::resolveUnilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
imp0 = 0.f;
imp1 = 0.f;
SimdScalar len = fabs(normalA.length())-1.f;
btScalar len = fabs(normalA.length())-1.f;
if (fabs(len) >= SIMD_EPSILON)
return;
@@ -55,24 +55,24 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
// SimdScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
SimdScalar massTerm = 1.f / (invMassA + invMassB);
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
btScalar massTerm = 1.f / (invMassA + invMassB);
// calculate rhs (or error) terms
const SimdScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const SimdScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
@@ -86,8 +86,8 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
//
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
@@ -105,31 +105,31 @@ void Solve2LinearConstraint::resolveUnilateralPairConstraint(
void Solve2LinearConstraint::resolveBilateralPairConstraint(
RigidBody* body1,
RigidBody* body2,
const SimdMatrix3x3& world2A,
const SimdMatrix3x3& world2B,
void btSolve2LinearConstraint::resolveBilateralPairConstraint(
btRigidBody* body1,
btRigidBody* body2,
const btMatrix3x3& world2A,
const btMatrix3x3& world2B,
const SimdVector3& invInertiaADiag,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdVector3& invInertiaBDiag,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
const btVector3& invInertiaADiag,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btVector3& invInertiaBDiag,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
imp0 = 0.f;
imp1 = 0.f;
SimdScalar len = fabs(normalA.length())-1.f;
btScalar len = fabs(normalA.length())-1.f;
if (fabs(len) >= SIMD_EPSILON)
return;
@@ -137,20 +137,20 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
JacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA,
invInertiaBDiag,invMassB);
//const SimdScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const SimdScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
//const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
const SimdScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const SimdScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
const btScalar vel0 = normalA.dot(body1->getVelocityInLocalPoint(rel_posA1)-body2->getVelocityInLocalPoint(rel_posA1));
const btScalar vel1 = normalB.dot(body1->getVelocityInLocalPoint(rel_posB1)-body2->getVelocityInLocalPoint(rel_posB1));
// calculate rhs (or error) terms
const SimdScalar dv0 = depthA * m_tau - vel0 * m_damping;
const SimdScalar dv1 = depthB * m_tau - vel1 * m_damping;
const btScalar dv0 = depthA * m_tau - vel0 * m_damping;
const btScalar dv1 = depthB * m_tau - vel1 * m_damping;
// dC/dv * dv = -C
@@ -163,8 +163,8 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
//
SimdScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
SimdScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB);
btScalar invDet = 1.0f / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
@@ -222,19 +222,19 @@ void Solve2LinearConstraint::resolveBilateralPairConstraint(
void Solve2LinearConstraint::resolveAngularConstraint( const SimdMatrix3x3& invInertiaAWS,
const SimdScalar invMassA,
const SimdVector3& linvelA,const SimdVector3& angvelA,
const SimdVector3& rel_posA1,
const SimdMatrix3x3& invInertiaBWS,
const SimdScalar invMassB,
const SimdVector3& linvelB,const SimdVector3& angvelB,
const SimdVector3& rel_posA2,
void btSolve2LinearConstraint::resolveAngularConstraint( const btMatrix3x3& invInertiaAWS,
const btScalar invMassA,
const btVector3& linvelA,const btVector3& angvelA,
const btVector3& rel_posA1,
const btMatrix3x3& invInertiaBWS,
const btScalar invMassB,
const btVector3& linvelB,const btVector3& angvelB,
const btVector3& rel_posA2,
SimdScalar depthA, const SimdVector3& normalA,
const SimdVector3& rel_posB1,const SimdVector3& rel_posB2,
SimdScalar depthB, const SimdVector3& normalB,
SimdScalar& imp0,SimdScalar& imp1)
btScalar depthA, const btVector3& normalA,
const btVector3& rel_posB1,const btVector3& rel_posB2,
btScalar depthB, const btVector3& normalB,
btScalar& imp0,btScalar& imp1)
{
}