/// <summary>
/// デフォルトコンストラクタ
/// </summary>
/// <param name="rigid">MMD剛体データ</param>
/// <param name="Model">MMDモデルデータ</param>
public MMDMotionState(MMDRigid rigid, MMDModel Model)
{
UserData = null;
m_rigid = rigid;
//初期の姿勢を計算
Matrix startTransform;
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
startTransform = Model.BoneManager[rigid.RelatedBoneName].GlobalTransform;
else
{
if (Model.BoneManager.IndexOf("センター") >= 0)
startTransform = Model.BoneManager[Model.BoneManager.IndexOf("センター")].GlobalTransform;
else
startTransform = Matrix.Identity;
}
//ボーンと剛体とのズレを計算
Matrix RigidBias = CreateRigidBias(rigid);
//初期姿勢を計算
startTransform = RigidBias * startTransform * Model.Transform;
//初期の姿勢をMotionStateに設定
btTransform startTrans;
MMDXMath.TobtTransform(ref startTransform, out startTrans);
m_graphicsWorldTrans = startTrans;
m_centerOfMassOffset = btTransform.Identity;
m_startWorldTrans = startTrans;
//これからの計算ように確保
m_rigidBias = RigidBias;
RelatedBoneName = rigid.RelatedBoneName;
m_model = Model;
}
public void drawSphere(float radius,ref btTransform transform,ref btVector3 color)
{
btVector3 start = transform.Origin;
btVector3 xoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(radius, 0, 0));
btVector3 yoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, radius, 0));
btVector3 zoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, 0, radius));
{
btVector3 temp;
temp = new btVector3(radius, 0, 0);
btMatrix3x3.Multiply(ref transform.Basis, ref temp, out xoffs);
temp = new btVector3(0, radius, 0);
btMatrix3x3.Multiply(ref transform.Basis, ref temp, out yoffs);
temp = new btVector3(0, 0, radius);
btMatrix3x3.Multiply(ref transform.Basis, ref temp, out zoffs);
}
// XY
/*drawLine(start - xoffs, start + yoffs, color);
drawLine(start + yoffs, start + xoffs, color);
drawLine(start + xoffs, start - yoffs, color);
drawLine(start - yoffs, start - xoffs, color);
// XZ
drawLine(start - xoffs, start + zoffs, color);
drawLine(start + zoffs, start + xoffs, color);
drawLine(start + xoffs, start - zoffs, color);
drawLine(start - zoffs, start - xoffs, color);
// YZ
drawLine(start - yoffs, start + zoffs, color);
drawLine(start + zoffs, start + yoffs, color);
drawLine(start + yoffs, start - zoffs, color);
drawLine(start - zoffs, start - yoffs, color);*/
btVector3 sax, say, saz, smx, smy, smz;
btVector3.Add(ref start, ref xoffs, out sax);
btVector3.Add(ref start, ref yoffs, out say);
btVector3.Add(ref start, ref zoffs, out saz);
btVector3.Subtract(ref start, ref xoffs, out smx);
btVector3.Subtract(ref start, ref yoffs, out smy);
btVector3.Subtract(ref start, ref zoffs, out smz);
// XY
drawLine(ref smx, ref say, ref color);
drawLine(ref say, ref sax, ref color);
drawLine(ref sax, ref smy, ref color);
drawLine(ref smy, ref smx, ref color);
// XZ
drawLine(ref smx, ref saz, ref color);
drawLine(ref saz, ref sax, ref color);
drawLine(ref sax, ref smz, ref color);
drawLine(ref smz, ref smx, ref color);
// YZ
drawLine(ref smy, ref saz, ref color);
drawLine(ref saz, ref say, ref color);
drawLine(ref say, ref smz, ref color);
drawLine(ref smz, ref smy, ref color);
}
public bool calcPenDepth(ISimplexSolver simplexSolver, ConvexShape pConvexA, ConvexShape pConvexB, btTransform transformA, btTransform transformB, ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB, IDebugDraw debugDraw)
{
btVector3 guessVector;// = transformA.Origin - transformB.Origin;
btVector3.Subtract(ref transformA.Origin,ref transformB.Origin, out guessVector);
GjkEpaSolver2.sResults results = new GjkEpaSolver2.sResults();
if (GjkEpaSolver2.Penetration(pConvexA, transformA,
pConvexB, transformB,
guessVector, ref 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.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return true;
}
else
{
if (GjkEpaSolver2.Distance(pConvexA, transformA, pConvexB, transformB, guessVector, ref results))
{
wWitnessOnA = results.witnesses0;
wWitnessOnB = results.witnesses1;
v = results.normal;
return false;
}
}
wWitnessOnA = new btVector3();
wWitnessOnB = new btVector3();
return false;
}
public static void calculateVelocity(btTransform transform0,btTransform transform1,float timeStep,out btVector3 linVel, out btVector3 angVel)
{
linVel = (transform1.Origin - transform0.Origin) / timeStep;
btVector3 axis;
float angle;
calculateDiffAxisAngle(transform0,transform1,out axis,out angle);
angVel = axis * angle / timeStep;
}
public PerturbedContactResult(/*ManifoldResult* originalResult,*/ btTransform transformA, btTransform transformB, btTransform unPerturbedTransform, bool perturbA, IDebugDraw debugDrawer)
{
//m_originalManifoldResult = originalResult;
m_transformA = transformA;
m_transformB = transformB;
m_unPerturbedTransform = unPerturbedTransform;
m_perturbA = perturbA;
m_debugDrawer = debugDrawer;
}
protected float[] m_springDamping = new float[6]; // between 0 and 1 (1 == no damping)
public Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
{
for (int i = 0; i < 6; i++)
{
m_springEnabled[i] = false;
m_equilibriumPoint[i] = 0f;
m_springStiffness[i] = 0f;
m_springDamping[i] = 1f;
}
}
public ManifoldResult(CollisionObject body0, CollisionObject body1)
{
m_manifoldPtr = null;
m_body0 = body0;
m_body1 = body1;
m_rootTransA = body0.WorldTransform;
m_rootTransB = body1.WorldTransform;
m_partId0 = -1;
m_partId1 = -1;
m_index0 = -1;
m_index1 = -1;
}
public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results, bool usemargins)
{
tShape shape=new MinkowskiDiff();
Initialize(shape0,wtrs0,shape1,wtrs1,ref results,ref shape,usemargins);
using (GJK gjk = GJK.CreateFromPool())
{
GJK.eStatus gjk_status = gjk.Evaluate(shape, -guess);
switch (gjk_status)
{
case GJK.eStatus.Inside:
{
using(EPA epa = EPA.CreateFromPool())
{
EPA.eStatus epa_status = epa.Evaluate(gjk, -guess);
if (epa_status != EPA.eStatus.Failed)
{
btVector3 w0 = new btVector3(0, 0, 0);
for (U i = 0; i < epa.m_result.rank; ++i)
{
#region w0 += shape.Support(epa.m_result.c[i].d, 0) * epa.m_result.p[i];
{
btVector3 temp1, temp2;
shape.Support(ref epa.m_result.c[i].d, 0, out temp1);
btVector3.Multiply(ref temp1, epa.m_result.p[i], out temp2);
w0.Add(ref temp2);
}
#endregion
}
results.status = sResults.eStatus.Penetrating;
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return (true);
}
else results.status = sResults.eStatus.EPA_Failed;
}
}
break;
case GJK.eStatus.Failed:
results.status = sResults.eStatus.GJK_Failed;
break;
default:
{
break;
}
}
return (false);
}
}
//!@}
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, btTransform frameInA, btTransform frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
{
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits = new RotationalLimitMotor[3];
for (int i = 0; i < m_angularLimits.Length; i++)
m_angularLimits[i] = new RotationalLimitMotor();
m_frameInA = frameInA;
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameA;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
m_useSolveConstraintObsolete = D6_USE_OBSOLETE_METHOD;
calculateTransforms();
}
public Generic6DofConstraint(RigidBody rbB, btTransform frameInB, bool useLinearReferenceFrameB)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, getFixedBody(), rbB)
{
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits = new RotationalLimitMotor[3];
for (int i = 0; i < m_angularLimits.Length; i++)
m_angularLimits[i] = new RotationalLimitMotor();
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameB;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_flags = bt6DofFlags.BT_6DOF_FLAGS_NONE;
m_useSolveConstraintObsolete = false;
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.CenterOfMassTransform * m_frameInB;
calculateTransforms();
}
public static void integrateTransform(btTransform curTrans,btVector3 linvel, btVector3 angvel, float timeStep,out btTransform predictedTransform)
{
predictedTransform = btTransform.Identity;
#region predictedTransform.Origin=curTrans.Origin + linvel * timeStep;
{
btVector3 temp;
btVector3.Multiply(ref linvel, timeStep, out temp);
btVector3.Add(ref curTrans.Origin, ref temp, out predictedTransform.Origin);
}
#endregion
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
float fAngle = angvel.Length;
//limit the angular motion
if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if (fAngle < 0.001f)
{
// use Taylor's expansions of sync function
#region axis = angvel * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle);
{
btVector3.Multiply(ref angvel, (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle), out axis);
}
#endregion
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
#region axis = angvel * ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle);
btVector3.Multiply(ref angvel, ((float)Math.Sin(0.5f * fAngle * timeStep) / fAngle), out axis);
#endregion
}
btQuaternion dorn = new btQuaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(fAngle * timeStep * 0.5f));
btQuaternion orn0 = curTrans.Rotation;
btQuaternion predictedOrn;
btQuaternion.Multiply(ref dorn, ref orn0, out predictedOrn);
predictedOrn.normalize();
predictedTransform.Rotation = predictedOrn;
}
public static bool Distance(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results)
{
tShape shape = new MinkowskiDiff();
Initialize(shape0, wtrs0, shape1, wtrs1, ref results, ref shape, false);
using(GJK gjk = GJK.CreateFromPool())
{
GJK.eStatus gjk_status = gjk.Evaluate(shape, guess);
if (gjk_status == GJK.eStatus.Valid)
{
btVector3 w0 = new btVector3(0, 0, 0);
btVector3 w1 = new btVector3(0, 0, 0);
for (U i = 0; i < gjk.m_simplex.rank; ++i)
{
float p = gjk.m_simplex.p[i];
btVector3 temp,temp2,temp3;
#region w0 += shape.Support(gjk.m_simplex.c[i].d, 0) * p;
shape.Support(ref gjk.m_simplex.c[i].d, 0, out temp);
btVector3.Multiply(ref temp, p, out temp2);
w0.Add(ref temp2);
#endregion
#region w1 += shape.Support(-gjk.m_simplex.c[i].d, 1) * p;
btVector3.Minus(ref gjk.m_simplex.c[i].d, out temp3);
shape.Support(ref temp3, 1, out temp);
btVector3.Multiply(ref temp, p, out temp2);
w1.Add(ref temp2);
#endregion
}
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * w1;
results.normal = w0 - w1;
results.distance = results.normal.Length;
results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
return (true);
}
else
{
results.status = gjk_status == GJK.eStatus.Inside ?
sResults.eStatus.Penetrating :
sResults.eStatus.GJK_Failed;
return (false);
}
}
}
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
public void calculateTemporalAabb(btTransform curTrans, btVector3 linvel, btVector3 angvel, float timeStep, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax)
{
//start with static aabb
getAabb(curTrans, out temporalAabbMin, out temporalAabbMax);
float temporalAabbMaxx = temporalAabbMax.X;
float temporalAabbMaxy = temporalAabbMax.Y;
float temporalAabbMaxz = temporalAabbMax.Z;
float temporalAabbMinx = temporalAabbMin.X;
float temporalAabbMiny = temporalAabbMin.Y;
float temporalAabbMinz = temporalAabbMin.Z;
// add linear motion
btVector3 linMotion;// = linvel * timeStep;
btVector3.Multiply(ref linvel, timeStep, out linMotion);
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.X > 0f)
temporalAabbMaxx += linMotion.X;
else
temporalAabbMinx += linMotion.X;
if (linMotion.Y > 0f)
temporalAabbMaxy += linMotion.Y;
else
temporalAabbMiny += linMotion.Y;
if (linMotion.Z > 0f)
temporalAabbMaxz += linMotion.Z;
else
temporalAabbMinz += linMotion.Z;
//add conservative angular motion
float angularMotion = angvel.Length * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d = new btVector3(angularMotion, angularMotion, angularMotion);
temporalAabbMin = new btVector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
temporalAabbMax = new btVector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
//temporalAabbMin -= angularMotion3d;
//temporalAabbMax += angularMotion3d;
temporalAabbMin.Subtract(ref angularMotion3d);
temporalAabbMax.Add(ref angularMotion3d);
}
public static void calculateDiffAxisAngle(btTransform transform0,btTransform transform1,out btVector3 axis,out float angle)
{
btMatrix3x3 dmat;// = transform1.Basis * transform0.Basis.inverse();
{
btMatrix3x3 temp;
transform0.Basis.inverse(out temp);
btMatrix3x3.Multiply(ref transform1.Basis, ref temp, out dmat);
}
btQuaternion dorn;
dmat.getRotation(out dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis = new btVector3(dorn.X, dorn.Y, dorn.Z);
axis.W = 0f;
//check for axis length
float len = axis.Length2;
if (len < BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON)
axis = new btVector3(1f, 0f, 0f);
else
axis /= (float)Math.Sqrt(len);
}
/// <summary>
/// synchronizes world transform from user to physics
/// </summary>
public virtual void getWorldTransform(out btTransform centerOfMassWorldTrans)
{
centerOfMassWorldTrans = m_centerOfMassOffset.inverse() * m_graphicsWorldTrans;
}
//upcastは要らない……
/// continuous collision detection needs prediction
public void predictIntegratedTransform(float timeStep, out btTransform predictedTransform)
{
TransformUtil.integrateTransform(WorldTransform, m_linearVelocity, m_angularVelocity, timeStep, out predictedTransform);
}
public void proceedToTransform(btTransform newTrans)
{
CenterOfMassTransform = newTrans;
}
protected int setAngularLimits(ConstraintInfo2 info, int row_offset, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB)
{
Generic6DofConstraint d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i = 0; i < 3; i++)
{
if (d6constraint.getRotationalLimitMotor(i).needApplyTorques())
{
btVector3 axis = d6constraint.getAxis(i);
bt6DofFlags flags = (bt6DofFlags)((int)m_flags >> ((i + 3) * (int)bt6DofFlags.BT_6DOF_FLAGS_AXIS_SHIFT));
if ((flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) == 0)
{
m_angularLimits[i].m_normalCFM = info.Constraints[info.CurrentRow].m_cfm;
}
if ((flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) == 0)
{
m_angularLimits[i].m_stopCFM = info.Constraints[info.CurrentRow].m_cfm;
}
if ((flags & bt6DofFlags.BT_6DOF_FLAGS_ERP_STOP) == 0)
{
m_angularLimits[i].m_stopERP = info.erp;
}
row += get_limit_motor_info2(d6constraint.getRotationalLimitMotor(i),
transA, transB, linVelA, linVelB, angVelA, angVelB, info, row,ref axis, true);
}
}
return row;
}
public static bool Penetration(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, btVector3 guess, ref sResults results)
{
return Penetration(shape0, wtrs0, shape1, wtrs1, guess, ref results, true);
}
public int get_limit_motor_info2(
RotationalLimitMotor limot,
btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB,
ConstraintInfo2 info, int row, ref btVector3 ax1, bool rotational)
{
return get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref ax1, rotational, false);
}
static void Initialize(ConvexShape shape0, btTransform wtrs0,
ConvexShape shape1, btTransform wtrs1,
ref GjkEpaSolver2.sResults results,
ref tShape shape,
bool withmargins)
{
/* Results */
results.witnesses0 =
results.witnesses1 = new btVector3(0, 0, 0);
results.status = GjkEpaSolver2.sResults.eStatus.Separated;
/* Shape */
shape.m_shapes0 = shape0;
shape.m_shapes1 = shape1;
//shape.m_toshape1 = wtrs1.Basis.transposeTimes(wtrs0.Basis);
wtrs1.Basis.transposeTimes(ref wtrs0.Basis, out shape.m_toshape1);
shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
shape.EnableMargin(withmargins);
}
public override void getAabb(btTransform t, out btVector3 aabbMin, out btVector3 aabbMax)
{
aabbMin = new btVector3(-BulletGlobal.BT_LARGE_FLOAT, -BulletGlobal.BT_LARGE_FLOAT, -BulletGlobal.BT_LARGE_FLOAT);
aabbMax = new btVector3(BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT);
}
public int get_limit_motor_info2(
RotationalLimitMotor limot,
btTransform transA,btTransform transB,btVector3 linVelA,btVector3 linVelB,btVector3 angVelA,btVector3 angVelB,
ConstraintInfo2 info, int row,ref btVector3 ax1, bool rotational,bool rotAllowed)
{
//ポインタを使わず、Listを直接受け取っているので、srow=rowとして、Listに入れるように修正
//int srow = row * info.rowskip;
int srow = row;
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if (powered || limit!=0)
{ // if the joint is powered, or has joint limits, add in the extra row
//rotationalでポインタ分けをしていたのを修正。
#if false
float *J1 = rotational ? info.m_J1angularAxis : info.m_J1linearAxis;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
#endif
if (rotational)
info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = ax1;
else
info.Constraints[srow + info.CurrentRow].m_contactNormal = ax1;
//btScalar* J2 = rotational ? info->m_J2angularAxis : 0;
if (rotational)
{
#if false
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
#endif
info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = -ax1;
}
if((!rotational))
{
if (m_useOffsetForConstraintFrame)
{
btVector3 tmpA, tmpB, relA, relB;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.Origin - transB.Origin;
// get its projection to constraint axis
btVector3 projB = ax1 * relB.dot(ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
btVector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.Origin - transA.Origin;
btVector3 projA = ax1 * relA.dot(ax1);
btVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
float desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
btVector3 totalDist;// = projA + ax1 * desiredOffs - projB;
{
btVector3 temp1, temp2;
btVector3.Multiply(ref ax1, desiredOffs, out temp1);
btVector3.Add(ref projA, ref temp1, out temp2);
btVector3.Subtract(ref temp2, ref projB, out totalDist);
}
// get offset vectors relA and relB
#region relA = orthoA + totalDist * m_factA;
{
btVector3 temp;
btVector3.Multiply(ref totalDist, m_factA, out temp);
btVector3.Add(ref orthoA, ref temp, out relA);
}
#endregion
#region relB = orthoB - totalDist * m_factB;
{
btVector3 temp;
btVector3.Multiply(ref totalDist, m_factB, out temp);
btVector3.Subtract(ref orthoB, ref temp, out relB);
}
#endregion
//tmpA = relA.cross(ax1);
relA.cross(ref ax1, out tmpA);
//tmpB = relB.cross(ax1);
relB.cross(ref ax1, out tmpB);
if (m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
//int i;
//for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
//for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = tmpA;
info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = -tmpB;
}
else
{
btVector3 ltd; // Linear Torque Decoupling vector
btVector3 c = m_calculatedTransformB.Origin - transA.Origin;
ltd = c.cross(ax1);
#if false
info->m_J1angularAxis[srow+0] = ltd[0];
info->m_J1angularAxis[srow+1] = ltd[1];
info->m_J1angularAxis[srow+2] = ltd[2];
#endif
info.Constraints[srow + info.CurrentRow].m_relpos1CrossNormal = ltd;
c = m_calculatedTransformB.Origin - transB.Origin;
ltd = -c.cross(ax1);
#if false
info->m_J2angularAxis[srow+0] = ltd[0];
info->m_J2angularAxis[srow+1] = ltd[1];
info->m_J2angularAxis[srow+2] = ltd[2];
#endif
info.Constraints[srow + info.CurrentRow].m_relpos2CrossNormal = ltd;
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit!=0 && (limot.m_loLimit == limot.m_hiLimit)) powered = false;
//info->m_constraintError[srow] = 0f;
info.Constraints[srow + info.CurrentRow].m_rhs = 0f;
if (powered)
{
//info->cfm[srow] = limot.m_normalCFM;
info.Constraints[srow + info.CurrentRow].m_cfm = limot.m_normalCFM;
if(limit==0)
{
float tag_vel = rotational ? limot.m_targetVelocity : -limot.m_targetVelocity;
float mot_fact = getMotorFactor( limot.m_currentPosition,
limot.m_loLimit,
limot.m_hiLimit,
tag_vel,
info.fps * limot.m_stopERP);
#if false
info->m_constraintError[srow] += mot_fact * limot.m_targetVelocity;
info->m_lowerLimit[srow] = -limot.m_maxMotorForce;
info->m_upperLimit[srow] = limot.m_maxMotorForce;
#endif
info.Constraints[srow + info.CurrentRow].m_rhs+=mot_fact * limot.m_targetVelocity;
info.Constraints[srow + info.CurrentRow].m_lowerLimit = -limot.m_maxMotorForce;
info.Constraints[srow + info.CurrentRow].m_upperLimit = limot.m_maxMotorForce;
}
}
if(limit!=0)
{
float k = info.fps * limot.m_stopERP;
if(!rotational)
{
//info->m_constraintError[srow] += k * limot.m_currentLimitError;
info.Constraints[srow + info.CurrentRow].m_rhs += k * limot.m_currentLimitError;
}
else
{
//info->m_constraintError[srow] += -k * limot.m_currentLimitError;
info.Constraints[srow + info.CurrentRow].m_rhs += -k * limot.m_currentLimitError;
}
//info->cfm[srow] = limot.m_stopCFM;
info.Constraints[srow + info.CurrentRow].m_cfm = limot.m_stopCFM;
if (limot.m_loLimit == limot.m_hiLimit)
{ // limited low and high simultaneously
//info->m_lowerLimit[srow] = float.NegativeInfinity;
//info->m_upperLimit[srow] = float.PositiveInfinity;
info.Constraints[srow + info.CurrentRow].m_lowerLimit = float.NegativeInfinity;
info.Constraints[srow + info.CurrentRow].m_upperLimit = float.PositiveInfinity;
}
else
{
if (limit == 1)
{
//info->m_lowerLimit[srow] = 0;
//info->m_upperLimit[srow] = float.PositiveInfinity;
info.Constraints[srow + info.CurrentRow].m_lowerLimit = 0;
info.Constraints[srow + info.CurrentRow].m_upperLimit = float.PositiveInfinity;
}
else
{
//info->m_lowerLimit[srow] = float.NegativeInfinity;
//info->m_upperLimit[srow] = 0;
info.Constraints[srow + info.CurrentRow].m_lowerLimit = float.NegativeInfinity;
info.Constraints[srow + info.CurrentRow].m_upperLimit = 0;
}
// deal with bounce
if (limot.m_bounce > 0)
{
// calculate joint velocity
float vel;
if (rotational)
{
vel = angVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= angVelB.dot(ax1);
}
else
{
vel = linVelA.dot(ax1);
//make sure that if no body -> angVelB == zero vec
// if (body1)
vel -= linVelB.dot(ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
float newc = -limot.m_bounce* vel;
//if (newc > info->m_constraintError[srow])
// info->m_constraintError[srow] = newc;
if (newc > info.Constraints[srow + info.CurrentRow].m_rhs)
info.Constraints[srow + info.CurrentRow].m_rhs = newc;
}
}
else
{
if (vel > 0)
{
float newc = -limot.m_bounce * vel;
//if (newc < info->m_constraintError[srow])
// info->m_constraintError[srow] = newc;
if (newc < info.Constraints[srow + info.CurrentRow].m_rhs)
info.Constraints[srow + info.CurrentRow].m_rhs = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
static float capsuleCapsuleDistance(
out btVector3 normalOnB,
out btVector3 pointOnB,
float capsuleLengthA,
float capsuleRadiusA,
float capsuleLengthB,
float capsuleRadiusB,
int capsuleAxisA,
int capsuleAxisB,
btTransform transformA,
btTransform transformB,
float distanceThreshold)
{
btVector3 directionA;// = transformA.Basis.getColumn(capsuleAxisA);
transformA.Basis.getColumn(capsuleAxisA, out directionA);
btVector3 translationA = transformA.Origin;
btVector3 directionB;// = transformB.Basis.getColumn(capsuleAxisB);
transformB.Basis.getColumn(capsuleAxisB, out directionB);
btVector3 translationB = transformB.Origin;
// translation between centers
btVector3 translation = translationB - translationA;
// compute the closest points of the capsule line segments
btVector3 ptsVector; // the vector between the closest points
btVector3 offsetA, offsetB; // offsets from segment centers to their closest points
float tA, tB; // parameters on line segment
segmentsClosestPoints(out ptsVector, out offsetA, out offsetB, out tA, out tB, ref translation,
ref directionA, capsuleLengthA, ref directionB, capsuleLengthB);
float distance = ptsVector.Length - capsuleRadiusA - capsuleRadiusB;
if (distance > distanceThreshold)
{
normalOnB = btVector3.Zero;
pointOnB = btVector3.Zero;
return distance;
}
float lenSqr = ptsVector.Length2;
if (lenSqr <= (BulletGlobal.SIMD_EPSILON * BulletGlobal.SIMD_EPSILON))
{
//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
btVector3 q;
btVector3.PlaneSpace1(ref directionA, out normalOnB, out q);
}
else
{
// compute the contact normal
//normalOnB = ptsVector * (-1.0f / (float)Math.Sqrt(lenSqr));
btVector3.Multiply(ref ptsVector, (-1.0f / (float)Math.Sqrt(lenSqr)), out normalOnB);
}
#region pointOnB = transformB.Origin + offsetB + normalOnB * capsuleRadiusB;
{
btVector3 temp1, temp2;
btVector3.Add(ref transformB.Origin, ref offsetB, out temp1);
btVector3.Multiply(ref normalOnB, capsuleRadiusB, out temp2);
btVector3.Add(ref temp1, ref temp2, out pointOnB);
}
#endregion
return distance;
}
public void getAabbNonVirtual(btTransform t, out btVector3 aabbMin, out btVector3 aabbMax)
{
switch (ShapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = (SphereShape)this;
float radius = sphereShape.ImplicitShapeDimensions.X;// * convexShape->getLocalScaling().getX();
float margin = radius + sphereShape.MarginNonVirtual;
btVector3 center = t.Origin;
btVector3 extent = new btVector3(margin, margin, margin);
//aabbMin = center - extent;
btVector3.Subtract(ref center, ref extent, out aabbMin);
//aabbMax = center + extent;
btVector3.Add(ref center, ref extent, out aabbMax);
}
break;
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
/* fall through */
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape convexShape = (BoxShape)this;
float margin = convexShape.MarginNonVirtual;
btVector3 halfExtents = convexShape.ImplicitShapeDimensions;
#region halfExtents += new btVector3(margin, margin, margin);
{
btVector3 temp = new btVector3(margin, margin, margin);
halfExtents.Add(ref temp);
}
#endregion
btMatrix3x3 abs_b;// = t.Basis.absolute();
t.Basis.absolute(out abs_b);
btVector3 center = t.Origin;
btVector3 extent = new btVector3(abs_b.el0.dot(halfExtents), abs_b.el1.dot(halfExtents), abs_b.el2.dot(halfExtents));
//aabbMin = center - extent;
btVector3.Subtract(ref center, ref extent, out aabbMin);
//aabbMax = center + extent;
btVector3.Add(ref center, ref extent, out aabbMax);
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
throw new NotImplementedException();
#if false
{
btTriangleShape* triangleShape = (btTriangleShape*)this;
float margin = triangleShape->getMarginNonVirtual();
for (int i=0;i<3;i++)
{
btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.));
vec[i] = btScalar(1.);
btVector3 sv = localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis());
btVector3 tmp = t(sv);
aabbMax[i] = tmp[i]+margin;
vec[i] = btScalar(-1.);
tmp = t(localGetSupportVertexWithoutMarginNonVirtual(vec*t.getBasis()));
aabbMin[i] = tmp[i]-margin;
}
}
break;
#endif
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = (CapsuleShape)this;
btVector3 halfExtents = new btVector3(capsuleShape.Radius, capsuleShape.Radius, capsuleShape.Radius);
int m_upAxis = capsuleShape.UpAxis;
halfExtents[m_upAxis] = capsuleShape.Radius + capsuleShape.HalfHeight;
#region halfExtents += new btVector3(capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual);
{
btVector3 temp = new btVector3(capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual, capsuleShape.MarginNonVirtual);
halfExtents.Add(ref temp);
}
#endregion
btMatrix3x3 abs_b;// = t.Basis.absolute();
t.Basis.absolute(out abs_b);
btVector3 center = t.Origin;
btVector3 extent = new btVector3(abs_b.el0.dot(halfExtents), abs_b.el1.dot(halfExtents), abs_b.el2.dot(halfExtents));
//aabbMin = center - extent;
btVector3.Subtract(ref center, ref extent, out aabbMin);
//aabbMax = center + extent;
btVector3.Add(ref center, ref extent, out aabbMax);
}
break;
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
throw new NotImplementedException();
#if false
{
btPolyhedralConvexAabbCachingShape* convexHullShape = (btPolyhedralConvexAabbCachingShape*)this;
btScalar margin = convexHullShape->getMarginNonVirtual();
convexHullShape->getNonvirtualAabb (t, aabbMin, aabbMax, margin);
}
break;
#endif
default:
this.getAabb(t, out aabbMin, out aabbMax);
Debug.Assert(false);
break;
}
// should never reach here
Debug.Assert(false);
}
protected int setLinearLimits(ConstraintInfo2 info, int row, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB)
{
// int row = 0;
//solve linear limits
limot.Reset();
for (int i = 0; i < 3; i++)
{
if (m_linearLimits.needApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = 0f;
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = 0f;
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
btVector3 axis;// = m_calculatedTransformA.Basis.getColumn(i);
m_calculatedTransformA.Basis.getColumn(i, out axis);
bt6DofFlags flags = (bt6DofFlags)((int)m_flags >> (i * (int)bt6DofFlags.BT_6DOF_FLAGS_AXIS_SHIFT));
//limot.m_normalCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0 ? m_linearLimits.m_normalCFM[i] : info.cfm[0];
//limot.m_stopCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0 ? m_linearLimits.m_stopCFM[i] : info.cfm[0];
limot.m_normalCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0 ? m_linearLimits.m_normalCFM[i] : info.Constraints[info.CurrentRow].m_cfm;
limot.m_stopCFM = (flags & bt6DofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0 ? m_linearLimits.m_stopCFM[i] : info.Constraints[info.CurrentRow].m_cfm;
limot.m_stopERP = (flags & bt6DofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0 ? m_linearLimits.m_stopERP[i] : info.erp;
if (m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
bool rotAllowed = true; // rotations around orthos to current axis
if (m_angularLimits[indx1].m_currentLimit != 0 && m_angularLimits[indx2].m_currentLimit != 0)
{
rotAllowed = false;
}
row += get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref axis, false, rotAllowed);
}
else
{
row += get_limit_motor_info2(limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, ref axis, false);
}
}
}
return row;
}
/// <summary>
/// synchronizes world transform from physics to user
/// Bullet only calls the update of worldtransform for active objects
/// </summary>
/// <param name="centerOfMassWorldTrans"></param>
public virtual void setWorldTransform(btTransform centerOfMassWorldTrans)
{
m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
}
public virtual void debugDrawObject(ref btTransform worldTransform, CollisionShape shape,ref btVector3 color)
{
// Draw a small simplex at the center of the object
{
btVector3 start = worldTransform.Origin;
btVector3 temp1, temp2, temp3, temp4;
//DebugDrawer.drawLine(start, start + btMatrix3x3.Multiply(worldTransform.Basis, new btVector3(1, 0, 0)), new btVector3(1, 0, 0));
//DebugDrawer.drawLine(start, start + btMatrix3x3.Multiply(worldTransform.Basis, new btVector3(0, 1, 0)), new btVector3(0, 1, 0));
//DebugDrawer.drawLine(start, start + btMatrix3x3.Multiply(worldTransform.Basis, new btVector3(0, 0, 1)), new btVector3(0, 0, 1));
temp1 = new btVector3(1, 0, 0);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref temp1, out temp2);
btVector3.Add(ref start, ref temp2, out temp3);
temp4 = new btVector3(1, 0, 0);
DebugDrawer.drawLine(ref start,ref temp3,ref temp4);
temp1 = new btVector3(0, 1, 0);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref temp1, out temp2);
btVector3.Add(ref start, ref temp2, out temp3);
temp4 = new btVector3(0, 1, 0);
DebugDrawer.drawLine(ref start,ref temp3,ref temp4);
temp1 = new btVector3(0, 0, 1);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref temp1, out temp2);
btVector3.Add(ref start, ref temp2, out temp3);
temp4 = new btVector3(0, 0, 1);
DebugDrawer.drawLine(ref start,ref temp3,ref temp4);
}
if (shape.ShapeType == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
throw new NotImplementedException();
#if false//未実装
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
debugDrawObject(worldTransform*childTrans,colShape,color);
}
#endif
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape boxShape = (BoxShape)(shape);
btVector3 halfExtents = boxShape.HalfExtentsWithMargin;
btVector3 temp;
btVector3.Minus(ref halfExtents, out temp);
DebugDrawer.drawBox(ref temp, ref halfExtents,ref worldTransform,ref color);
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = (SphereShape)(shape);
float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
DebugDrawer.drawSphere(radius,ref worldTransform,ref color);
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
throw new NotImplementedException();
#if false
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
DebugDrawer.drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color);
}
break;
#endif
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = (CapsuleShape)(shape);
float radius = capsuleShape.Radius;
float halfHeight = capsuleShape.HalfHeight;
int upAxis = capsuleShape.UpAxis;
btVector3 capStart = btVector3.Zero;
capStart[upAxis] = -halfHeight;
btVector3 capEnd = btVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
btTransform childTransform = worldTransform;
childTransform.Origin = worldTransform * capStart;
DebugDrawer.drawSphere(radius,ref childTransform,ref color);
}
{
btTransform childTransform = worldTransform;
childTransform.Origin = worldTransform * capEnd;
DebugDrawer.drawSphere(radius, ref childTransform,ref color);
}
// Draw some additional lines
btVector3 start = worldTransform.Origin;
capStart[(upAxis + 1) % 3] = radius;
capEnd[(upAxis + 1) % 3] = radius;
#region DebugDrawer.drawLine(start + btMatrix3x3.Multiply( worldTransform.Basis , capStart), start + btMatrix3x3.Multiply(worldTransform.Basis , capEnd), color);
{
btVector3 capStart2, capEnd2, start2,end2;
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capStart, out capStart2);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capEnd, out capEnd2);
btVector3.Add(ref start, ref capStart2, out start2);
btVector3.Add(ref start, ref capEnd2, out end2);
DebugDrawer.drawLine(ref start2,ref end2,ref color);
}
#endregion
capStart[(upAxis + 1) % 3] = -radius;
capEnd[(upAxis + 1) % 3] = -radius;
#region DebugDrawer.drawLine(start + btMatrix3x3.Multiply(worldTransform.Basis, capStart), start + btMatrix3x3.Multiply(worldTransform.Basis, capEnd), color);
{
btVector3 capStart2, capEnd2,start2,end2;
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capStart, out capStart2);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capEnd, out capEnd2);
btVector3.Add(ref start, ref capStart2, out start2);
btVector3.Add(ref start, ref capEnd2, out end2);
DebugDrawer.drawLine(ref start2, ref end2, ref color);
}
#endregion
capStart[(upAxis + 1) % 3] = 0f;
capEnd[(upAxis + 1) % 3] = 0f;
capStart[(upAxis + 2) % 3] = radius;
capEnd[(upAxis + 2) % 3] = radius;
#region DebugDrawer.drawLine(start + btMatrix3x3.Multiply(worldTransform.Basis, capStart), start + btMatrix3x3.Multiply(worldTransform.Basis, capEnd), color);
{
btVector3 capStart2, capEnd2, start2, end2;
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capStart, out capStart2);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capEnd, out capEnd2);
btVector3.Add(ref start, ref capStart2, out start2);
btVector3.Add(ref start, ref capEnd2, out end2);
DebugDrawer.drawLine(ref start2, ref end2, ref color);
}
#endregion
capStart[(upAxis + 2) % 3] = -radius;
capEnd[(upAxis + 2) % 3] = -radius;
#region DebugDrawer.drawLine(start + btMatrix3x3.Multiply(worldTransform.Basis, capStart), start + btMatrix3x3.Multiply(worldTransform.Basis, capEnd), color);
{
btVector3 capStart2, capEnd2, start2, end2;
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capStart, out capStart2);
btMatrix3x3.Multiply(ref worldTransform.Basis, ref capEnd, out capEnd2);
btVector3.Add(ref start, ref capStart2, out start2);
btVector3.Add(ref start, ref capEnd2, out end2);
DebugDrawer.drawLine(ref start2, ref end2, ref color);
}
#endregion
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
throw new NotImplementedException();
#if false
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
btScalar height = coneShape->getHeight();//+coneShape->getMargin();
btVector3 start = worldTransform.Origin;
int upAxis= coneShape->getConeUpIndex();
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = height * btScalar(0.5);
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight),start+worldTransform.Basis * (-offsetHeight+offsetRadius),color);
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight),start+worldTransform.Basis * (-offsetHeight-offsetRadius),color);
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight),start+worldTransform.Basis * (-offsetHeight+offset2Radius),color);
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight),start+worldTransform.Basis * (-offsetHeight-offset2Radius),color);
break;
#endif
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
throw new NotImplementedException();
#if false
const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
int upAxis = cylinder->getUpAxis();
btScalar radius = cylinder->getRadius();
btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
btVector3 start = worldTransform.Origin;
btVector3 offsetHeight(0,0,0);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight+offsetRadius),start+worldTransform.Basis * (-offsetHeight+offsetRadius),color);
DebugDrawer.drawLine(start+worldTransform.Basis * (offsetHeight-offsetRadius),start+worldTransform.Basis * (-offsetHeight-offsetRadius),color);
break;
#endif
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = (StaticPlaneShape)(shape);
float planeConst = staticPlaneShape.PlaneConstant;
btVector3 planeNormal = staticPlaneShape.PlaneNormal;
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0, vec1;
btVector3.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
btVector3 pt0;// = planeOrigin + vec0 * vecLen;
{
btVector3 temp;
btVector3.Multiply(ref vec0, vecLen, out temp);
btVector3.Add(ref planeOrigin, ref temp, out pt0);
}
btVector3 pt1;// = planeOrigin - vec0 * vecLen;
{
btVector3 temp;
btVector3.Multiply(ref vec0, vecLen, out temp);
btVector3.Subtract(ref planeOrigin, ref temp, out pt1);
}
btVector3 pt2;// = planeOrigin + vec1 * vecLen;
{
btVector3 temp;
btVector3.Multiply(ref vec1, vecLen, out temp);
btVector3.Add(ref planeOrigin, ref temp, out pt2);
}
btVector3 pt3 = planeOrigin - vec1 * vecLen;
#region DebugDrawer.drawLine(worldTransform * pt0, worldTransform * pt1, color);
{
btVector3 temp1, temp2;
btTransform.Multiply(ref worldTransform, ref pt0, out temp1);
btTransform.Multiply(ref worldTransform, ref pt1, out temp2);
DebugDrawer.drawLine(ref temp1, ref temp2, ref color);
}
#endregion
#region DebugDrawer.drawLine(worldTransform * pt2, worldTransform * pt3, color);
{
btVector3 temp1, temp2;
btTransform.Multiply(ref worldTransform, ref pt2, out temp1);
btTransform.Multiply(ref worldTransform, ref pt3, out temp2);
DebugDrawer.drawLine(ref temp1, ref temp2, ref color);
}
#endregion
break;
}
default:
{
if (shape.isConcave)
{
throw new NotImplementedException();
#if false
ConcaveShape concaveMesh = (ConcaveShape) shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
btVector3 aabbMax=new btVector3(BulletGlobal.BT_LARGE_FLOAT,BulletGlobal.BT_LARGE_FLOAT,BulletGlobal.BT_LARGE_FLOAT);
btVector3 aabbMin=new btVector3(-BulletGlobal.BT_LARGE_FLOAT,-BulletGlobal.BT_LARGE_FLOAT,-BulletGlobal.BT_LARGE_FLOAT);
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
#endif
}
if (shape.ShapeType == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
throw new NotImplementedException();
#if false
btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
//todo: pass camera for some culling
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT));
//DebugDrawcallback drawCallback;
DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
#endif
}
/// for polyhedral shapes
if (shape.isPolyhedral)
{
throw new NotImplementedException();
#if false
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
int i;
for (i=0;i<polyshape->getNumEdges();i++)
{
btVector3 a,b;
polyshape->getEdge(i,a,b);
btVector3 wa = worldTransform * a;
btVector3 wb = worldTransform * b;
DebugDrawer.drawLine(wa,wb,color);
}
#endif
}
break;
}
}
}
}
///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. public abstract void getAabb(btTransform t, out btVector3 aabbMin, out btVector3 aabbMax);
void getInfo2NonVirtual(ConstraintInfo2 info, btTransform transA, btTransform transB, btVector3 linVelA, btVector3 linVelB, btVector3 angVelA, btVector3 angVelB)
{
Debug.Assert(!m_useSolveConstraintObsolete);
//prepare constraint
calculateTransforms(transA, transB);
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
else
{ // leave old version for compatibility
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
