summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp239
1 files changed, 0 insertions, 239 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
deleted file mode 100644
index 1ea20edcb2..0000000000
--- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp
+++ /dev/null
@@ -1,239 +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)
-{
-
-}
-*/
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-21 07:02:55 +0000