diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 84f6f39fe..d2ebf29ef 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -4,8 +4,8 @@ Copyright (c) 2013 Erwin Coumans http://bulletphysics.org 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, +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. @@ -26,7 +26,7 @@ subject to the following restrictions: #endif btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB) - :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), + :btMultiBodyConstraint(body,0,link,-1,BTMBP2PCONSTRAINT_DIM,false), m_rigidBodyA(0), m_rigidBodyB(bodyB), m_pivotInA(pivotInA), @@ -95,15 +95,23 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co const btContactSolverInfo& infoGlobal) { -// int i=1; - for (int i=0;igetCompanionId(); pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; } else @@ -134,20 +142,22 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co { if (m_bodyB) pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); - + } btScalar posError = i < 3 ? (pivotAworld-pivotBworld).dot(contactNormalOnB) : 0; -#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST - +#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST + fillMultiBodyConstraint(constraintRow, data, 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); @@ -167,4 +177,4 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co ); #endif } -} \ No newline at end of file +}