summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp')
-rw-r--r--thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp113
1 files changed, 52 insertions, 61 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index 2b59f0b7a6..37d3aede37 100644
--- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -21,29 +21,29 @@ subject to the following restrictions:
#include "LinearMath/btIDebugDraw.h"
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
- #define BTMBP2PCONSTRAINT_DIM 3
+#define BTMBP2PCONSTRAINT_DIM 3
#else
- #define BTMBP2PCONSTRAINT_DIM 6
+#define BTMBP2PCONSTRAINT_DIM 6
#endif
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB)
- :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false),
- m_rigidBodyA(0),
- m_rigidBodyB(bodyB),
- m_pivotInA(pivotInA),
- m_pivotInB(pivotInB)
+ : btMultiBodyConstraint(body, 0, link, -1, BTMBP2PCONSTRAINT_DIM, false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(bodyB),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB)
{
- m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+ m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
}
btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
- :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBP2PCONSTRAINT_DIM,false),
- m_rigidBodyA(0),
- m_rigidBodyB(0),
- m_pivotInA(pivotInA),
- m_pivotInB(pivotInB)
+ : btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBP2PCONSTRAINT_DIM, false),
+ m_rigidBodyA(0),
+ m_rigidBodyB(0),
+ m_pivotInA(pivotInA),
+ m_pivotInB(pivotInB)
{
- m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
+ m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
}
void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -56,7 +56,6 @@ btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
{
}
-
int btMultiBodyPoint2Point::getIslandIdA() const
{
if (m_rigidBodyA)
@@ -73,7 +72,7 @@ int btMultiBodyPoint2Point::getIslandIdA() const
else
{
if (m_bodyA->getLink(m_linkA).m_collider)
- return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
+ return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
}
}
return -1;
@@ -100,48 +99,43 @@ int btMultiBodyPoint2Point::getIslandIdB() const
return -1;
}
-
-
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
- btMultiBodyJacobianData& data,
- const btContactSolverInfo& infoGlobal)
+ btMultiBodyJacobianData& data,
+ const btContactSolverInfo& infoGlobal)
{
-
-// int i=1;
-int numDim = BTMBP2PCONSTRAINT_DIM;
- for (int i=0;i<numDim;i++)
+ // int i=1;
+ int numDim = BTMBP2PCONSTRAINT_DIM;
+ for (int i = 0; i < numDim; i++)
{
-
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
- //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
- constraintRow.m_orgConstraint = this;
- constraintRow.m_orgDofIndex = i;
- constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
- constraintRow.m_contactNormal1.setValue(0,0,0);
- constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
- constraintRow.m_contactNormal2.setValue(0,0,0);
- constraintRow.m_angularComponentA.setValue(0,0,0);
- constraintRow.m_angularComponentB.setValue(0,0,0);
+ //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+ constraintRow.m_orgConstraint = this;
+ constraintRow.m_orgDofIndex = i;
+ constraintRow.m_relpos1CrossNormal.setValue(0, 0, 0);
+ constraintRow.m_contactNormal1.setValue(0, 0, 0);
+ constraintRow.m_relpos2CrossNormal.setValue(0, 0, 0);
+ constraintRow.m_contactNormal2.setValue(0, 0, 0);
+ constraintRow.m_angularComponentA.setValue(0, 0, 0);
+ constraintRow.m_angularComponentB.setValue(0, 0, 0);
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
- btVector3 contactNormalOnB(0,0,0);
+ btVector3 contactNormalOnB(0, 0, 0);
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
contactNormalOnB[i] = -1;
#else
- contactNormalOnB[i%3] = -1;
+ contactNormalOnB[i % 3] = -1;
#endif
-
- // Convert local points back to world
+ // Convert local points back to world
btVector3 pivotAworld = m_pivotInA;
if (m_rigidBodyA)
{
-
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
- pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
- } else
+ pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+ }
+ else
{
if (m_bodyA)
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
@@ -150,44 +144,41 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
if (m_rigidBodyB)
{
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
- pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
- } else
+ pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+ }
+ else
{
if (m_bodyB)
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
-
}
- btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0;
+ btScalar posError = i < 3 ? (pivotAworld - pivotBworld).dot(contactNormalOnB) : 0;
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
-
- fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0,0,0),
- contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
- posError,
- infoGlobal,
- -m_maxAppliedImpulse, m_maxAppliedImpulse
- );
- //@todo: support the case of btMultiBody versus btRigidBody,
- //see btPoint2PointConstraint::getInfo2NonVirtual
+ fillMultiBodyConstraint(constraintRow, data, 0, 0, btVector3(0, 0, 0),
+ contactNormalOnB, pivotAworld, pivotBworld, //sucks but let it be this way "for the time being"
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse);
+ //@todo: support the case of btMultiBody versus btRigidBody,
+ //see btPoint2PointConstraint::getInfo2NonVirtual
#else
const btVector3 dummy(0, 0, 0);
btAssert(m_bodyA->isMultiDof());
btScalar* jac1 = jacobianA(i);
- const btVector3 &normalAng = i >= 3 ? contactNormalOnB : dummy;
- const btVector3 &normalLin = i < 3 ? contactNormalOnB : dummy;
+ const btVector3& normalAng = i >= 3 ? contactNormalOnB : dummy;
+ const btVector3& normalLin = i < 3 ? contactNormalOnB : dummy;
m_bodyA->filConstraintJacobianMultiDof(m_linkA, pivotAworld, normalAng, normalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
fillMultiBodyConstraint(constraintRow, data, jac1, 0,
- dummy, dummy, dummy, //sucks but let it be this way "for the time being"
- posError,
- infoGlobal,
- -m_maxAppliedImpulse, m_maxAppliedImpulse
- );
+ dummy, dummy, dummy, //sucks but let it be this way "for the time being"
+ posError,
+ infoGlobal,
+ -m_maxAppliedImpulse, m_maxAppliedImpulse);
#endif
}
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-16 12:34:25 +0000