public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass)
{
IndexedMatrix trA = GetCalculatedTransformA();
IndexedMatrix trB = GetCalculatedTransformB();
Debug.Assert(!m_useSolveConstraintObsolete);
int i, s = 1;
float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f;
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = rbAinvMass;
float miB = rbBinvMass;
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
IndexedVector3 ax1 = IndexedVector3.Zero, p, q;
IndexedVector3 ax1A = trA._basis.GetColumn(0);
IndexedVector3 ax1B = trB._basis.GetColumn(0);
if (m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// construct two orthos to slider axis
TransformUtil.PlaneSpace1(ref ax1, out p, out q);
}
else
{ // old way - use frameA
ax1 = trA._basis.GetColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA._basis.GetColumn(1);
q = trA._basis.GetColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info.m_solverConstraints[0].m_relpos1CrossNormal = p;
info.m_solverConstraints[s].m_relpos1CrossNormal = q;
info.m_solverConstraints[0].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// float k = info.fps * info.erp * getSoftnessOrthoAng();
float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
float k = info.fps * currERP;
IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B);
info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q);
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0)
{
info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
int srow = nrow;
float limit_err;
int limit;
bool powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero;
if (m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to slider axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
float sliderOffs = m_linPos - m_depth.X;
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill two rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(relA, q);
tmpB = IndexedVector3.Cross(relB, q);
if (hasStaticBody && GetSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
else
{
// old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
IndexedVector3 tmp = IndexedVector3.Cross(c, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp;
tmp = IndexedVector3.Cross(c, q);
info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
// compute two elements of right hand side
// k = info.fps * info.erp * getSoftnessOrthoLin();
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp;
k = info.fps * currERP;
float rhs = k * IndexedVector3.Dot(p, ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(q, ofs);
info.m_solverConstraints[s3].m_rhs = rhs;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0)
{
info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin;
info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin;
}
// check linear limits
limit_err = 0.0f;
limit = 0;
if (GetSolveLinLimit())
{
limit_err = GetLinDepth() * signFact;
limit = (limit_err > 0f) ? 2 : 1;
}
powered = false;
if (GetPoweredLinMotor())
{
powered = true;
}
// if the slider has joint limits or motor, add in the extra row
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_contactNormal = ax1;
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if (m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if (!hasStaticBody)
{
tmpA = IndexedVector3.Cross(relA, ax1);
tmpB = IndexedVector3.Cross(relB, ax1);
info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB;
}
}
else
{
// The old way. May be incorrect if bodies are not on the slider axis
IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque)
info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd;
}
// right-hand part
float lostop = GetLowerLinLimit();
float histop = GetUpperLinLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0f;
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin;
}
float tag_vel = GetTargetLinMotorVelocity();
float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin;
}
if (MathUtil.CompareFloat(lostop, histop))
{ // limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
float bounce = Math.Abs(1.0f - GetDampingLimLin());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(linVelA, ax1);
vel -= IndexedVector3.Dot(linVelB, ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = 0.0f;
limit = 0;
if (GetSolveAngLimit())
{
limit_err = GetAngDepth();
limit = (limit_err > 0.0f) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = false;
if (GetPoweredAngMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerAngLimit();
float histop = GetUpperAngLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng;
}
float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
float bounce = Math.Abs(1.0f - GetDampingLimAng());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1);
vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
#endif
}
public bool EncloseOrigin()
{
switch (m_simplex.rank)
{
case 1:
{
for (int i = 0; i < 3; ++i)
{
IndexedVector3 axis = IndexedVector3.Zero;
axis[i] = 1f;
AppendVertice(m_simplex, ref axis);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
IndexedVector3 temp = -axis;
AppendVertice(m_simplex, ref temp);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
}
break;
}
case 2:
{
IndexedVector3 d = m_simplex.c[1].w - m_simplex.c[0].w;
for (int i = 0; i < 3; ++i)
{
IndexedVector3 axis = IndexedVector3.Zero;
axis[i] = 1f;
IndexedVector3 p = IndexedVector3.Cross(d, axis);
if (p.LengthSquared() > 0)
{
AppendVertice(m_simplex, ref p);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
IndexedVector3 temp = -p;
AppendVertice(m_simplex, ref temp);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
}
}
break;
}
case 3:
{
IndexedVector3 n = IndexedVector3.Cross(m_simplex.c[1].w - m_simplex.c[0].w,
m_simplex.c[2].w - m_simplex.c[0].w);
if (n.LengthSquared() > 0)
{
AppendVertice(m_simplex, ref n);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
IndexedVector3 temp = -n;
AppendVertice(m_simplex, ref temp);
if (EncloseOrigin())
{
return(true);
}
RemoveVertice(m_simplex);
}
break;
}
case 4:
{
if (Math.Abs(GJK.Det(m_simplex.c[0].w - m_simplex.c[3].w,
m_simplex.c[1].w - m_simplex.c[3].w,
m_simplex.c[2].w - m_simplex.c[3].w)) > 0)
{
return(true);
}
break;
}
default:
{
break;
}
}
return(false);
}
public eStatus Evaluate(GJK gjk, ref IndexedVector3 guess)
{
sSimplex simplex = gjk.m_simplex;
if ((simplex.rank > 1) && gjk.EncloseOrigin())
{
/* Clean up */
while (m_hull.Count > 0)
{
sFace f = m_hull[0];
Remove(m_hull, f);
Append(m_stock, f);
}
m_status = eStatus.Valid;
m_nextsv = 0;
/* Orient simplex */
if (GJK.Det(simplex.c[0].w - simplex.c[3].w,
simplex.c[1].w - simplex.c[3].w,
simplex.c[2].w - simplex.c[3].w) < 0)
{
SwapSv(simplex.c, 0, 1);
SwapFloat(simplex.p, 0, 1);
}
/* Build initial hull */
tetra[0] = NewFace(simplex.c[0], simplex.c[1], simplex.c[2], true);
tetra[1] = NewFace(simplex.c[1], simplex.c[0], simplex.c[3], true);
tetra[2] = NewFace(simplex.c[2], simplex.c[1], simplex.c[3], true);
tetra[3] = NewFace(simplex.c[0], simplex.c[2], simplex.c[3], true);
if (m_hull.Count == 4)
{
sFace best = FindBest();
sFace outer = best;
uint pass = 0;
uint iterations = 0;
Bind(tetra[0], 0, tetra[1], 0);
Bind(tetra[0], 1, tetra[2], 0);
Bind(tetra[0], 2, tetra[3], 0);
Bind(tetra[1], 1, tetra[3], 2);
Bind(tetra[1], 2, tetra[2], 1);
Bind(tetra[2], 2, tetra[3], 1);
m_status = eStatus.Valid;
for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
{
if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
{
sHorizon horizon = new sHorizon();
sSV w = m_sv_store[m_nextsv++];
bool valid = true;
best.pass = (uint)(++pass);
gjk.GetSupport(ref best.n, ref w);
float wdist = IndexedVector3.Dot(ref best.n, ref w.w) - best.d;
if (wdist > GjkEpaSolver2.EPA_ACCURACY)
{
for (int j = 0; (j < 3) && valid; ++j)
{
valid &= Expand(pass, w,
best.f[j], best.e[j],
ref horizon);
}
if (valid && (horizon.nf >= 3))
{
Bind(horizon.cf, 1, horizon.ff, 2);
Remove(m_hull, best);
Append(m_stock, best);
best = FindBest();
if (best.p >= outer.p)
{
outer = best;
}
}
else
{
m_status = eStatus.InvalidHull;
break;
}
}
else
{
m_status = eStatus.AccuraryReached;
break;
}
}
else
{
m_status = eStatus.OutOfVertices;
break;
}
}
IndexedVector3 projection = outer.n * outer.d;
m_normal = outer.n;
m_depth = outer.d;
m_result.rank = 3;
m_result.c[0] = outer.c[0];
m_result.c[1] = outer.c[1];
m_result.c[2] = outer.c[2];
m_result.p[0] = IndexedVector3.Cross(outer.c[1].w - projection,
outer.c[2].w - projection).Length();
m_result.p[1] = IndexedVector3.Cross(outer.c[2].w - projection,
outer.c[0].w - projection).Length();
m_result.p[2] = IndexedVector3.Cross(outer.c[0].w - projection,
outer.c[1].w - projection).Length();
float sum = m_result.p[0] + m_result.p[1] + m_result.p[2];
m_result.p[0] /= sum;
m_result.p[1] /= sum;
m_result.p[2] /= sum;
return(m_status);
}
}
/* Fallback */
m_status = eStatus.FallBack;
m_normal = -guess;
float nl = m_normal.LengthSquared();
if (nl > 0)
{
m_normal.Normalize();
}
else
{
m_normal = new IndexedVector3(1, 0, 0);
}
m_depth = 0;
m_result.rank = 1;
m_result.c[0] = simplex.c[0];
m_result.p[0] = 1;
return(m_status);
}
public TestRig(DemoApplication demoApplication, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, bool bFixed)
{
m_dynamicsWorld = ownerWorld;
IndexedVector3 vUp = new IndexedVector3(0, 1, 0);
//
// Setup geometry
//
float fBodySize = 0.25f;
float fLegLength = 0.45f;
float fForeLegLength = 0.75f;
m_shapes[0] = new CapsuleShape(fBodySize, 0.10f);
for (int i = 0; i < NUM_LEGS; i++)
{
m_shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
m_shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
}
//
// Setup rigid bodies
//
float fHeight = 0.5f;
IndexedMatrix offset = IndexedMatrix.Identity;
offset._origin = positionOffset;
// root
IndexedVector3 vRoot = new IndexedVector3(0, fHeight, 0);
IndexedMatrix transform = IndexedMatrix.Identity;
transform._origin = vRoot;
if (bFixed)
{
m_bodies[0] = demoApplication.LocalCreateRigidBody(0.0f, offset * transform, m_shapes[0]);
}
else
{
m_bodies[0] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[0]);
}
// legs
for (int i = 0; i < NUM_LEGS; i++)
{
float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
transform = IndexedMatrix.Identity;
IndexedVector3 vBoneOrigin = new IndexedVector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));
transform._origin = vBoneOrigin;
// thigh
IndexedVector3 vToBone = (vBoneOrigin - vRoot);
vToBone.Normalize();
IndexedVector3 vAxis = IndexedVector3.Cross(vToBone, vUp);
transform._basis = new IndexedBasisMatrix(new IndexedQuaternion(vAxis, MathUtil.SIMD_HALF_PI));
transform._origin = vBoneOrigin;
m_bodies[1 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[1 + 2 * i]);
// shin
transform = IndexedMatrix.Identity;
transform._origin = new IndexedVector3(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
m_bodies[2 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[2 + 2 * i]);
}
// Setup some damping on the m_bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
m_bodies[i].SetDamping(0.05f, 0.85f);
m_bodies[i].SetDeactivationTime(0.8f);
//m_bodies[i].setSleepingThresholds(1.6, 2.5);
m_bodies[i].SetSleepingThresholds(0.5f, 0.5f);
}
//
// Setup the constraints
//
HingeConstraint hingeC;
IndexedMatrix localA, localB, localC;
for (int i = 0; i < NUM_LEGS; i++)
{
float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
// hip joints
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
localA._origin = new IndexedVector3(fCos * fBodySize, 0.0f, fSin * fBodySize);
localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
if (BulletGlobals.g_streamWriter != null && false)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalA", localA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalB", localB);
}
hingeC = new HingeConstraint(m_bodies[0], m_bodies[1 + 2 * i], ref localA, ref localB);
hingeC.SetLimit(-0.75f * MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI / 2f);
m_joints[2 * i] = hingeC;
m_dynamicsWorld.AddConstraint(m_joints[2 * i], true);
// knee joints
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localC = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
localA._origin = new IndexedVector3(fCos * (fBodySize + fLegLength), 0.0f, fSin * (fBodySize + fLegLength));
localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
localC = m_bodies[2 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
if (BulletGlobals.g_streamWriter != null && false)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalA", localA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalB", localB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalC", localC);
}
hingeC = new HingeConstraint(m_bodies[1 + 2 * i], m_bodies[2 + 2 * i], ref localB, ref localC);
//hingeC.setLimit(float(-0.01), float(0.01));
hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI / 2f, 0.2f);
m_joints[1 + 2 * i] = hingeC;
m_dynamicsWorld.AddConstraint(m_joints[1 + 2 * i], true);
}
}
public void InternalGetVelocityInLocalPointObsolete(ref IndexedVector3 rel_pos, ref IndexedVector3 velocity)
{
velocity = GetLinearVelocity() + m_deltaLinearVelocity + IndexedVector3.Cross((GetAngularVelocity() + m_deltaAngularVelocity), rel_pos);
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
//skip self-collisions
if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex))
{
return;
}
//skip duplicates (disabled for now)
//if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex))
// return;
//search for shared vertices and edges
int numshared = 0;
int[] sharedVertsA = new int[] { -1, -1, -1 };
int[] sharedVertsB = new int[] { -1, -1, -1 };
///skip degenerate triangles
float crossBSqr = IndexedVector3.Cross((triangle[1] - triangle[0]), (triangle[2] - triangle[0])).LengthSquared();
if (crossBSqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
float crossASqr = IndexedVector3.Cross((m_triangleVerticesA[1] - m_triangleVerticesA[0]), (m_triangleVerticesA[2] - m_triangleVerticesA[0])).LengthSquared();
///skip degenerate triangles
if (crossASqr < m_triangleInfoMap.m_equalVertexThreshold)
{
return;
}
#if false
printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n",
m_triangleVerticesA[0].GetX(), m_triangleVerticesA[0].GetY(), m_triangleVerticesA[0].GetZ(),
m_triangleVerticesA[1].GetX(), m_triangleVerticesA[1].GetY(), m_triangleVerticesA[1].GetZ(),
m_triangleVerticesA[2].GetX(), m_triangleVerticesA[2].GetY(), m_triangleVerticesA[2].GetZ());
printf("partId=%d, triangleIndex=%d\n", partId, triangleIndex);
printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n",
triangle[0].GetX(), triangle[0].GetY(), triangle[0].GetZ(),
triangle[1].GetX(), triangle[1].GetY(), triangle[1].GetZ(),
triangle[2].GetX(), triangle[2].GetY(), triangle[2].GetZ());
#endif
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
if ((m_triangleVerticesA[i] - triangle[j]).LengthSquared() < m_triangleInfoMap.m_equalVertexThreshold)
{
sharedVertsA[numshared] = i;
sharedVertsB[numshared] = j;
numshared++;
///degenerate case
if (numshared >= 3)
{
return;
}
}
}
///degenerate case
if (numshared >= 3)
{
return;
}
}
switch (numshared)
{
case 0:
{
break;
}
case 1:
{
//shared vertex
break;
}
case 2:
{
//shared edge
//we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct
if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2)
{
sharedVertsA[0] = 2;
sharedVertsA[1] = 0;
int tmp = sharedVertsB[1];
sharedVertsB[1] = sharedVertsB[0];
sharedVertsB[0] = tmp;
}
int hash = GetHash(m_partIdA, m_triangleIndexA);
TriangleInfo info = null;
if (m_triangleInfoMap.ContainsKey(hash))
{
info = m_triangleInfoMap[hash];
}
else
{
info = new TriangleInfo();
m_triangleInfoMap[hash] = info;
}
int sumvertsA = sharedVertsA[0] + sharedVertsA[1];
int otherIndexA = 3 - sumvertsA;
IndexedVector3 edge = new IndexedVector3(m_triangleVerticesA[sharedVertsA[1]] - m_triangleVerticesA[sharedVertsA[0]]);
TriangleShape tA = new TriangleShape(m_triangleVerticesA[0], m_triangleVerticesA[1], m_triangleVerticesA[2]);
int otherIndexB = 3 - (sharedVertsB[0] + sharedVertsB[1]);
TriangleShape tB = new TriangleShape(triangle[sharedVertsB[1]], triangle[sharedVertsB[0]], triangle[otherIndexB]);
//btTriangleShape tB(triangle[0],triangle[1],triangle[2]);
IndexedVector3 normalA;
IndexedVector3 normalB;
tA.CalcNormal(out normalA);
tB.CalcNormal(out normalB);
edge.Normalize();
IndexedVector3 edgeCrossA = IndexedVector3.Normalize(IndexedVector3.Cross(edge, normalA));
{
IndexedVector3 tmp = m_triangleVerticesA[otherIndexA] - m_triangleVerticesA[sharedVertsA[0]];
if (IndexedVector3.Dot(edgeCrossA, tmp) < 0)
{
edgeCrossA *= -1;
}
}
IndexedVector3 edgeCrossB = IndexedVector3.Cross(edge, normalB).Normalized();
{
IndexedVector3 tmp = triangle[otherIndexB] - triangle[sharedVertsB[0]];
if (IndexedVector3.Dot(edgeCrossB, tmp) < 0)
{
edgeCrossB *= -1;
}
}
float angle2 = 0;
float ang4 = 0.0f;
IndexedVector3 calculatedEdge = IndexedVector3.Cross(edgeCrossA, edgeCrossB);
float len2 = calculatedEdge.LengthSquared();
float correctedAngle = 0f;
IndexedVector3 calculatedNormalB = normalA;
bool isConvex = false;
if (len2 < m_triangleInfoMap.m_planarEpsilon)
{
angle2 = 0.0f;
ang4 = 0.0f;
}
else
{
calculatedEdge.Normalize();
IndexedVector3 calculatedNormalA = IndexedVector3.Cross(calculatedEdge, edgeCrossA);
calculatedNormalA.Normalize();
angle2 = GetAngle(ref calculatedNormalA, ref edgeCrossA, ref edgeCrossB);
ang4 = MathUtil.SIMD_PI - angle2;
float dotA = IndexedVector3.Dot(normalA, edgeCrossB);
///@todo: check if we need some epsilon, due to floating point imprecision
isConvex = (dotA < 0f);
correctedAngle = isConvex ? ang4 : -ang4;
IndexedQuaternion orn2 = new IndexedQuaternion(calculatedEdge, -correctedAngle);
IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromQuaternion(orn2);
calculatedNormalB = new IndexedBasisMatrix(orn2) * normalA;
}
//alternatively use
//IndexedVector3 calculatedNormalB2 = quatRotate(orn,normalA);
switch (sumvertsA)
{
case 1:
{
IndexedVector3 edge1 = m_triangleVerticesA[0] - m_triangleVerticesA[1];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
float bla = IndexedVector3.Dot(computedNormalB, normalB);
if (bla < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001f)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif//DEBUG_INTERNAL_EDGE
info.m_edgeV0V1Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V0V1_CONVEX;
}
break;
}
case 2:
{
IndexedVector3 edge1 = m_triangleVerticesA[2] - m_triangleVerticesA[0];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
computedNormalB *= -1;
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_SWAP_NORMALB;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV2V0Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V2V0_CONVEX;
}
break;
}
case 3:
{
IndexedVector3 edge1 = m_triangleVerticesA[1] - m_triangleVerticesA[2];
IndexedQuaternion orn = new IndexedQuaternion(edge1, -correctedAngle);
IndexedVector3 computedNormalB = MathUtil.QuatRotate(orn, normalA);
if (IndexedVector3.Dot(computedNormalB, normalB) < 0)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_SWAP_NORMALB;
computedNormalB *= -1;
}
#if DEBUG_INTERNAL_EDGE
if ((computedNormalB - normalB).Length() > 0.0001)
{
System.Console.WriteLine("warning: normals not identical");
}
#endif //DEBUG_INTERNAL_EDGE
info.m_edgeV1V2Angle = -correctedAngle;
if (isConvex)
{
info.m_flags |= TriangleInfoMap.TRI_INFO_V1V2_CONVEX;
}
break;
}
}
break;
}
default:
{
// printf("warning: duplicate triangle\n");
break;
}
}
}
public static bool FindSeparatingAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, out IndexedVector3 sep)
{
gActualSATPairTests++;
// dummy value to satisfy exit points.
sep = new IndexedVector3(0, 1, 0);
#if TEST_INTERNAL_OBJECTS
IndexedVector3 c0 = transA * hullA.m_localCenter;
IndexedVector3 c1 = transB * hullB.m_localCenter;
IndexedVector3 DeltaC2 = c0 - c1;
#endif
float dmin = float.MaxValue;
int curPlaneTests = 0;
int numFacesA = hullA.m_faces.Count;
// Test normals from hullA
for (int i = 0; i < numFacesA; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref faceANormalWS, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref faceANormalWS, out d))
{
return(false);
}
if (d < dmin)
{
dmin = d;
sep = faceANormalWS;
}
}
int numFacesB = hullB.m_faces.Count;
// Test normals from hullB
for (int i = 0; i < numFacesB; i++)
{
IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]);
IndexedVector3 WorldNormal = transB._basis * Normal;
curPlaneTests++;
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref WorldNormal, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float d;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref WorldNormal, out d))
{
return(false);
}
if (d < dmin)
{
dmin = d;
sep = WorldNormal;
}
}
IndexedVector3 edgeAstart, edgeAend, edgeBstart, edgeBend;
int curEdgeEdge = 0;
// Test edges
for (int e0 = 0; e0 < hullA.m_uniqueEdges.Count; e0++)
{
IndexedVector3 edge0 = hullA.m_uniqueEdges[e0];
IndexedVector3 WorldEdge0 = transA._basis * edge0;
for (int e1 = 0; e1 < hullB.m_uniqueEdges.Count; e1++)
{
IndexedVector3 edge1 = hullB.m_uniqueEdges[e1];
IndexedVector3 WorldEdge1 = transB._basis * edge1;
IndexedVector3 Cross = IndexedVector3.Cross(WorldEdge0, WorldEdge1);
curEdgeEdge++;
if (!MathUtil.IsAlmostZero(ref Cross))
{
Cross.Normalize();
#if TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(ref transA, ref transB, ref DeltaC2, ref Cross, hullA, hullB, dmin))
{
continue;
}
gActualNbTests++;
#endif
float dist;
if (!TestSepAxis(hullA, hullB, ref transA, ref transB, ref Cross, out dist))
{
return(false);
}
if (dist < dmin)
{
dmin = dist;
sep = Cross;
}
}
}
}
IndexedVector3 deltaC = transB._origin - transA._origin;
if ((IndexedVector3.Dot(deltaC, sep)) > 0.0f)
{
sep = -sep;
}
return(true);
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
{
m_angularOnly = false;
m_enableAngularMotor = false;
m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
m_useReferenceFrameA = useReferenceFrameA;
m_rbAFrame._origin = pivotInA;
#if _BT_USE_CENTER_LIMIT_
m_limit = new AngularLimit();
#endif
m_flags = 0;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);
IndexedVector3 rbAxisA2 = IndexedVector3.Zero;
float projection = IndexedVector3.Dot(axisInA, rbAxisA1);
if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
{
rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
}
else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
{
rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
}
else
{
rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
}
//m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);
IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);
m_rbBFrame._origin = pivotInB;
//m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);
#if !_BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = float(1.0f);
m_upperLimit = float(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
}
public void CalcNormal(out IndexedVector3 normal)
{
normal = IndexedVector3.Cross(m_vertices1[1] - m_vertices1[0], m_vertices1[2] - m_vertices1[0]);
normal.Normalize();
}
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
// transforms in world space
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
// linear (all fixed)
//info.m_J1linearAxis[0] = 1;
//info.m_J1linearAxis[s + 1] = 1;
//info.m_J1linearAxis[2 * s + 2] = 1;
if (!m_angularOnly)
{
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
}
IndexedVector3 a1 = pivotAInW - transA._origin;
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
//if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15)
//{
// int ibreak = 0;
//}
int ibreak = 0;
}
IndexedVector3 a2 = pivotBInW - transB._origin;
{
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// linear RHS
float k = info.fps * info.erp;
if (!m_angularOnly)
{
for (int i = 0; i < 3; i++)
{
float val = k * (pivotBInW[i] - pivotAInW[i]);
info.m_solverConstraints[i].m_rhs = val;
}
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get hinge axis (Z)
IndexedVector3 ax1 = trA._basis.GetColumn(2);
// get 2 orthos to hinge axis (X, Y)
IndexedVector3 p = trA._basis.GetColumn(0);
IndexedVector3 q = trA._basis.GetColumn(1);
// set the two hinge angular rows
MathUtil.SanityCheckVector(ax1);
MathUtil.SanityCheckVector(p);
MathUtil.SanityCheckVector(q);
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
IndexedVector3 ax2 = trB._basis.GetColumn(2);
IndexedVector3 u = IndexedVector3.Cross(ax1, ax2);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
// check angular limits
int nrow = 4; // last filled row
float limit_err = 0.0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0.0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(angVelA, ax1);
vel -= IndexedVector3.Dot(angVelB, ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[nrow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
// transforms in world space
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB);
}
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB);
}
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
#if true
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = GetRigidBodyA().GetInvMass();
float miB = GetRigidBodyB().GetInvMass();
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
IndexedVector3 ax1A = trA._basis.GetColumn(2);
IndexedVector3 ax1B = trB._basis.GetColumn(2);
IndexedVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
int s0 = 0;
int s1 = 1;
int s2 = 2;
int nrow = 2; // last filled row
IndexedVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to hinge axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
IndexedVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill three rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref q);
tmpB = IndexedVector3.Cross(ref relB, ref q);
if (hasStaticBody && GetSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref ax1);
tmpB = IndexedVector3.Cross(ref relB, ref ax1);
if (hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
float k = info.fps * info.erp;
if (!m_angularOnly)
{
info.m_solverConstraints[s0].m_contactNormal = p;
info.m_solverConstraints[s1].m_contactNormal = q;
info.m_solverConstraints[s2].m_contactNormal = ax1;
// compute three elements of right hand side
float rhs = k * IndexedVector3.Dot(ref p, ref ofs);
info.m_solverConstraints[s0].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref q, ref ofs);
info.m_solverConstraints[s1].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref ax1, ref ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info.fps * info.erp;
IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
float limit_err = 0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > 0f) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[srow].m_rhs = 0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[srow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[srow].m_lowerLimit = 0;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(ref angVelA, ref ax1);
vel -= IndexedVector3.Dot(ref angVelB, ref ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[srow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public virtual void ProcessTriangle(IndexedVector3[] triangle, int partId, int triangleIndex)
{
IndexedVector3 v10 = triangle[1] - triangle[0];
IndexedVector3 v20 = triangle[2] - triangle[0];
IndexedVector3 triangleNormal = v10.Cross(ref v20);
float dist = IndexedVector3.Dot(ref triangle[0], ref triangleNormal);
float dist_a = IndexedVector3.Dot(ref triangleNormal, ref m_from);
dist_a -= dist;
float dist_b = IndexedVector3.Dot(ref triangleNormal, ref m_to);
dist_b -= dist;
if (dist_a * dist_b >= 0f)
{
return; // same sign
}
//@BP Mod - Backface filtering
if (((m_flags & EFlags.kF_FilterBackfaces) != 0) && (dist_a > 0f))
{
// Backface, skip check
return;
}
float proj_length = dist_a - dist_b;
float distance = (dist_a) / (proj_length);
// Now we have the intersection point on the plane, we'll see if it's inside the triangle
// Add an epsilon as a tolerance for the raycast,
// in case the ray hits exacly on the edge of the triangle.
// It must be scaled for the triangle size.
if (distance < m_hitFraction)
{
float edge_tolerance = triangleNormal.LengthSquared();
edge_tolerance *= -0.0001f;
IndexedVector3 point;
point = MathUtil.Interpolate3(ref m_from, ref m_to, distance);
{
IndexedVector3 v0p = triangle[0] - point;
IndexedVector3 v1p = triangle[1] - point;
IndexedVector3 cp0 = v0p.Cross(ref v1p);
if (IndexedVector3.Dot(ref cp0, ref triangleNormal) >= edge_tolerance)
{
IndexedVector3 v2p = triangle[2] - point;
IndexedVector3 cp1 = v1p.Cross(ref v2p);//= IndexedVector3.Cross(v1p,v2p);
if (IndexedVector3.Dot(ref cp1, ref triangleNormal) >= edge_tolerance)
{
IndexedVector3 cp2 = v2p.Cross(ref v0p);
if (IndexedVector3.Dot(ref cp2, ref triangleNormal) >= edge_tolerance)
{
//@BP Mod
// Triangle normal isn't normalized
triangleNormal.Normalize();
//@BP Mod - Allow for unflipped normal when raycasting against backfaces
if (((m_flags & EFlags.kF_KeepUnflippedNormal) == 0) && (dist_a <= 0.0f))
{
IndexedVector3 negNormal = -triangleNormal;
m_hitFraction = ReportHit(ref negNormal, distance, partId, triangleIndex);
}
else
{
m_hitFraction = ReportHit(ref triangleNormal, distance, partId, triangleIndex);
}
}
}
}
}
}
}
public void Initialize()
{
Dictionary <InternalVertexPair, InternalEdge> edges = new Dictionary <InternalVertexPair, InternalEdge>();
float TotalArea = 0.0f;
m_localCenter = IndexedVector3.Zero;
for (int i = 0; i < m_faces.Count; i++)
{
int numVertices = m_faces[i].m_indices.Count;
int NbTris = numVertices;
for (int j = 0; j < NbTris; j++)
{
int k = (j + 1) % numVertices;
InternalVertexPair vp = new InternalVertexPair(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
InternalEdge edptr = edges[vp];
IndexedVector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
edge.Normalize();
bool found = false;
for (int p = 0; p < m_uniqueEdges.Count; p++)
{
if (MathUtil.IsAlmostZero(m_uniqueEdges[p] - edge) ||
MathUtil.IsAlmostZero(m_uniqueEdges[p] + edge))
{
found = true;
break;
}
}
if (!found)
{
m_uniqueEdges.Add(edge);
}
if (edptr != null)
{
Debug.Assert(edptr.m_face0 >= 0);
Debug.Assert(edptr.m_face1 < 0);
edptr.m_face1 = (int)i;
}
else
{
InternalEdge ed = new InternalEdge();
ed.m_face0 = i;
edges[vp] = ed;
}
}
}
#if USE_CONNECTED_FACES
for (int i = 0; i < m_faces.Count; i++)
{
int numVertices = m_faces[i].m_indices.Count;
m_faces[i].m_connectedFaces.Resize(numVertices);
for (int j = 0; j < numVertices; j++)
{
int k = (j + 1) % numVertices;
InternalVertexPair vp = new InternalVertexPair(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
InternalEdge edptr = edges[vp];
Debug.Assert(edptr != null);
Debug.Assert(edptr.m_face0 >= 0);
Debug.Assert(edptr.m_face1 >= 0);
int connectedFace = (edptr.m_face0 == i) ? edptr.m_face1 : edptr.m_face0;
m_faces[i].m_connectedFaces[j] = connectedFace;
}
}
#endif
for (int i = 0; i < m_faces.Count; i++)
{
int numVertices = m_faces[i].m_indices.Count;
int NbTris = numVertices - 2;
IndexedVector3 p0 = m_vertices[m_faces[i].m_indices[0]];
for (int j = 1; j <= NbTris; j++)
{
int k = (j + 1) % numVertices;
IndexedVector3 p1 = m_vertices[m_faces[i].m_indices[j]];
IndexedVector3 p2 = m_vertices[m_faces[i].m_indices[k]];
float Area = IndexedVector3.Cross((p0 - p1), (p0 - p2)).Length() * 0.5f;
IndexedVector3 Center = (p0 + p1 + p2) / 3.0f;
m_localCenter += Area * Center;
TotalArea += Area;
}
}
m_localCenter /= TotalArea;
#if TEST_INTERNAL_OBJECTS
if (true)
{
m_radius = float.MaxValue;
for (int i = 0; i < m_faces.Count; i++)
{
IndexedVector3 Normal = new IndexedVector3(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
float dist = Math.Abs(m_localCenter.Dot(Normal) + m_faces[i].m_plane[3]);
if (dist < m_radius)
{
m_radius = dist;
}
}
float MinX = float.MaxValue;
float MinY = float.MaxValue;
float MinZ = float.MaxValue;
float MaxX = float.MinValue;
float MaxY = float.MinValue;
float MaxZ = float.MinValue;
for (int i = 0; i < m_vertices.Count; i++)
{
IndexedVector3 pt = m_vertices[i];
if (pt.X < MinX)
{
MinX = pt.X;
}
if (pt.X > MaxX)
{
MaxX = pt.X;
}
if (pt.Y < MinY)
{
MinY = pt.Y;
}
if (pt.Y > MaxY)
{
MaxY = pt.Y;
}
if (pt.Z < MinZ)
{
MinZ = pt.Z;
}
if (pt.Z > MaxZ)
{
MaxZ = pt.Z;
}
}
mC = new IndexedVector3(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
mE = new IndexedVector3(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
// const float r = m_radius / sqrtf(2.0f);
float r = m_radius / (float)Math.Sqrt(3.0f);
int LargestExtent = mE.MaxAxis();
float Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
m_extents.X = m_extents.Y = m_extents.Z = r;
m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
bool FoundBox = false;
for (int j = 0; j < 1024; j++)
{
if (TestContainment())
{
FoundBox = true;
break;
}
m_extents[LargestExtent] -= Step;
}
if (!FoundBox)
{
m_extents.X = m_extents.Y = m_extents.Z = r;
}
else
{
// Refine the box
float innerStep = (m_radius - r) / 1024.0f;
int e0 = (1 << LargestExtent) & 3;
int e1 = (1 << e0) & 3;
for (int j = 0; j < 1024; j++)
{
float Saved0 = m_extents[e0];
float Saved1 = m_extents[e1];
m_extents[e0] += innerStep;
m_extents[e1] += innerStep;
if (!TestContainment())
{
m_extents[e0] = Saved0;
m_extents[e1] = Saved1;
break;
}
}
}
}
#endif
}
public void DrawSpherePatch(ref IndexedVector3 center, ref IndexedVector3 up, ref IndexedVector3 axis, float radius, float minTh, float maxTh, float minPs, float maxPs, ref IndexedVector3 color, float stepDegrees)
{
IndexedVector3[] vA;
IndexedVector3[] vB;
IndexedVector3[] pvA, pvB, pT;
IndexedVector3 npole = center + up * radius;
IndexedVector3 spole = center - up * radius;
IndexedVector3 arcStart = IndexedVector3.Zero;
float step = stepDegrees * MathUtil.SIMD_RADS_PER_DEG;
IndexedVector3 kv = up;
IndexedVector3 iv = axis;
IndexedVector3 jv = IndexedVector3.Cross(kv, iv);
bool drawN = false;
bool drawS = false;
if (minTh <= -MathUtil.SIMD_HALF_PI)
{
minTh = -MathUtil.SIMD_HALF_PI + step;
drawN = true;
}
if (maxTh >= MathUtil.SIMD_HALF_PI)
{
maxTh = MathUtil.SIMD_HALF_PI - step;
drawS = true;
}
if (minTh > maxTh)
{
minTh = -MathUtil.SIMD_HALF_PI + step;
maxTh = MathUtil.SIMD_HALF_PI - step;
drawN = drawS = true;
}
int n_hor = (int)((maxTh - minTh) / step) + 1;
if (n_hor < 2) n_hor = 2;
float step_h = (maxTh - minTh) / (n_hor - 1);
bool isClosed = false;
if (minPs > maxPs)
{
minPs = -MathUtil.SIMD_PI + step;
maxPs = MathUtil.SIMD_PI;
isClosed = true;
}
else if ((maxPs - minPs) >= MathUtil.SIMD_PI * 2f)
{
isClosed = true;
}
else
{
isClosed = false;
}
int n_vert = (int)((maxPs - minPs) / step) + 1;
if (n_vert < 2) n_vert = 2;
vA = new IndexedVector3[n_vert];
vB = new IndexedVector3[n_vert];
pvA = vA; pvB = vB;
float step_v = (maxPs - minPs) / (float)(n_vert - 1);
for (int i = 0; i < n_hor; i++)
{
float th = minTh + i * step_h;
float sth = radius * (float)Math.Sin(th);
float cth = radius * (float)Math.Cos(th);
for (int j = 0; j < n_vert; j++)
{
float psi = minPs + (float)j * step_v;
float sps = (float)Math.Sin(psi);
float cps = (float)Math.Cos(psi);
pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
if (i != 0)
{
DrawLine(pvA[j], pvB[j], color);
}
else if (drawS)
{
DrawLine(spole, pvB[j], color);
}
if (j != 0)
{
DrawLine(pvB[j - 1], pvB[j], color);
}
else
{
arcStart = pvB[j];
}
if ((i == (n_hor - 1)) && drawN)
{
DrawLine(npole, pvB[j], color);
}
if (isClosed)
{
if (j == (n_vert - 1))
{
DrawLine(arcStart, pvB[j], color);
}
}
else
{
if (((i == 0) || (i == (n_hor - 1))) && ((j == 0) || (j == (n_vert - 1))))
{
DrawLine(center, pvB[j], color);
}
}
}
pT = pvA; pvA = pvB; pvB = pT;
}
}
public virtual void UpdateFriction(float timeStep)
{
//calculate the impulse, so that the wheels don't move sidewards
int numWheel = GetNumWheels();
if (numWheel == 0)
{
return;
}
//m_forwardWS.resize(numWheel);
//m_axle.resize(numWheel);
//m_forwardImpulse.resize(numWheel);
//m_sideImpulse.resize(numWheel);
int numWheelsOnGround = 0;
//collapse all those loops into one!
for (int i = 0; i < numWheel; i++)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
numWheelsOnGround++;
}
m_sideImpulse[i] = 0f;
m_forwardImpulse[i] = 0f;
}
if (numWheelsOnGround != 4)
{
int ibreak = 0;
}
{
//foreach(WheelInfo wheelInfo in m_wheelInfo)
for (int i = 0; i < numWheel; ++i)
{
WheelInfo wheelInfo = m_wheelInfo[i];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
if (groundObject != null)
{
IndexedMatrix wheelTrans = GetWheelTransformWS(i);
IndexedBasisMatrix wheelBasis0 = wheelTrans._basis;
m_axle[i] = new IndexedVector3(
wheelBasis0._el0[m_indexRightAxis],
wheelBasis0._el1[m_indexRightAxis],
wheelBasis0._el2[m_indexRightAxis]);
IndexedVector3 surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS;
float proj = IndexedVector3.Dot(m_axle[i], surfNormalWS);
m_axle[i] -= surfNormalWS * proj;
m_axle[i].Normalize();
m_forwardWS[i] = IndexedVector3.Cross(surfNormalWS, m_axle[i]);
m_forwardWS[i].Normalize();
IndexedVector3 tempAxle = m_axle[i];
float tempImpulse = m_sideImpulse[i];
ContactConstraint.ResolveSingleBilateral(m_chassisBody, ref wheelInfo.m_raycastInfo.m_contactPointWS,
groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS,
0f, ref tempAxle, ref tempImpulse, timeStep);
m_sideImpulse[i] = (tempImpulse * sideFrictionStiffness2);
}
}
}
float sideFactor = 1f;
float fwdFactor = 0.5f;
bool sliding = false;
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
RigidBody groundObject = wheelInfo.m_raycastInfo.m_groundObject as RigidBody;
float rollingFriction = 0f;
if (groundObject != null)
{
if (wheelInfo.m_engineForce != 0.0f)
{
rollingFriction = wheelInfo.m_engineForce * timeStep;
}
else
{
float defaultRollingFrictionImpulse = 0f;
float maxImpulse = (wheelInfo.m_brake != 0f) ? wheelInfo.m_brake : defaultRollingFrictionImpulse;
IndexedVector3 tempWheel = m_forwardWS[wheel];
WheelContactPoint contactPt = new WheelContactPoint(m_chassisBody, groundObject, ref wheelInfo.m_raycastInfo.m_contactPointWS, ref tempWheel, maxImpulse);
m_forwardWS[wheel] = tempWheel;
rollingFriction = CalcRollingFriction(contactPt);
}
}
//switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
m_forwardImpulse[wheel] = 0f;
m_wheelInfo[wheel].m_skidInfo = 1f;
if (groundObject != null)
{
m_wheelInfo[wheel].m_skidInfo = 1f;
float maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip;
float maximpSide = maximp;
float maximpSquared = maximp * maximpSide;
m_forwardImpulse[wheel] = rollingFriction; //wheelInfo.m_engineForce* timeStep;
float x = (m_forwardImpulse[wheel]) * fwdFactor;
float y = (m_sideImpulse[wheel]) * sideFactor;
float impulseSquared = (x * x + y * y);
if (impulseSquared > maximpSquared)
{
sliding = true;
float factor = (float)(maximp / Math.Sqrt(impulseSquared));
m_wheelInfo[wheel].m_skidInfo *= factor;
}
}
}
}
if (sliding)
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
if (m_sideImpulse[wheel] != 0f)
{
if (m_wheelInfo[wheel].m_skidInfo < 1f)
{
m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo;
}
}
}
}
// apply the impulses
{
for (int wheel = 0; wheel < numWheel; wheel++)
{
WheelInfo wheelInfo = m_wheelInfo[wheel];
IndexedVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS -
m_chassisBody.GetCenterOfMassPosition();
if (m_forwardImpulse[wheel] > 5f || m_sideImpulse[wheel] > 5f)
{
int ibreak = 0;
}
if (m_forwardImpulse[wheel] != 0f)
{
m_chassisBody.ApplyImpulse(m_forwardWS[wheel] * (m_forwardImpulse[wheel]), rel_pos);
}
if (m_sideImpulse[wheel] != 0f)
{
RigidBody groundObject = m_wheelInfo[wheel].m_raycastInfo.m_groundObject as RigidBody;
IndexedVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS -
groundObject.GetCenterOfMassPosition();
IndexedVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel];
#if ROLLING_INFLUENCE_FIX // fix. It only worked if car's up was along Y - VT.
IndexedVector3 vChassisWorldUp = GetRigidBody().GetCenterOfMassTransform()._basis.GetColumn(m_indexUpAxis);
rel_pos -= vChassisWorldUp * (IndexedVector3.Dot(vChassisWorldUp, rel_pos) * (1.0f - wheelInfo.m_rollInfluence));
#else
rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence;
#endif
m_chassisBody.ApplyImpulse(ref sideImp, ref rel_pos);
//apply friction impulse on the ground
IndexedVector3 temp = -sideImp;
groundObject.ApplyImpulse(ref temp, ref rel_pos2);
}
}
}
}
public static void ClipFaceAgainstHull(ref IndexedVector3 separatingNormal, ConvexPolyhedron hullA, ref IndexedMatrix transA, ObjectArray <IndexedVector3> worldVertsB1, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
{
ObjectArray <IndexedVector3> worldVertsB2 = new ObjectArray <IndexedVector3>();
ObjectArray <IndexedVector3> pVtxIn = worldVertsB1;
ObjectArray <IndexedVector3> pVtxOut = worldVertsB2;
pVtxOut.Capacity = pVtxIn.Count;
int closestFaceA = -1;
{
float dmin = float.MaxValue;
for (int face = 0; face < hullA.m_faces.Count; face++)
{
IndexedVector3 Normal = new IndexedVector3(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]);
IndexedVector3 faceANormalWS = transA._basis * Normal;
float d = IndexedVector3.Dot(faceANormalWS, separatingNormal);
if (d < dmin)
{
dmin = d;
closestFaceA = face;
}
}
}
if (closestFaceA < 0)
{
return;
}
Face polyA = hullA.m_faces[closestFaceA];
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
int numContacts = pVtxIn.Count;
int numVerticesA = polyA.m_indices.Count;
for (int e0 = 0; e0 < numVerticesA; e0++)
{
IndexedVector3 a = hullA.m_vertices[polyA.m_indices[e0]];
IndexedVector3 b = hullA.m_vertices[polyA.m_indices[(e0 + 1) % numVerticesA]];
IndexedVector3 edge0 = a - b;
IndexedVector3 WorldEdge0 = transA._basis * edge0;
IndexedVector3 worldPlaneAnormal1 = transA._basis * new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
IndexedVector3 planeNormalWS1 = -WorldEdge0.Cross(worldPlaneAnormal1);//.cross(WorldEdge0);
IndexedVector3 worldA1 = transA * a;
float planeEqWS1 = -worldA1.Dot(planeNormalWS1);
//int otherFace=0;
#if BLA1
int otherFace = polyA.m_connectedFaces[e0];
btVector3 localPlaneNormal(hullA.m_faces[otherFace].m_plane[0], hullA.m_faces[otherFace].m_plane[1], hullA.m_faces[otherFace].m_plane[2]);
btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3];
btVector3 planeNormalWS = transA.getBasis() * localPlaneNormal;
btScalar planeEqWS = localPlaneEq - planeNormalWS.dot(transA.getOrigin());
#else
IndexedVector3 planeNormalWS = planeNormalWS1;
float planeEqWS = planeEqWS1;
#endif //clip face
ClipFace(pVtxIn, pVtxOut, ref planeNormalWS, planeEqWS);
//btSwap(pVtxIn,pVtxOut);
ObjectArray <IndexedVector3> temp = pVtxIn;
pVtxIn = pVtxOut;
pVtxOut = temp;
pVtxOut.Clear();
}
//#define ONLY_REPORT_DEEPEST_POINT
IndexedVector3 point;
// only keep points that are behind the witness face
{
IndexedVector3 localPlaneNormal = new IndexedVector3(polyA.m_plane[0], polyA.m_plane[1], polyA.m_plane[2]);
float localPlaneEq = polyA.m_plane[3];
IndexedVector3 planeNormalWS = transA._basis * localPlaneNormal;
float planeEqWS = localPlaneEq - IndexedVector3.Dot(planeNormalWS, transA._origin);
for (int i = 0; i < pVtxIn.Count; i++)
{
float depth = IndexedVector3.Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
if (depth <= minDist)
{
// printf("clamped: depth=%f to minDist=%f\n",depth,minDist);
depth = minDist;
}
if (depth <= maxDist && depth >= minDist)
{
IndexedVector3 point2 = pVtxIn[i];
#if ONLY_REPORT_DEEPEST_POINT
curMaxDist = depth;
#else
#if false
if (depth < -3)
{
printf("error in btPolyhedralContactClipping depth = %f\n", depth);
printf("likely wrong separatingNormal passed in\n");
}
#endif
resultOut.AddContactPoint(ref separatingNormal, ref point2, depth);
#endif
}
}
}
#if ONLY_REPORT_DEEPEST_POINT
if (curMaxDist < maxDist)
{
resultOut.AddContactPoint(ref separatingNormal, ref point, curMaxDist);
}
#endif //ONLY_REPORT_DEEPEST_POINT
}
/// sort cached points so most isolated points come first
private int SortCachedPoints(ManifoldPoint pt)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
#if KEEP_DEEPEST_POINT
float maxPenetration = pt.GetDistance();
for (int i = 0; i < m_pointCache.Length; i++)
{
if (m_pointCache[i].GetDistance() < maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = m_pointCache[i].GetDistance();
}
}
#endif //KEEP_DEEPEST_POINT
float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
if (maxPenetrationIndex != 0)
{
IndexedVector3 a0 = pt.GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 b0 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a0, b0);
res0 = cross.LengthSquared();
}
if (maxPenetrationIndex != 1)
{
IndexedVector3 a1 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b1 = m_pointCache[3].GetLocalPointA() - m_pointCache[2].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a1, b1);
res1 = cross.LengthSquared();
}
if (maxPenetrationIndex != 2)
{
IndexedVector3 a2 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b2 = m_pointCache[3].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a2, b2);
res2 = cross.LengthSquared();
}
if (maxPenetrationIndex != 3)
{
IndexedVector3 a3 = pt.GetLocalPointA() - m_pointCache[0].GetLocalPointA();
IndexedVector3 b3 = m_pointCache[2].GetLocalPointA() - m_pointCache[1].GetLocalPointA();
IndexedVector3 cross = IndexedVector3.Cross(a3, b3);
res3 = cross.LengthSquared();
}
IndexedVector4 maxvec = new IndexedVector4(res0, res1, res2, res3);
int biggestarea = MathUtil.ClosestAxis(ref maxvec);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugPersistentManifold)
{
BulletGlobals.g_streamWriter.WriteLine("sortCachedPoints [{0}]", biggestarea);
}
#endif
return(biggestarea);
}
public void DrawXNA(ref IndexedMatrix m, CollisionShape shape, ref IndexedVector3 color, DebugDrawModes debugMode, ref IndexedVector3 worldBoundsMin, ref IndexedVector3 worldBoundsMax, ref IndexedMatrix view, ref IndexedMatrix projection)
{
//btglMultMatrix(m);
if (shape == null)
{
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
UniformScalingShape scalingShape = (UniformScalingShape)shape;
ConvexShape convexShape = scalingShape.GetChildShape();
float scalingFactor = scalingShape.GetUniformScalingFactor();
IndexedMatrix scaleMatrix = IndexedMatrix.CreateScale(scalingFactor);
IndexedMatrix finalMatrix = scaleMatrix * m;
DrawXNA(ref finalMatrix, convexShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax, ref view, ref projection);
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
CompoundShape compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
IndexedMatrix childMat = childTrans;
//childMat = MathUtil.bulletMatrixMultiply(m, childMat);
//childMat = childMat * m;
childMat = m * childMat;
DrawXNA(ref childMat, colShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax, ref view, ref projection);
}
}
else
{
bool useWireframeFallback = true;
if ((debugMode & DebugDrawModes.DBG_DrawWireframe) == 0)
{
///you can comment out any of the specific cases, and use the default
///the benefit of 'default' is that it approximates the actual collision shape including collision margin
//BroadphaseNativeTypes shapetype = m_textureEnabled ? BroadphaseNativeTypes.MAX_BROADPHASE_COLLISION_TYPES : shape.getShapeType();
BroadphaseNativeTypes shapetype = shape.GetShapeType();
switch (shapetype)
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape boxShape = shape as BoxShape;
IndexedVector3 halfExtents = boxShape.GetHalfExtentsWithMargin();
DrawSolidCube(ref halfExtents, ref m, ref view, ref projection, ref color);
//drawSolidSphere(halfExtents.X, 10, 10, ref m, ref view, ref projection);
//drawCylinder(halfExtents.X, halfExtents.Y, 1, ref m, ref view, ref projection);
//drawSolidCone(halfExtents.Y, halfExtents.X, ref m, ref view, ref projection);
//DrawText("Hello World", new IndexedVector3(20, 20, 0), new IndexedVector3(255, 255, 255));
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.GetMargin();//radius doesn't include the margin, so draw with margin
DrawSolidSphere(radius, 10, 10, ref m, ref view, ref projection, ref color);
//glutSolidSphere(radius,10,10);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = shape as CapsuleShape;
float radius = capsuleShape.GetRadius();
float halfHeight = capsuleShape.GetHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
IndexedVector3 capStart = IndexedVector3.Zero;
capStart[upAxis] = -halfHeight;
IndexedVector3 capEnd = IndexedVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capStart;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capEnd;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection, ref color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
ConeShape coneShape = (ConeShape)(shape);
int upIndex = coneShape.GetConeUpIndex();
float radius = coneShape.GetRadius(); //+coneShape.getMargin();
float height = coneShape.GetHeight(); //+coneShape.getMargin();
IndexedMatrix rotateMatrix = IndexedMatrix.Identity;
switch (upIndex)
{
case 0:
rotateMatrix = IndexedMatrix.CreateRotationX(-MathUtil.SIMD_HALF_PI);
break;
case 1:
break;
case 2:
rotateMatrix = IndexedMatrix.CreateRotationX(MathUtil.SIMD_HALF_PI);
break;
default:
{
break;
}
}
;
IndexedMatrix translationMatrix = IndexedMatrix.CreateTranslation(0f, 0f, -0.5f * height);
IndexedMatrix resultant = m * rotateMatrix * translationMatrix;
DrawSolidCone(height, radius, ref resultant, ref view, ref projection, ref color);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = shape as StaticPlaneShape;
float planeConst = staticPlaneShape.GetPlaneConstant();
IndexedVector3 planeNormal = staticPlaneShape.GetPlaneNormal();
IndexedVector3 planeOrigin = planeNormal * planeConst;
IndexedVector3 vec0, vec1;
TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
IndexedVector3 pt0 = planeOrigin + vec0 * vecLen;
IndexedVector3 pt1 = planeOrigin - vec0 * vecLen;
IndexedVector3 pt2 = planeOrigin + vec1 * vecLen;
IndexedVector3 pt3 = planeOrigin - vec1 * vecLen;
// Fallback to debug draw - needs tidying
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawLine(ref pt0, ref pt1, ref colour);
DrawLine(ref pt1, ref pt2, ref colour);
DrawLine(ref pt2, ref pt3, ref colour);
DrawLine(ref pt3, ref pt1, ref colour);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylinder = (CylinderShape)(shape);
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = cylinder.GetHalfExtentsWithMargin()[upAxis];
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection, ref color);
break;
}
default:
{
if (shape.IsConvex())
{
ShapeCache sc = Cache(shape as ConvexShape);
//if (shape.getUserPointer())
{
//glutSolidCube(1.0);
ShapeHull hull = sc.m_shapehull /*(btShapeHull*)shape.getUserPointer()*/;
int numTriangles = hull.NumTriangles();
int numIndices = hull.NumIndices();
int numVertices = hull.NumVertices();
if (numTriangles > 0)
{
int index = 0;
IList <int> idx = hull.m_indices;
IList <IndexedVector3> vtx = hull.m_vertices;
for (int i = 0; i < numTriangles; i++)
{
int i1 = index++;
int i2 = index++;
int i3 = index++;
Debug.Assert(i1 < numIndices &&
i2 < numIndices &&
i3 < numIndices);
int index1 = idx[i1];
int index2 = idx[i2];
int index3 = idx[i3];
Debug.Assert(index1 < numVertices &&
index2 < numVertices &&
index3 < numVertices);
IndexedVector3 v1 = m * vtx[index1];
IndexedVector3 v2 = m * vtx[index2];
IndexedVector3 v3 = m * vtx[index3];
IndexedVector3 normal = IndexedVector3.Cross((v3 - v1), (v2 - v1));
normal.Normalize();
Vector2 tex = new Vector2(0, 0);
AddVertex(ref v1, ref normal, ref tex);
AddVertex(ref v2, ref normal, ref tex);
AddVertex(ref v3, ref normal, ref tex);
}
}
}
}
break;
}
}
}
/// for polyhedral shapes
if (debugMode == DebugDrawModes.DBG_DrawFeaturesText && (shape.IsPolyhedral()))
{
PolyhedralConvexShape polyshape = (PolyhedralConvexShape)shape;
{
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape.getExtraDebugInfo());
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
for (int i = 0; i < polyshape.GetNumVertices(); i++)
{
IndexedVector3 vtx;
polyshape.GetVertex(i, out vtx);
String buf = " " + i;
DrawText(buf, ref vtx, ref colour);
}
for (int i = 0; i < polyshape.GetNumPlanes(); i++)
{
IndexedVector3 normal;
IndexedVector3 vtx;
polyshape.GetPlane(out normal, out vtx, i);
float d = IndexedVector3.Dot(vtx, normal);
vtx *= d;
String buf = " plane " + i;
DrawText(buf, ref vtx, ref colour);
}
}
}
if (shape.IsConcave() && !shape.IsInfinite()) //>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape.getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
// if (shape.getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
ConcaveShape concaveMesh = shape as ConcaveShape;
XNADrawcallback drawCallback = new XNADrawcallback(this, ref m);
drawCallback.m_wireframe = (debugMode & DebugDrawModes.DBG_DrawWireframe) != 0;
concaveMesh.ProcessAllTriangles(drawCallback, ref worldBoundsMin, ref worldBoundsMax);
}
//glDisable(GL_DEPTH_TEST);
//glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
if ((debugMode & DebugDrawModes.DBG_DrawText) != 0)
{
IndexedVector3 position = IndexedVector3.Zero;
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawText(shape.GetName(), ref position, ref colour);
}
if ((debugMode & DebugDrawModes.DBG_DrawFeaturesText) != 0)
{
//drawText(shape.getEx]
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape.getExtraDebugInfo());
}
//glEnable(GL_DEPTH_TEST);
//// glPopMatrix();
//if(m_textureenabled) glDisable(GL_TEXTURE_2D);
// }
// glPopMatrix();
}
}