diff options
author | Guillermo S. Romero <gsr.b3d@infernal-iceberg.com> | 2010-06-17 07:29:16 +0400 |
---|---|---|
committer | Guillermo S. Romero <gsr.b3d@infernal-iceberg.com> | 2010-06-17 07:29:16 +0400 |
commit | 3398d901a17fbb093f593830b3ee40f32ada993c (patch) | |
tree | e5c9946e6c3acaa197a32804049c2ba71d4a5b13 /extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp | |
parent | ed59822857de7e7b41b33b79c306f5e9b8755c62 (diff) |
Revert changes back to original state, before 29506.
Diffstat (limited to 'extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp')
-rw-r--r-- | extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp | 255 |
1 files changed, 0 insertions, 255 deletions
diff --git a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp deleted file mode 100644 index 0c7dbd668bb..00000000000 --- a/extern/bullet2/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp +++ /dev/null @@ -1,255 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "btSolve2LinearConstraint.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" - - -void btSolve2LinearConstraint::resolveUnilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - 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, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - (void)linvelA; - (void)linvelB; - (void)angvelB; - (void)angvelA; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - 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)); - -// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv - btScalar massTerm = btScalar(1.) / (invMassA + invMassB); - - - // calculate rhs (or error) terms - const btScalar dv0 = depthA * m_tau * massTerm - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau * massTerm - vel1 * m_damping; - - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - -} - - - -void btSolve2LinearConstraint::resolveBilateralPairConstraint( - btRigidBody* body1, - btRigidBody* body2, - const btMatrix3x3& world2A, - const btMatrix3x3& world2B, - - 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, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - - (void)linvelA; - (void)linvelB; - (void)angvelA; - (void)angvelB; - - - - imp0 = btScalar(0.); - imp1 = btScalar(0.); - - btScalar len = btFabs(normalA.length()) - btScalar(1.); - if (btFabs(len) >= SIMD_EPSILON) - return; - - btAssert(len < SIMD_EPSILON); - - - //this jacobian entry could be re-used for all iterations - btJacobianEntry jacA(world2A,world2B,rel_posA1,rel_posA2,normalA,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - btJacobianEntry jacB(world2A,world2B,rel_posB1,rel_posB2,normalB,invInertiaADiag,invMassA, - invInertiaBDiag,invMassB); - - //const btScalar vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - //const btScalar vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); - - 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 btScalar dv0 = depthA * m_tau - vel0 * m_damping; - const btScalar dv1 = depthB * m_tau - vel1 * m_damping; - - // dC/dv * dv = -C - - // jacobian * impulse = -error - // - - //impulse = jacobianInverse * -error - - // inverting 2x2 symmetric system (offdiagonal are equal!) - // - - - btScalar nonDiag = jacA.getNonDiagonal(jacB,invMassA,invMassB); - btScalar invDet = btScalar(1.0) / (jacA.getDiagonal() * jacB.getDiagonal() - nonDiag * nonDiag ); - - //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; - imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; - - //[a b] [d -c] - //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) - - //[jA nD] * [imp0] = [dv0] - //[nD jB] [imp1] [dv1] - - if ( imp0 > btScalar(0.0)) - { - if ( imp1 > btScalar(0.0) ) - { - //both positive - } - else - { - imp1 = btScalar(0.); - - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } - } - else - { - imp0 = btScalar(0.); - - imp1 = dv1 / jacB.getDiagonal(); - if ( imp1 <= btScalar(0.0) ) - { - imp1 = btScalar(0.); - // now imp0>0 imp1<0 - imp0 = dv0 / jacA.getDiagonal(); - if ( imp0 > btScalar(0.0) ) - { - } else - { - imp0 = btScalar(0.); - } - } else - { - } - } -} - - -/* -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, - - btScalar depthA, const btVector3& normalA, - const btVector3& rel_posB1,const btVector3& rel_posB2, - btScalar depthB, const btVector3& normalB, - btScalar& imp0,btScalar& imp1) -{ - -} -*/ - |