Convert DOS (CRLF) source files to Unix (LF) line endings
Excluded `examples/pybullet/gym/pybullet_data/` which has many (3000+) CRLF data files (obj, mtl, urdf), and `docs/pybullet_quickstart_guide` which has generated .js and .htm files with CRLF line endings too.
This commit is contained in:
Rémi Verschelde
@@ -1,354 +1,354 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2014 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,
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
|
||||
#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
|
||||
#include "btGjkEpa3.h"
|
||||
#include "btGjkCollisionDescription.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
||||
|
||||
template <typename btConvexTemplate>
|
||||
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
|
||||
const btGjkCollisionDescription& colDesc,
|
||||
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
|
||||
{
|
||||
(void)v;
|
||||
|
||||
// const btScalar radialmargin(btScalar(0.));
|
||||
|
||||
btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
|
||||
|
||||
btGjkEpaSolver3::sResults results;
|
||||
|
||||
if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
|
||||
|
||||
{
|
||||
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
|
||||
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
|
||||
wWitnessOnA = results.witnesses[0];
|
||||
wWitnessOnB = results.witnesses[1];
|
||||
v = results.normal;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
|
||||
{
|
||||
wWitnessOnA = results.witnesses[0];
|
||||
wWitnessOnB = results.witnesses[1];
|
||||
v = results.normal;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <typename btConvexTemplate, typename btGjkDistanceTemplate>
|
||||
int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
|
||||
{
|
||||
bool m_catchDegeneracies = true;
|
||||
btScalar m_cachedSeparatingDistance = 0.f;
|
||||
|
||||
btScalar distance = btScalar(0.);
|
||||
btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
|
||||
|
||||
btVector3 pointOnA, pointOnB;
|
||||
btTransform localTransA = a.getWorldTransform();
|
||||
btTransform localTransB = b.getWorldTransform();
|
||||
|
||||
btScalar marginA = a.getMargin();
|
||||
btScalar marginB = b.getMargin();
|
||||
|
||||
int m_curIter = 0;
|
||||
int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
|
||||
btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
int m_degenerateSimplex = 0;
|
||||
|
||||
int m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
btScalar squaredDistance = BT_LARGE_FLOAT;
|
||||
btScalar delta = btScalar(0.);
|
||||
|
||||
btScalar margin = marginA + marginB;
|
||||
|
||||
simplexSolver.reset();
|
||||
|
||||
for (;;)
|
||||
//while (true)
|
||||
{
|
||||
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
|
||||
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
|
||||
|
||||
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
|
||||
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
|
||||
|
||||
btVector3 pWorld = localTransA(pInA);
|
||||
btVector3 qWorld = localTransB(qInB);
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
|
||||
{
|
||||
m_degenerateSimplex = 10;
|
||||
checkSimplex = true;
|
||||
//checkPenetration = false;
|
||||
break;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if (simplexSolver.inSimplex(w))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
// are we getting any closer ?
|
||||
btScalar f0 = squaredDistance - delta;
|
||||
btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
|
||||
|
||||
if (f0 <= f1)
|
||||
{
|
||||
if (f0 <= btScalar(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_degenerateSimplex = 11;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//add current vertex to simplex
|
||||
simplexSolver.addVertex(w, pWorld, qWorld);
|
||||
btVector3 newCachedSeparatingAxis;
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!simplexSolver.closest(newCachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 3;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
|
||||
{
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
m_degenerateSimplex = 6;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
btScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = newCachedSeparatingAxis.length2();
|
||||
#if 0
|
||||
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
|
||||
if (squaredDistance>previousSquaredDistance)
|
||||
{
|
||||
m_degenerateSimplex = 7;
|
||||
squaredDistance = previousSquaredDistance;
|
||||
checkSimplex = false;
|
||||
break;
|
||||
}
|
||||
#endif //
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
m_degenerateSimplex = 12;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined(_DEBUG)
|
||||
|
||||
printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
m_cachedSeparatingAxis.getZ(),
|
||||
squaredDistance);
|
||||
#endif
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
bool check = (!simplexSolver.fullSimplex());
|
||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
||||
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
m_degenerateSimplex = 13;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkSimplex)
|
||||
{
|
||||
simplexSolver.compute_points(pointOnA, pointOnB);
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
|
||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
m_degenerateSimplex = 5;
|
||||
}
|
||||
if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
|
||||
normalInB *= rlen; //normalize
|
||||
|
||||
btScalar s = btSqrt(squaredDistance);
|
||||
|
||||
btAssert(s > btScalar(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((btScalar(1.) / rlen) - margin);
|
||||
isValid = true;
|
||||
|
||||
m_lastUsedMethod = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool catchDegeneratePenetrationCase =
|
||||
(m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01));
|
||||
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
//if there is no way to handle penetrations, bail out
|
||||
|
||||
// Penetration depth case.
|
||||
btVector3 tmpPointOnA, tmpPointOnB;
|
||||
|
||||
m_cachedSeparatingAxis.setZero();
|
||||
|
||||
bool isValid2 = btGjkEpaCalcPenDepth(a, b,
|
||||
colDesc,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
|
||||
|
||||
if (isValid2)
|
||||
{
|
||||
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
|
||||
btScalar lenSqr = tmpNormalInB.length2();
|
||||
if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB = m_cachedSeparatingAxis;
|
||||
lenSqr = m_cachedSeparatingAxis.length2();
|
||||
}
|
||||
|
||||
if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB /= btSqrt(lenSqr);
|
||||
btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 8;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 9;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
{
|
||||
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
|
||||
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
|
||||
///reports a valid positive distance. Use the results of the second GJK instead of failing.
|
||||
///thanks to Jacob.Langford for the reproduction case
|
||||
///http://code.google.com/p/bullet/issues/detail?id=250
|
||||
|
||||
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
|
||||
{
|
||||
btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
|
||||
//only replace valid distances when the distance is less
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
pointOnA -= m_cachedSeparatingAxis * marginA;
|
||||
pointOnB += m_cachedSeparatingAxis * marginB;
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
normalInB.normalize();
|
||||
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 6;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
|
||||
{
|
||||
m_cachedSeparatingAxis = normalInB;
|
||||
m_cachedSeparatingDistance = distance;
|
||||
distInfo->m_distance = distance;
|
||||
distInfo->m_normalBtoA = normalInB;
|
||||
distInfo->m_pointOnB = pointOnB;
|
||||
distInfo->m_pointOnA = pointOnB + normalInB * distance;
|
||||
return 0;
|
||||
}
|
||||
return -m_lastUsedMethod;
|
||||
}
|
||||
|
||||
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2014 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,
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
#define BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
|
||||
#include "LinearMath/btTransform.h" // Note that btVector3 might be double precision...
|
||||
#include "btGjkEpa3.h"
|
||||
#include "btGjkCollisionDescription.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
|
||||
|
||||
template <typename btConvexTemplate>
|
||||
bool btGjkEpaCalcPenDepth(const btConvexTemplate& a, const btConvexTemplate& b,
|
||||
const btGjkCollisionDescription& colDesc,
|
||||
btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB)
|
||||
{
|
||||
(void)v;
|
||||
|
||||
// const btScalar radialmargin(btScalar(0.));
|
||||
|
||||
btVector3 guessVector(b.getWorldTransform().getOrigin() - a.getWorldTransform().getOrigin()); //?? why not use the GJK input?
|
||||
|
||||
btGjkEpaSolver3::sResults results;
|
||||
|
||||
if (btGjkEpaSolver3_Penetration(a, b, guessVector, results))
|
||||
|
||||
{
|
||||
// debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
|
||||
//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
|
||||
wWitnessOnA = results.witnesses[0];
|
||||
wWitnessOnB = results.witnesses[1];
|
||||
v = results.normal;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (btGjkEpaSolver3_Distance(a, b, guessVector, results))
|
||||
{
|
||||
wWitnessOnA = results.witnesses[0];
|
||||
wWitnessOnB = results.witnesses[1];
|
||||
v = results.normal;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template <typename btConvexTemplate, typename btGjkDistanceTemplate>
|
||||
int btComputeGjkEpaPenetration(const btConvexTemplate& a, const btConvexTemplate& b, const btGjkCollisionDescription& colDesc, btVoronoiSimplexSolver& simplexSolver, btGjkDistanceTemplate* distInfo)
|
||||
{
|
||||
bool m_catchDegeneracies = true;
|
||||
btScalar m_cachedSeparatingDistance = 0.f;
|
||||
|
||||
btScalar distance = btScalar(0.);
|
||||
btVector3 normalInB(btScalar(0.), btScalar(0.), btScalar(0.));
|
||||
|
||||
btVector3 pointOnA, pointOnB;
|
||||
btTransform localTransA = a.getWorldTransform();
|
||||
btTransform localTransB = b.getWorldTransform();
|
||||
|
||||
btScalar marginA = a.getMargin();
|
||||
btScalar marginB = b.getMargin();
|
||||
|
||||
int m_curIter = 0;
|
||||
int gGjkMaxIter = colDesc.m_maxGjkIterations; //this is to catch invalid input, perhaps check for #NaN?
|
||||
btVector3 m_cachedSeparatingAxis = colDesc.m_firstDir;
|
||||
|
||||
bool isValid = false;
|
||||
bool checkSimplex = false;
|
||||
bool checkPenetration = true;
|
||||
int m_degenerateSimplex = 0;
|
||||
|
||||
int m_lastUsedMethod = -1;
|
||||
|
||||
{
|
||||
btScalar squaredDistance = BT_LARGE_FLOAT;
|
||||
btScalar delta = btScalar(0.);
|
||||
|
||||
btScalar margin = marginA + marginB;
|
||||
|
||||
simplexSolver.reset();
|
||||
|
||||
for (;;)
|
||||
//while (true)
|
||||
{
|
||||
btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis) * localTransA.getBasis();
|
||||
btVector3 seperatingAxisInB = m_cachedSeparatingAxis * localTransB.getBasis();
|
||||
|
||||
btVector3 pInA = a.getLocalSupportWithoutMargin(seperatingAxisInA);
|
||||
btVector3 qInB = b.getLocalSupportWithoutMargin(seperatingAxisInB);
|
||||
|
||||
btVector3 pWorld = localTransA(pInA);
|
||||
btVector3 qWorld = localTransB(qInB);
|
||||
|
||||
btVector3 w = pWorld - qWorld;
|
||||
delta = m_cachedSeparatingAxis.dot(w);
|
||||
|
||||
// potential exit, they don't overlap
|
||||
if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * colDesc.m_maximumDistanceSquared))
|
||||
{
|
||||
m_degenerateSimplex = 10;
|
||||
checkSimplex = true;
|
||||
//checkPenetration = false;
|
||||
break;
|
||||
}
|
||||
|
||||
//exit 0: the new point is already in the simplex, or we didn't come any closer
|
||||
if (simplexSolver.inSimplex(w))
|
||||
{
|
||||
m_degenerateSimplex = 1;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
// are we getting any closer ?
|
||||
btScalar f0 = squaredDistance - delta;
|
||||
btScalar f1 = squaredDistance * colDesc.m_gjkRelError2;
|
||||
|
||||
if (f0 <= f1)
|
||||
{
|
||||
if (f0 <= btScalar(0.))
|
||||
{
|
||||
m_degenerateSimplex = 2;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_degenerateSimplex = 11;
|
||||
}
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
//add current vertex to simplex
|
||||
simplexSolver.addVertex(w, pWorld, qWorld);
|
||||
btVector3 newCachedSeparatingAxis;
|
||||
|
||||
//calculate the closest point to the origin (update vector v)
|
||||
if (!simplexSolver.closest(newCachedSeparatingAxis))
|
||||
{
|
||||
m_degenerateSimplex = 3;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (newCachedSeparatingAxis.length2() < colDesc.m_gjkRelError2)
|
||||
{
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
m_degenerateSimplex = 6;
|
||||
checkSimplex = true;
|
||||
break;
|
||||
}
|
||||
|
||||
btScalar previousSquaredDistance = squaredDistance;
|
||||
squaredDistance = newCachedSeparatingAxis.length2();
|
||||
#if 0
|
||||
///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo
|
||||
if (squaredDistance>previousSquaredDistance)
|
||||
{
|
||||
m_degenerateSimplex = 7;
|
||||
squaredDistance = previousSquaredDistance;
|
||||
checkSimplex = false;
|
||||
break;
|
||||
}
|
||||
#endif //
|
||||
|
||||
//redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
|
||||
|
||||
//are we getting any closer ?
|
||||
if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance)
|
||||
{
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
checkSimplex = true;
|
||||
m_degenerateSimplex = 12;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
m_cachedSeparatingAxis = newCachedSeparatingAxis;
|
||||
|
||||
//degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject
|
||||
if (m_curIter++ > gGjkMaxIter)
|
||||
{
|
||||
#if defined(DEBUG) || defined(_DEBUG)
|
||||
|
||||
printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter);
|
||||
printf("sepAxis=(%f,%f,%f), squaredDistance = %f\n",
|
||||
m_cachedSeparatingAxis.getX(),
|
||||
m_cachedSeparatingAxis.getY(),
|
||||
m_cachedSeparatingAxis.getZ(),
|
||||
squaredDistance);
|
||||
#endif
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
bool check = (!simplexSolver.fullSimplex());
|
||||
//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
|
||||
|
||||
if (!check)
|
||||
{
|
||||
//do we need this backup_closest here ?
|
||||
// m_simplexSolver->backup_closest(m_cachedSeparatingAxis);
|
||||
m_degenerateSimplex = 13;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (checkSimplex)
|
||||
{
|
||||
simplexSolver.compute_points(pointOnA, pointOnB);
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
|
||||
btScalar lenSqr = m_cachedSeparatingAxis.length2();
|
||||
|
||||
//valid normal
|
||||
if (lenSqr < 0.0001)
|
||||
{
|
||||
m_degenerateSimplex = 5;
|
||||
}
|
||||
if (lenSqr > SIMD_EPSILON * SIMD_EPSILON)
|
||||
{
|
||||
btScalar rlen = btScalar(1.) / btSqrt(lenSqr);
|
||||
normalInB *= rlen; //normalize
|
||||
|
||||
btScalar s = btSqrt(squaredDistance);
|
||||
|
||||
btAssert(s > btScalar(0.0));
|
||||
pointOnA -= m_cachedSeparatingAxis * (marginA / s);
|
||||
pointOnB += m_cachedSeparatingAxis * (marginB / s);
|
||||
distance = ((btScalar(1.) / rlen) - margin);
|
||||
isValid = true;
|
||||
|
||||
m_lastUsedMethod = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 2;
|
||||
}
|
||||
}
|
||||
|
||||
bool catchDegeneratePenetrationCase =
|
||||
(m_catchDegeneracies && m_degenerateSimplex && ((distance + margin) < 0.01));
|
||||
|
||||
//if (checkPenetration && !isValid)
|
||||
if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
|
||||
{
|
||||
//penetration case
|
||||
|
||||
//if there is no way to handle penetrations, bail out
|
||||
|
||||
// Penetration depth case.
|
||||
btVector3 tmpPointOnA, tmpPointOnB;
|
||||
|
||||
m_cachedSeparatingAxis.setZero();
|
||||
|
||||
bool isValid2 = btGjkEpaCalcPenDepth(a, b,
|
||||
colDesc,
|
||||
m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB);
|
||||
|
||||
if (isValid2)
|
||||
{
|
||||
btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
|
||||
btScalar lenSqr = tmpNormalInB.length2();
|
||||
if (lenSqr <= (SIMD_EPSILON * SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB = m_cachedSeparatingAxis;
|
||||
lenSqr = m_cachedSeparatingAxis.length2();
|
||||
}
|
||||
|
||||
if (lenSqr > (SIMD_EPSILON * SIMD_EPSILON))
|
||||
{
|
||||
tmpNormalInB /= btSqrt(lenSqr);
|
||||
btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length();
|
||||
//only replace valid penetrations when the result is deeper (check)
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
normalInB = tmpNormalInB;
|
||||
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 3;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 8;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 9;
|
||||
}
|
||||
}
|
||||
else
|
||||
|
||||
{
|
||||
///this is another degenerate case, where the initial GJK calculation reports a degenerate case
|
||||
///EPA reports no penetration, and the second GJK (using the supporting vector without margin)
|
||||
///reports a valid positive distance. Use the results of the second GJK instead of failing.
|
||||
///thanks to Jacob.Langford for the reproduction case
|
||||
///http://code.google.com/p/bullet/issues/detail?id=250
|
||||
|
||||
if (m_cachedSeparatingAxis.length2() > btScalar(0.))
|
||||
{
|
||||
btScalar distance2 = (tmpPointOnA - tmpPointOnB).length() - margin;
|
||||
//only replace valid distances when the distance is less
|
||||
if (!isValid || (distance2 < distance))
|
||||
{
|
||||
distance = distance2;
|
||||
pointOnA = tmpPointOnA;
|
||||
pointOnB = tmpPointOnB;
|
||||
pointOnA -= m_cachedSeparatingAxis * marginA;
|
||||
pointOnB += m_cachedSeparatingAxis * marginB;
|
||||
normalInB = m_cachedSeparatingAxis;
|
||||
normalInB.normalize();
|
||||
|
||||
isValid = true;
|
||||
m_lastUsedMethod = 6;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastUsedMethod = 5;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (isValid && ((distance < 0) || (distance * distance < colDesc.m_maximumDistanceSquared)))
|
||||
{
|
||||
m_cachedSeparatingAxis = normalInB;
|
||||
m_cachedSeparatingDistance = distance;
|
||||
distInfo->m_distance = distance;
|
||||
distInfo->m_normalBtoA = normalInB;
|
||||
distInfo->m_pointOnB = pointOnB;
|
||||
distInfo->m_pointOnA = pointOnB + normalInB * distance;
|
||||
return 0;
|
||||
}
|
||||
return -m_lastUsedMethod;
|
||||
}
|
||||
|
||||
#endif //BT_GJK_EPA_PENETATION_CONVEX_COLLISION_H
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,4 +1,4 @@
|
||||
/*
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,23 +1,23 @@
|
||||
project "BulletCollision"
|
||||
|
||||
kind "StaticLib"
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
includedirs {
|
||||
"..",
|
||||
}
|
||||
files {
|
||||
"*.cpp",
|
||||
"*.h",
|
||||
"BroadphaseCollision/*.cpp",
|
||||
"BroadphaseCollision/*.h",
|
||||
"CollisionDispatch/*.cpp",
|
||||
"CollisionDispatch/*.h",
|
||||
"CollisionShapes/*.cpp",
|
||||
"CollisionShapes/*.h",
|
||||
"Gimpact/*.cpp",
|
||||
"Gimpact/*.h",
|
||||
"NarrowPhaseCollision/*.cpp",
|
||||
"NarrowPhaseCollision/*.h",
|
||||
}
|
||||
project "BulletCollision"
|
||||
|
||||
kind "StaticLib"
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
includedirs {
|
||||
"..",
|
||||
}
|
||||
files {
|
||||
"*.cpp",
|
||||
"*.h",
|
||||
"BroadphaseCollision/*.cpp",
|
||||
"BroadphaseCollision/*.h",
|
||||
"CollisionDispatch/*.cpp",
|
||||
"CollisionDispatch/*.h",
|
||||
"CollisionShapes/*.cpp",
|
||||
"CollisionShapes/*.h",
|
||||
"Gimpact/*.cpp",
|
||||
"Gimpact/*.h",
|
||||
"NarrowPhaseCollision/*.cpp",
|
||||
"NarrowPhaseCollision/*.h",
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -1,215 +1,215 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyFixedConstraint.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#define BTMBFIXEDCONSTRAINT_DIM 6
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB)
|
||||
{
|
||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB)
|
||||
{
|
||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyFixedConstraint::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyFixedConstraint::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numDim = BTMBFIXEDCONSTRAINT_DIM;
|
||||
for (int i = 0; i < numDim; i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
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;
|
||||
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
btMatrix3x3 frameAworld = m_frameInA;
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA)
|
||||
{
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
|
||||
}
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
btMatrix3x3 frameBworld = m_frameInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB)
|
||||
{
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
|
||||
}
|
||||
}
|
||||
|
||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||
btVector3 angleDiff;
|
||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||
|
||||
btVector3 constraintNormalLin(0, 0, 0);
|
||||
btVector3 constraintNormalAng(0, 0, 0);
|
||||
btScalar posError = 0.0;
|
||||
if (i < 3)
|
||||
{
|
||||
constraintNormalLin[i] = 1;
|
||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||
}
|
||||
else
|
||||
{ //i>=3
|
||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||
posError = angleDiff[i % 3];
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyFixedConstraint.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#define BTMBFIXEDCONSTRAINT_DIM 6
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB)
|
||||
{
|
||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
btMultiBodyFixedConstraint::btMultiBodyFixedConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBFIXEDCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB)
|
||||
{
|
||||
m_data.resize(BTMBFIXEDCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodyFixedConstraint::~btMultiBodyFixedConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyFixedConstraint::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyFixedConstraint::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
int numDim = BTMBFIXEDCONSTRAINT_DIM;
|
||||
for (int i = 0; i < numDim; i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
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;
|
||||
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
btMatrix3x3 frameAworld = m_frameInA;
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
frameAworld = frameAworld.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA)
|
||||
{
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld);
|
||||
}
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
btMatrix3x3 frameBworld = m_frameInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
frameBworld = frameBworld.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB)
|
||||
{
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, frameBworld);
|
||||
}
|
||||
}
|
||||
|
||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||
btVector3 angleDiff;
|
||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||
|
||||
btVector3 constraintNormalLin(0, 0, 0);
|
||||
btVector3 constraintNormalAng(0, 0, 0);
|
||||
btScalar posError = 0.0;
|
||||
if (i < 3)
|
||||
{
|
||||
constraintNormalLin[i] = 1;
|
||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||
}
|
||||
else
|
||||
{ //i>=3
|
||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||
posError = angleDiff[i % 3];
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyFixedConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,183 +1,183 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyJointMotor.h"
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
m_kp(0),
|
||||
m_erp(1),
|
||||
m_rhsClamp(SIMD_INFINITY)
|
||||
{
|
||||
m_maxAppliedImpulse = maxMotorImpulse;
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
}
|
||||
|
||||
void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
{
|
||||
allocateJacobiansMultiDof();
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
int linkDoF = 0;
|
||||
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
|
||||
|
||||
// row 0: the lower bound
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
|
||||
m_numDofsFinalized = m_jacSizeBoth;
|
||||
}
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
m_kp(0),
|
||||
m_erp(1),
|
||||
m_rhsClamp(SIMD_INFINITY)
|
||||
{
|
||||
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
||||
|
||||
m_maxAppliedImpulse = maxMotorImpulse;
|
||||
}
|
||||
btMultiBodyJointMotor::~btMultiBodyJointMotor()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyJointMotor::getIslandIdA() const
|
||||
{
|
||||
if (this->m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
{
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyJointMotor::getIslandIdB() const
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
{
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// only positions need to be updated -- data.m_jacobians and force
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
{
|
||||
finalizeMultiDof();
|
||||
}
|
||||
|
||||
//don't crash
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
return;
|
||||
|
||||
if (m_maxAppliedImpulse == 0.f)
|
||||
return;
|
||||
|
||||
const btScalar posError = 0;
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
for (int row = 0; row < getNumRows(); row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
|
||||
int dof = 0;
|
||||
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
|
||||
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
|
||||
btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
|
||||
|
||||
btScalar velocityError = (m_desiredVelocity - currentVelocity);
|
||||
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
|
||||
if (rhs > m_rhsClamp)
|
||||
{
|
||||
rhs = m_rhsClamp;
|
||||
}
|
||||
if (rhs < -m_rhsClamp)
|
||||
{
|
||||
rhs = -m_rhsClamp;
|
||||
}
|
||||
|
||||
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
|
||||
constraintRow.m_orgConstraint = this;
|
||||
constraintRow.m_orgDofIndex = row;
|
||||
{
|
||||
//expect either prismatic or revolute joint type for now
|
||||
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
||||
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
||||
{
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
constraintRow.m_contactNormal1.setZero();
|
||||
constraintRow.m_contactNormal2.setZero();
|
||||
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
||||
constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
|
||||
constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
|
||||
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePrismatic:
|
||||
{
|
||||
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
||||
constraintRow.m_contactNormal1 = prismaticAxisInWorld;
|
||||
constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
|
||||
constraintRow.m_relpos1CrossNormal.setZero();
|
||||
constraintRow.m_relpos2CrossNormal.setZero();
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyJointMotor.h"
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
m_kp(0),
|
||||
m_erp(1),
|
||||
m_rhsClamp(SIMD_INFINITY)
|
||||
{
|
||||
m_maxAppliedImpulse = maxMotorImpulse;
|
||||
// the data.m_jacobians never change, so may as well
|
||||
// initialize them here
|
||||
}
|
||||
|
||||
void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
{
|
||||
allocateJacobiansMultiDof();
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
int linkDoF = 0;
|
||||
unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
|
||||
|
||||
// row 0: the lower bound
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
|
||||
m_numDofsFinalized = m_jacSizeBoth;
|
||||
}
|
||||
|
||||
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
|
||||
//:btMultiBodyConstraint(body,0,link,-1,1,true),
|
||||
: btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 1, true),
|
||||
m_desiredVelocity(desiredVelocity),
|
||||
m_desiredPosition(0),
|
||||
m_kd(1.),
|
||||
m_kp(0),
|
||||
m_erp(1),
|
||||
m_rhsClamp(SIMD_INFINITY)
|
||||
{
|
||||
btAssert(linkDoF < body->getLink(link).m_dofCount);
|
||||
|
||||
m_maxAppliedImpulse = maxMotorImpulse;
|
||||
}
|
||||
btMultiBodyJointMotor::~btMultiBodyJointMotor()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyJointMotor::getIslandIdA() const
|
||||
{
|
||||
if (this->m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
{
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyJointMotor::getIslandIdB() const
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
{
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// only positions need to be updated -- data.m_jacobians and force
|
||||
// directions were set in the ctor and never change.
|
||||
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
{
|
||||
finalizeMultiDof();
|
||||
}
|
||||
|
||||
//don't crash
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
return;
|
||||
|
||||
if (m_maxAppliedImpulse == 0.f)
|
||||
return;
|
||||
|
||||
const btScalar posError = 0;
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
for (int row = 0; row < getNumRows(); row++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
|
||||
int dof = 0;
|
||||
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
|
||||
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
|
||||
btScalar positionStabiliationTerm = m_erp * (m_desiredPosition - currentPosition) / infoGlobal.m_timeStep;
|
||||
|
||||
btScalar velocityError = (m_desiredVelocity - currentVelocity);
|
||||
btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity + m_kd * velocityError;
|
||||
if (rhs > m_rhsClamp)
|
||||
{
|
||||
rhs = m_rhsClamp;
|
||||
}
|
||||
if (rhs < -m_rhsClamp)
|
||||
{
|
||||
rhs = -m_rhsClamp;
|
||||
}
|
||||
|
||||
fillMultiBodyConstraint(constraintRow, data, jacobianA(row), jacobianB(row), dummy, dummy, dummy, dummy, posError, infoGlobal, -m_maxAppliedImpulse, m_maxAppliedImpulse, false, 1, false, rhs);
|
||||
constraintRow.m_orgConstraint = this;
|
||||
constraintRow.m_orgDofIndex = row;
|
||||
{
|
||||
//expect either prismatic or revolute joint type for now
|
||||
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute) || (m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
|
||||
switch (m_bodyA->getLink(m_linkA).m_jointType)
|
||||
{
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
constraintRow.m_contactNormal1.setZero();
|
||||
constraintRow.m_contactNormal2.setZero();
|
||||
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
|
||||
constraintRow.m_relpos1CrossNormal = revoluteAxisInWorld;
|
||||
constraintRow.m_relpos2CrossNormal = -revoluteAxisInWorld;
|
||||
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::ePrismatic:
|
||||
{
|
||||
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(), m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
|
||||
constraintRow.m_contactNormal1 = prismaticAxisInWorld;
|
||||
constraintRow.m_contactNormal2 = -prismaticAxisInWorld;
|
||||
constraintRow.m_relpos1CrossNormal.setZero();
|
||||
constraintRow.m_relpos2CrossNormal.setZero();
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,77 +1,77 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
|
||||
#define BT_MULTIBODY_JOINT_MOTOR_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
struct btSolverInfo;
|
||||
|
||||
class btMultiBodyJointMotor : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
btScalar m_desiredVelocity;
|
||||
btScalar m_desiredPosition;
|
||||
btScalar m_kd;
|
||||
btScalar m_kp;
|
||||
btScalar m_erp;
|
||||
btScalar m_rhsClamp; //maximum error
|
||||
|
||||
public:
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
virtual ~btMultiBodyJointMotor();
|
||||
virtual void finalizeMultiDof();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
|
||||
{
|
||||
m_desiredVelocity = velTarget;
|
||||
m_kd = kd;
|
||||
}
|
||||
|
||||
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
|
||||
{
|
||||
m_desiredPosition = posTarget;
|
||||
m_kp = kp;
|
||||
}
|
||||
|
||||
virtual void setErp(btScalar erp)
|
||||
{
|
||||
m_erp = erp;
|
||||
}
|
||||
virtual btScalar getErp() const
|
||||
{
|
||||
return m_erp;
|
||||
}
|
||||
virtual void setRhsClamp(btScalar rhsClamp)
|
||||
{
|
||||
m_rhsClamp = rhsClamp;
|
||||
}
|
||||
virtual void debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
//todo(erwincoumans)
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_MOTOR_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#ifndef BT_MULTIBODY_JOINT_MOTOR_H
|
||||
#define BT_MULTIBODY_JOINT_MOTOR_H
|
||||
|
||||
#include "btMultiBodyConstraint.h"
|
||||
struct btSolverInfo;
|
||||
|
||||
class btMultiBodyJointMotor : public btMultiBodyConstraint
|
||||
{
|
||||
protected:
|
||||
btScalar m_desiredVelocity;
|
||||
btScalar m_desiredPosition;
|
||||
btScalar m_kd;
|
||||
btScalar m_kp;
|
||||
btScalar m_erp;
|
||||
btScalar m_rhsClamp; //maximum error
|
||||
|
||||
public:
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
virtual ~btMultiBodyJointMotor();
|
||||
virtual void finalizeMultiDof();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal);
|
||||
|
||||
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 1.f)
|
||||
{
|
||||
m_desiredVelocity = velTarget;
|
||||
m_kd = kd;
|
||||
}
|
||||
|
||||
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 1.f)
|
||||
{
|
||||
m_desiredPosition = posTarget;
|
||||
m_kp = kp;
|
||||
}
|
||||
|
||||
virtual void setErp(btScalar erp)
|
||||
{
|
||||
m_erp = erp;
|
||||
}
|
||||
virtual btScalar getErp() const
|
||||
{
|
||||
return m_erp;
|
||||
}
|
||||
virtual void setRhsClamp(btScalar rhsClamp)
|
||||
{
|
||||
m_rhsClamp = rhsClamp;
|
||||
}
|
||||
virtual void debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
//todo(erwincoumans)
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_MOTOR_H
|
||||
|
||||
@@ -1,239 +1,239 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_LINK_H
|
||||
#define BT_MULTIBODY_LINK_H
|
||||
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
enum btMultiBodyLinkFlags
|
||||
{
|
||||
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
|
||||
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
|
||||
};
|
||||
|
||||
//both defines are now permanently enabled
|
||||
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
#define TEST_SPATIAL_ALGEBRA_LAYER
|
||||
|
||||
//
|
||||
// Various spatial helper functions
|
||||
//
|
||||
|
||||
//namespace {
|
||||
|
||||
#include "LinearMath/btSpatialAlgebra.h"
|
||||
|
||||
//}
|
||||
|
||||
//
|
||||
// Link struct
|
||||
//
|
||||
|
||||
struct btMultibodyLink
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btScalar m_mass; // mass of link
|
||||
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
|
||||
|
||||
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
|
||||
|
||||
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
|
||||
|
||||
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
|
||||
//this is set to zero for planar joint (see also m_eVector comment)
|
||||
|
||||
// m_eVector is constant, but depends on the joint type:
|
||||
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
|
||||
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
|
||||
// todo: fix the planar so it is consistent with the other joints
|
||||
|
||||
btVector3 m_eVector;
|
||||
|
||||
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
||||
|
||||
enum eFeatherstoneJointType
|
||||
{
|
||||
eRevolute = 0,
|
||||
ePrismatic = 1,
|
||||
eSpherical = 2,
|
||||
ePlanar = 3,
|
||||
eFixed = 4,
|
||||
eInvalid
|
||||
};
|
||||
|
||||
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
||||
// for prismatic: m_axesTop[0] = zero;
|
||||
// m_axesBottom[0] = unit vector along the joint axis.
|
||||
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
|
||||
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
|
||||
//
|
||||
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
|
||||
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
|
||||
//
|
||||
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
|
||||
// m_axesTop[1][2] = zero
|
||||
// m_axesBottom[0] = zero
|
||||
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
||||
btSpatialMotionVector m_axes[6];
|
||||
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
||||
void setAxisBottom(int dof, const btVector3 &axis)
|
||||
{
|
||||
m_axes[dof].m_bottomVec = axis;
|
||||
}
|
||||
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||
{
|
||||
m_axes[dof].m_topVec.setValue(x, y, z);
|
||||
}
|
||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||
{
|
||||
m_axes[dof].m_bottomVec.setValue(x, y, z);
|
||||
}
|
||||
const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
||||
const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
||||
|
||||
int m_dofOffset, m_cfgOffset;
|
||||
|
||||
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
|
||||
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
|
||||
|
||||
btVector3 m_appliedForce; // In WORLD frame
|
||||
btVector3 m_appliedTorque; // In WORLD frame
|
||||
|
||||
btVector3 m_appliedConstraintForce; // In WORLD frame
|
||||
btVector3 m_appliedConstraintTorque; // In WORLD frame
|
||||
|
||||
btScalar m_jointPos[7];
|
||||
|
||||
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
||||
//It gets set to zero after each internal stepSimulation call
|
||||
btScalar m_jointTorque[6];
|
||||
|
||||
class btMultiBodyLinkCollider *m_collider;
|
||||
int m_flags;
|
||||
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
eFeatherstoneJointType m_jointType;
|
||||
|
||||
struct btMultiBodyJointFeedback *m_jointFeedback;
|
||||
|
||||
btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
|
||||
|
||||
const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
|
||||
const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
|
||||
const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
|
||||
|
||||
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
|
||||
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
|
||||
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
: m_mass(1),
|
||||
m_parent(-1),
|
||||
m_zeroRotParentToThis(0, 0, 0, 1),
|
||||
m_cachedRotParentToThis(0, 0, 0, 1),
|
||||
m_collider(0),
|
||||
m_flags(0),
|
||||
m_dofCount(0),
|
||||
m_posVarCount(0),
|
||||
m_jointType(btMultibodyLink::eInvalid),
|
||||
m_jointFeedback(0),
|
||||
m_linkName(0),
|
||||
m_jointName(0),
|
||||
m_userPtr(0),
|
||||
m_jointDamping(0),
|
||||
m_jointFriction(0),
|
||||
m_jointLowerLimit(0),
|
||||
m_jointUpperLimit(0),
|
||||
m_jointMaxForce(0),
|
||||
m_jointMaxVelocity(0)
|
||||
{
|
||||
m_inertiaLocal.setValue(1, 1, 1);
|
||||
setAxisTop(0, 0., 0., 0.);
|
||||
setAxisBottom(0, 1., 0., 0.);
|
||||
m_dVector.setValue(0, 0, 0);
|
||||
m_eVector.setValue(0, 0, 0);
|
||||
m_cachedRVector.setValue(0, 0, 0);
|
||||
m_appliedForce.setValue(0, 0, 0);
|
||||
m_appliedTorque.setValue(0, 0, 0);
|
||||
m_appliedConstraintForce.setValue(0, 0, 0);
|
||||
m_appliedConstraintTorque.setValue(0, 0, 0);
|
||||
//
|
||||
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
||||
m_jointPos[3] = 1.f; //"quat.w"
|
||||
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
||||
m_cachedWorldTransform.setIdentity();
|
||||
}
|
||||
|
||||
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
||||
void updateCacheMultiDof(btScalar *pq = 0)
|
||||
{
|
||||
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||
|
||||
switch (m_jointType)
|
||||
{
|
||||
case eRevolute:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePrismatic:
|
||||
{
|
||||
// m_cachedRotParentToThis never changes, so no need to update
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
|
||||
|
||||
break;
|
||||
}
|
||||
case eSpherical:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePlanar:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case eFixed:
|
||||
{
|
||||
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//invalid type
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_LINK_H
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
#ifndef BT_MULTIBODY_LINK_H
|
||||
#define BT_MULTIBODY_LINK_H
|
||||
|
||||
#include "LinearMath/btQuaternion.h"
|
||||
#include "LinearMath/btVector3.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
|
||||
enum btMultiBodyLinkFlags
|
||||
{
|
||||
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
|
||||
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
|
||||
};
|
||||
|
||||
//both defines are now permanently enabled
|
||||
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
#define TEST_SPATIAL_ALGEBRA_LAYER
|
||||
|
||||
//
|
||||
// Various spatial helper functions
|
||||
//
|
||||
|
||||
//namespace {
|
||||
|
||||
#include "LinearMath/btSpatialAlgebra.h"
|
||||
|
||||
//}
|
||||
|
||||
//
|
||||
// Link struct
|
||||
//
|
||||
|
||||
struct btMultibodyLink
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btScalar m_mass; // mass of link
|
||||
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
|
||||
|
||||
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
|
||||
|
||||
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
|
||||
|
||||
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
|
||||
//this is set to zero for planar joint (see also m_eVector comment)
|
||||
|
||||
// m_eVector is constant, but depends on the joint type:
|
||||
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
|
||||
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
|
||||
// todo: fix the planar so it is consistent with the other joints
|
||||
|
||||
btVector3 m_eVector;
|
||||
|
||||
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
|
||||
|
||||
enum eFeatherstoneJointType
|
||||
{
|
||||
eRevolute = 0,
|
||||
ePrismatic = 1,
|
||||
eSpherical = 2,
|
||||
ePlanar = 3,
|
||||
eFixed = 4,
|
||||
eInvalid
|
||||
};
|
||||
|
||||
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
|
||||
// for prismatic: m_axesTop[0] = zero;
|
||||
// m_axesBottom[0] = unit vector along the joint axis.
|
||||
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
|
||||
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
|
||||
//
|
||||
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
|
||||
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
|
||||
//
|
||||
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
|
||||
// m_axesTop[1][2] = zero
|
||||
// m_axesBottom[0] = zero
|
||||
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
||||
btSpatialMotionVector m_axes[6];
|
||||
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
||||
void setAxisBottom(int dof, const btVector3 &axis)
|
||||
{
|
||||
m_axes[dof].m_bottomVec = axis;
|
||||
}
|
||||
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||
{
|
||||
m_axes[dof].m_topVec.setValue(x, y, z);
|
||||
}
|
||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
|
||||
{
|
||||
m_axes[dof].m_bottomVec.setValue(x, y, z);
|
||||
}
|
||||
const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
||||
const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
||||
|
||||
int m_dofOffset, m_cfgOffset;
|
||||
|
||||
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
|
||||
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
|
||||
|
||||
btVector3 m_appliedForce; // In WORLD frame
|
||||
btVector3 m_appliedTorque; // In WORLD frame
|
||||
|
||||
btVector3 m_appliedConstraintForce; // In WORLD frame
|
||||
btVector3 m_appliedConstraintTorque; // In WORLD frame
|
||||
|
||||
btScalar m_jointPos[7];
|
||||
|
||||
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
||||
//It gets set to zero after each internal stepSimulation call
|
||||
btScalar m_jointTorque[6];
|
||||
|
||||
class btMultiBodyLinkCollider *m_collider;
|
||||
int m_flags;
|
||||
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
eFeatherstoneJointType m_jointType;
|
||||
|
||||
struct btMultiBodyJointFeedback *m_jointFeedback;
|
||||
|
||||
btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
|
||||
|
||||
const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
|
||||
const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
|
||||
const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
|
||||
|
||||
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
|
||||
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
|
||||
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
: m_mass(1),
|
||||
m_parent(-1),
|
||||
m_zeroRotParentToThis(0, 0, 0, 1),
|
||||
m_cachedRotParentToThis(0, 0, 0, 1),
|
||||
m_collider(0),
|
||||
m_flags(0),
|
||||
m_dofCount(0),
|
||||
m_posVarCount(0),
|
||||
m_jointType(btMultibodyLink::eInvalid),
|
||||
m_jointFeedback(0),
|
||||
m_linkName(0),
|
||||
m_jointName(0),
|
||||
m_userPtr(0),
|
||||
m_jointDamping(0),
|
||||
m_jointFriction(0),
|
||||
m_jointLowerLimit(0),
|
||||
m_jointUpperLimit(0),
|
||||
m_jointMaxForce(0),
|
||||
m_jointMaxVelocity(0)
|
||||
{
|
||||
m_inertiaLocal.setValue(1, 1, 1);
|
||||
setAxisTop(0, 0., 0., 0.);
|
||||
setAxisBottom(0, 1., 0., 0.);
|
||||
m_dVector.setValue(0, 0, 0);
|
||||
m_eVector.setValue(0, 0, 0);
|
||||
m_cachedRVector.setValue(0, 0, 0);
|
||||
m_appliedForce.setValue(0, 0, 0);
|
||||
m_appliedTorque.setValue(0, 0, 0);
|
||||
m_appliedConstraintForce.setValue(0, 0, 0);
|
||||
m_appliedConstraintTorque.setValue(0, 0, 0);
|
||||
//
|
||||
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
|
||||
m_jointPos[3] = 1.f; //"quat.w"
|
||||
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
|
||||
m_cachedWorldTransform.setIdentity();
|
||||
}
|
||||
|
||||
// routine to update m_cachedRotParentToThis and m_cachedRVector
|
||||
void updateCacheMultiDof(btScalar *pq = 0)
|
||||
{
|
||||
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
|
||||
|
||||
switch (m_jointType)
|
||||
{
|
||||
case eRevolute:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePrismatic:
|
||||
{
|
||||
// m_cachedRotParentToThis never changes, so no need to update
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
|
||||
|
||||
break;
|
||||
}
|
||||
case eSpherical:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case ePlanar:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
|
||||
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
case eFixed:
|
||||
{
|
||||
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
||||
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//invalid type
|
||||
btAssert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_LINK_H
|
||||
|
||||
@@ -1,216 +1,216 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyPoint2Point.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
#define BTMBP2PCONSTRAINT_DIM 3
|
||||
#else
|
||||
#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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// 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);
|
||||
|
||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||
|
||||
btVector3 contactNormalOnB(0, 0, 0);
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
contactNormalOnB[i] = -1;
|
||||
#else
|
||||
contactNormalOnB[i % 3] = -1;
|
||||
#endif
|
||||
|
||||
// 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
|
||||
{
|
||||
if (m_bodyA)
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
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;
|
||||
|
||||
#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
|
||||
#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;
|
||||
|
||||
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);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodyPoint2Point.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
#define BTMBP2PCONSTRAINT_DIM 3
|
||||
#else
|
||||
#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)
|
||||
{
|
||||
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)
|
||||
{
|
||||
m_data.resize(BTMBP2PCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodyPoint2Point::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
|
||||
btMultiBodyJacobianData& data,
|
||||
const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// 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);
|
||||
|
||||
constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
|
||||
constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
|
||||
|
||||
btVector3 contactNormalOnB(0, 0, 0);
|
||||
#ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
|
||||
contactNormalOnB[i] = -1;
|
||||
#else
|
||||
contactNormalOnB[i % 3] = -1;
|
||||
#endif
|
||||
|
||||
// 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
|
||||
{
|
||||
if (m_bodyA)
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
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;
|
||||
|
||||
#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
|
||||
#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;
|
||||
|
||||
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);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,234 +1,234 @@
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodySliderConstraint.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#define BTMBSLIDERCONSTRAINT_DIM 5
|
||||
#define EPSILON 0.000001
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB),
|
||||
m_jointAxis(jointAxis)
|
||||
{
|
||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB),
|
||||
m_jointAxis(jointAxis)
|
||||
{
|
||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodySliderConstraint::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodySliderConstraint::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodySliderConstraint::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
btMatrix3x3 frameAworld = m_frameInA;
|
||||
btVector3 jointAxis = m_jointAxis;
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||
jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
|
||||
}
|
||||
else if (m_bodyA)
|
||||
{
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
|
||||
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
btMatrix3x3 frameBworld = m_frameInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||
}
|
||||
else if (m_bodyB)
|
||||
{
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
|
||||
}
|
||||
|
||||
btVector3 constraintAxis[2];
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
|
||||
if (constraintAxis[0].safeNorm() > EPSILON)
|
||||
{
|
||||
constraintAxis[0] = constraintAxis[0].normalized();
|
||||
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
|
||||
constraintAxis[1] = constraintAxis[1].normalized();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||
btVector3 angleDiff;
|
||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||
|
||||
int numDim = BTMBSLIDERCONSTRAINT_DIM;
|
||||
for (int i = 0; i < numDim; i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
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;
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
}
|
||||
|
||||
btVector3 constraintNormalLin(0, 0, 0);
|
||||
btVector3 constraintNormalAng(0, 0, 0);
|
||||
btScalar posError = 0.0;
|
||||
if (i < 2)
|
||||
{
|
||||
constraintNormalLin = constraintAxis[i];
|
||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||
}
|
||||
else
|
||||
{ //i>=2
|
||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||
posError = angleDiff[i % 3];
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
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,
|
||||
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.
|
||||
*/
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
|
||||
#include "btMultiBodySliderConstraint.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
|
||||
#include "LinearMath/btIDebugDraw.h"
|
||||
|
||||
#define BTMBSLIDERCONSTRAINT_DIM 5
|
||||
#define EPSILON 0.000001
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(body, 0, link, -1, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(bodyB),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB),
|
||||
m_jointAxis(jointAxis)
|
||||
{
|
||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
|
||||
: btMultiBodyConstraint(bodyA, bodyB, linkA, linkB, BTMBSLIDERCONSTRAINT_DIM, false),
|
||||
m_rigidBodyA(0),
|
||||
m_rigidBodyB(0),
|
||||
m_pivotInA(pivotInA),
|
||||
m_pivotInB(pivotInB),
|
||||
m_frameInA(frameInA),
|
||||
m_frameInB(frameInB),
|
||||
m_jointAxis(jointAxis)
|
||||
{
|
||||
m_data.resize(BTMBSLIDERCONSTRAINT_DIM); //at least store the applied impulses
|
||||
}
|
||||
|
||||
void btMultiBodySliderConstraint::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodySliderConstraint::~btMultiBodySliderConstraint()
|
||||
{
|
||||
}
|
||||
|
||||
int btMultiBodySliderConstraint::getIslandIdA() const
|
||||
{
|
||||
if (m_rigidBodyA)
|
||||
return m_rigidBodyA->getIslandTag();
|
||||
|
||||
if (m_bodyA)
|
||||
{
|
||||
if (m_linkA < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyA->getLink(m_linkA).m_collider)
|
||||
return m_bodyA->getLink(m_linkA).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
int btMultiBodySliderConstraint::getIslandIdB() const
|
||||
{
|
||||
if (m_rigidBodyB)
|
||||
return m_rigidBodyB->getIslandTag();
|
||||
if (m_bodyB)
|
||||
{
|
||||
if (m_linkB < 0)
|
||||
{
|
||||
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
|
||||
if (col)
|
||||
return col->getIslandTag();
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_bodyB->getLink(m_linkB).m_collider)
|
||||
return m_bodyB->getLink(m_linkB).m_collider->getIslandTag();
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
// Convert local points back to world
|
||||
btVector3 pivotAworld = m_pivotInA;
|
||||
btMatrix3x3 frameAworld = m_frameInA;
|
||||
btVector3 jointAxis = m_jointAxis;
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
pivotAworld = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
frameAworld = m_frameInA.transpose() * btMatrix3x3(m_rigidBodyA->getOrientation());
|
||||
jointAxis = quatRotate(m_rigidBodyA->getOrientation(), m_jointAxis);
|
||||
}
|
||||
else if (m_bodyA)
|
||||
{
|
||||
pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
|
||||
jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis);
|
||||
}
|
||||
btVector3 pivotBworld = m_pivotInB;
|
||||
btMatrix3x3 frameBworld = m_frameInB;
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
pivotBworld = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
frameBworld = m_frameInB.transpose() * btMatrix3x3(m_rigidBodyB->getOrientation());
|
||||
}
|
||||
else if (m_bodyB)
|
||||
{
|
||||
pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
|
||||
}
|
||||
|
||||
btVector3 constraintAxis[2];
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
|
||||
if (constraintAxis[0].safeNorm() > EPSILON)
|
||||
{
|
||||
constraintAxis[0] = constraintAxis[0].normalized();
|
||||
constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
|
||||
constraintAxis[1] = constraintAxis[1].normalized();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
btMatrix3x3 relRot = frameAworld.inverse() * frameBworld;
|
||||
btVector3 angleDiff;
|
||||
btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot, angleDiff);
|
||||
|
||||
int numDim = BTMBSLIDERCONSTRAINT_DIM;
|
||||
for (int i = 0; i < numDim; i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
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;
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
|
||||
}
|
||||
|
||||
btVector3 constraintNormalLin(0, 0, 0);
|
||||
btVector3 constraintNormalAng(0, 0, 0);
|
||||
btScalar posError = 0.0;
|
||||
if (i < 2)
|
||||
{
|
||||
constraintNormalLin = constraintAxis[i];
|
||||
posError = (pivotAworld - pivotBworld).dot(constraintNormalLin);
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse);
|
||||
}
|
||||
else
|
||||
{ //i>=2
|
||||
constraintNormalAng = frameAworld.getColumn(i % 3);
|
||||
posError = angleDiff[i % 3];
|
||||
fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
|
||||
constraintNormalLin, pivotAworld, pivotBworld,
|
||||
posError,
|
||||
infoGlobal,
|
||||
-m_maxAppliedImpulse, m_maxAppliedImpulse, true);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer)
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
|
||||
if (m_rigidBodyA)
|
||||
{
|
||||
btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyA)
|
||||
{
|
||||
btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
|
||||
tr.setOrigin(pivotAworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_rigidBodyB)
|
||||
{
|
||||
// that ideally should draw the same frame
|
||||
btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
|
||||
tr.setOrigin(pivot);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
if (m_bodyB)
|
||||
{
|
||||
btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
|
||||
tr.setOrigin(pivotBworld);
|
||||
drawer->drawTransform(tr, 0.1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,14 +1,14 @@
|
||||
project "BulletSoftBody"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
includedirs {
|
||||
"..",
|
||||
}
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h"
|
||||
project "BulletSoftBody"
|
||||
|
||||
kind "StaticLib"
|
||||
|
||||
includedirs {
|
||||
"..",
|
||||
}
|
||||
if os.is("Linux") then
|
||||
buildoptions{"-fPIC"}
|
||||
end
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h"
|
||||
}
|
||||
Reference in New Issue
Block a user