public void SetPivotB(ref IndexedVector3 pivotB)
{
m_pivotInB = pivotB;
}
public override void CalculateLocalInertia(float mass, out IndexedVector3 inertia)
{
//Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
//#define USE_BOX_INERTIA_APPROXIMATION 1
#if !USE_BOX_INERTIA_APPROXIMATION
/*
* cylinder is defined as following:
*
* - principle axis aligned along y by default, radius in x, z-value not used
* - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
* - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
*
*/
float radius2; // square of cylinder radius
float height2; // square of cylinder height
IndexedVector3 halfExtents = GetHalfExtentsWithMargin(); // get cylinder dimension
float div12 = mass / 12.0f;
float div4 = mass / 4.0f;
float div2 = mass / 2.0f;
int idxRadius, idxHeight;
switch (m_upAxis) // get indices of radius and height of cylinder
{
case 0: // cylinder is aligned along x
idxRadius = 1;
idxHeight = 0;
break;
case 2: // cylinder is aligned along z
idxRadius = 0;
idxHeight = 2;
break;
default: // cylinder is aligned along y
idxRadius = 0;
idxHeight = 1;
break;
}
// calculate squares
radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
height2 = 4.0f * halfExtents[idxHeight] * halfExtents[idxHeight];
// calculate tensor terms
float t1 = div12 * height2 + div4 * radius2;
float t2 = div2 * radius2;
switch (m_upAxis) // set diagonal elements of inertia tensor
{
case 0: // cylinder is aligned along x
inertia = new IndexedVector3(t2, t1, t1);
break;
case 2: // cylinder is aligned along z
inertia = new IndexedVector3(t1, t1, t2);
break;
default: // cylinder is aligned along y
inertia = new IndexedVector3(t1, t2, t1);
break;
}
#else
//approximation of box shape, todo: implement cylinder shape inertia before people notice ;-)
IndexedVector3 halfExtents = GetHalfExtentsWithMargin();
float lx = 2.0f * (halfExtents.X);
float ly = 2.0f * (halfExtents.Y);
float lz = 2.0f * (halfExtents.Z);
inertia = new IndexedVector3(mass / 12.0f * (ly * ly + lz * lz),
mass / 12.0f * (lx * lx + lz * lz),
mass / 12.0f * (lx * lx + ly * ly));
#endif //USE_BOX_INERTIA_APPROXIMATION
}
} // for pool
public DebugDrawcallback(IDebugDraw debugDrawer, ref IndexedMatrix worldTrans, ref IndexedVector3 color)
{
m_debugDrawer = debugDrawer;
m_color = color;
m_worldTrans = worldTrans;
}
public IndexedVector3 CylinderLocalSupportZ(ref IndexedVector3 halfExtents, ref IndexedVector3 v)
{
return(CylinderLocalSupport(ref halfExtents, ref v, 2, 0, 2, 1));
}
public CylinderShapeZ(ref IndexedVector3 halfExtents)
: base(ref halfExtents)
{
m_upAxis = 2;
}
public RagDoll(RagDollDemo ragDollDemo, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, StreamWriter streamWriter)
{
m_ownerWorld = ownerWorld;
m_ragDollDemo = ragDollDemo;
// Setup the geometry
m_shapes[(int)BODYPART.PELVIS] = new CapsuleShape(0.15f, 0.20f);
m_shapes[(int)BODYPART.SPINE] = new CapsuleShape(0.15f, 0.28f);
m_shapes[(int)BODYPART.HEAD] = new CapsuleShape(0.10f, 0.05f);
m_shapes[(int)BODYPART.LEFT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
m_shapes[(int)BODYPART.LEFT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
m_shapes[(int)BODYPART.RIGHT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
m_shapes[(int)BODYPART.RIGHT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
m_shapes[(int)BODYPART.LEFT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
m_shapes[(int)BODYPART.LEFT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);
m_shapes[(int)BODYPART.RIGHT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
m_shapes[(int)BODYPART.RIGHT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);
// Setup all the rigid bodies
IndexedMatrix offset = IndexedMatrix.CreateTranslation(positionOffset);
IndexedMatrix transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1, 0));
IndexedMatrix adjusted = offset * transform;
m_bodies[(int)BODYPART.PELVIS] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.PELVIS], true);
m_bodies[(int)BODYPART.PELVIS].SetUserPointer("PELVIS");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.SPINE] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.SPINE], true);
m_bodies[(int)BODYPART.SPINE].SetUserPointer("SPINE");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.6f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.HEAD] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.HEAD], true);
m_bodies[(int)BODYPART.HEAD].SetUserPointer("HEAD");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.65f, 0));
adjusted = offset * transform
;
m_bodies[(int)BODYPART.LEFT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_LEG], true);
m_bodies[(int)BODYPART.LEFT_UPPER_LEG].SetUserPointer("LEFTUPPERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_LEG], true);
m_bodies[(int)BODYPART.LEFT_LOWER_LEG].SetUserPointer("LEFTLOWERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.65f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_LEG], true);
m_bodies[(int)BODYPART.RIGHT_UPPER_LEG].SetUserPointer("RIGHTUPPERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_LEG], true);
m_bodies[(int)BODYPART.RIGHT_LOWER_LEG].SetUserPointer("RIGHTLOWERLEG");
transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(-0.35f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_ARM], true);
m_bodies[(int)BODYPART.LEFT_UPPER_ARM].SetUserPointer("LEFTUPPERARM");
transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(-0.7f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_ARM], true);
m_bodies[(int)BODYPART.LEFT_LOWER_ARM].SetUserPointer("LEFTLOWERARM");
transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(0.35f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_ARM], true);
m_bodies[(int)BODYPART.RIGHT_UPPER_ARM].SetUserPointer("RIGHTUPPERARM");
transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(0.7f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_ARM], true);
m_bodies[(int)BODYPART.RIGHT_LOWER_ARM].SetUserPointer("RIGHTLOWERARM");
// Setup some damping on the m_bodies
for (int i = 0; i < (int)BODYPART.COUNT; ++i)
{
if (m_bodies[i] != null)
{
//m_bodies[i].SetDamping(0.05f, 0.85f);
m_bodies[i].SetDamping(0.5f, 0.85f);
m_bodies[i].SetDeactivationTime(0.8f);
m_bodies[i].SetSleepingThresholds(1.6f, 2.5f);
}
}
// Now setup the constraints
HingeConstraint hingeC;
ConeTwistConstraint coneC;
IndexedMatrix localA = IndexedMatrix.Identity;
IndexedMatrix localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.15f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.SPINE], ref localA, ref localB);
hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.PELVIS_SPINE] = hingeC;
m_joints[(int)JOINT.PELVIS_SPINE].m_debugName = "PELVIS_SPINE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.PELVIS_SPINE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.HEAD], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.SPINE_HEAD] = coneC;
m_joints[(int)JOINT.SPINE_HEAD].m_debugName = "SPINE_HEAD";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.SPINE_HEAD], true);
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
localA._origin = new IndexedVector3(-0.18f, -0.10f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.LEFT_UPPER_LEG], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
m_joints[(int)JOINT.LEFT_HIP] = coneC;
m_joints[(int)JOINT.LEFT_HIP].m_debugName = "LEFT_HIP";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_HIP], true);
localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_LEG], m_bodies[(int)BODYPART.LEFT_LOWER_LEG], ref localA, ref localB);
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.LEFT_KNEE] = hingeC;
m_joints[(int)JOINT.LEFT_KNEE].m_debugName = "LEFT_KNEE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_KNEE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
localA._origin = new IndexedVector3(0.18f, -0.10f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
m_joints[(int)JOINT.RIGHT_HIP] = coneC;
m_joints[(int)JOINT.RIGHT_HIP].m_debugName = "RIGHT_HIP";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_HIP], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], m_bodies[(int)BODYPART.RIGHT_LOWER_LEG], ref localA, ref localB);
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.RIGHT_KNEE] = hingeC;
m_joints[(int)JOINT.RIGHT_KNEE].m_debugName = "RIGHT_KNEE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_KNEE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_PI);
localA._origin = new IndexedVector3(-0.2f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.LEFT_UPPER_ARM], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[(int)JOINT.LEFT_SHOULDER] = coneC;
m_joints[(int)JOINT.LEFT_SHOULDER].m_debugName = "LEFT_SHOULDER";
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_SHOULDER], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_ARM], m_bodies[(int)BODYPART.LEFT_LOWER_ARM], ref localA, ref localB);
// hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.LEFT_ELBOW] = hingeC;
m_joints[(int)JOINT.LEFT_ELBOW].m_debugName = "LEFT_ELBOW";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_ELBOW], true);
localA = MathUtil.SetEulerZYX(0, 0, 0);
localA._origin = new IndexedVector3(0.2f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
m_joints[(int)JOINT.RIGHT_SHOULDER] = coneC;
m_joints[(int)JOINT.RIGHT_SHOULDER].m_debugName = "RIGHT_SHOULDER";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_SHOULDER], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], m_bodies[(int)BODYPART.RIGHT_LOWER_ARM], ref localA, ref localB);
// hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.RIGHT_ELBOW] = hingeC;
m_joints[(int)JOINT.RIGHT_ELBOW].m_debugName = "RIGHT_ELBOW";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_ELBOW], true);
}
public IndexedVector3 CylinderLocalSupportY(ref IndexedVector3 halfExtents, ref IndexedVector3 v)
{
return(CylinderLocalSupport(ref halfExtents, ref v, 1, 0, 1, 2));
}
public void SetMinAABB(ref IndexedVector3 min)
{
m_aabbMin = min;
}
public void SetMaxAABB(ref IndexedVector3 max)
{
m_aabbMax = max;
}
//----------------------------------------------------------------------------------------------
protected virtual void RenderScenePassMultiWorld(int pass, GameTime gameTime, DiscreteDynamicsWorld world)
{
IndexedMatrix m = IndexedMatrix.Identity;
IndexedBasisMatrix rot = IndexedBasisMatrix.Identity;
int numObjects = world.GetNumCollisionObjects();
IndexedVector3 wireColor = new IndexedVector3(1, 0, 0);
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = world.GetCollisionObjectArray()[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null && body.GetMotionState() != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.GetMotionState();
//myMotionState.m_graphicsWorldTrans.getOpenGLMatrix(m);
m = myMotionState.m_graphicsWorldTrans;
rot = myMotionState.m_graphicsWorldTrans._basis;
}
else
{
//colObj.getWorldTransform().getOpenGLMatrix(m);
m = colObj.GetWorldTransform();
rot = colObj.GetWorldTransform()._basis;
}
wireColor = new IndexedVector3(1.0f, 1.0f, 0.5f); //wants deactivation
if ((i & 1) != 0)
{
wireColor = new IndexedVector3(0f, 0f, 1f);
}
///color differently for active, sleeping, wantsdeactivation states
if (colObj.GetActivationState() == ActivationState.ACTIVE_TAG) //active
{
if ((i & 1) != 0)
{
wireColor += new IndexedVector3(1f, 0f, 0f);
}
else
{
wireColor += new IndexedVector3(.5f, 0f, 0f);
}
}
if (colObj.GetActivationState() == ActivationState.ISLAND_SLEEPING) //ISLAND_SLEEPING
{
if ((i & 1) != 0)
{
wireColor += new IndexedVector3(0f, 1f, 0f);
}
else
{
wireColor += new IndexedVector3(0f, 05f, 0f);
}
}
IndexedVector3 min, max;
world.GetBroadphase().GetBroadphaseAabb(out min, out max);
min -= MathUtil.MAX_VECTOR;
max += MathUtil.MAX_VECTOR;
// printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
// printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
// m_dynamicsWorld.getDebugDrawer().drawAabb(aabbMin,aabbMax,btVector3(1,1,1));
switch (pass)
{
case 0:
{
m_shapeDrawer.DrawXNA(ref m, colObj.GetCollisionShape(), ref wireColor, m_debugDraw.GetDebugMode(), ref min, ref max, ref m_lookAt, ref m_perspective);
break;
}
case 1:
{
IndexedVector3 shadow = rot * m_lightDirection;
m_shapeDrawer.DrawShadow(ref m, ref shadow, colObj.GetCollisionShape(), ref min, ref max);
break;
}
case 2:
{
IndexedVector3 adjustedWireColor = wireColor * 0.3f;
m_shapeDrawer.DrawXNA(ref m, colObj.GetCollisionShape(), ref adjustedWireColor, 0, ref min, ref max, ref m_lookAt, ref m_perspective);
break;
}
}
}
}
///findOrAddVertex is an internal method, use addTriangle instead
public int FindOrAddVertex(ref Microsoft.Xna.Framework.Vector3 vertex, bool removeDuplicateVertices)
{
IndexedVector3 iv3 = new IndexedVector3(vertex);
return(FindOrAddVertex(ref iv3, removeDuplicateVertices));
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix body0_trans, IndexedMatrix body1_trans)
{
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1;
info.m_solverConstraints[1].m_contactNormal.Y = 1;
info.m_solverConstraints[2].m_contactNormal.Z = 1;
IndexedVector3 a1 = body0_trans._basis * GetPivotInA();
{
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);
}
/*info->m_J2linearAxis[0] = -1;
* info->m_J2linearAxis[s+1] = -1;
* info->m_J2linearAxis[2*s+2] = -1;
*/
IndexedVector3 a2 = body1_trans._basis * GetPivotInB();
{
IndexedVector3 a2n = -a2;
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);
}
// set right hand side
float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp;
float k = info.fps * currERP;
int j;
IndexedVector3 body0Origin = body0_trans._origin;
IndexedVector3 body1Origin = body1_trans._origin;
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0)
{
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_cfm = m_cfm;
}
}
float impulseClamp = m_setting.m_impulseClamp;//
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info.m_solverConstraints[j].m_lowerLimit = -impulseClamp;
info.m_solverConstraints[j].m_upperLimit = impulseClamp;
}
}
info.m_damping = m_setting.m_damping;
}
public Point2PointConstraint(RigidBody rbA, ref IndexedVector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA)
{
m_pivotInA = pivotInA;
m_pivotInB = rbA.GetCenterOfMassTransform() * pivotInA;
}
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB)
: base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB)
{
m_pivotInA = pivotInA;
m_pivotInB = pivotInB;
}
public static void SetMax(ref IndexedVector3 a, ref IndexedVector3 b)
{
a.X = Math.Max(a.X, b.X);
a.Y = Math.Max(a.Y, b.Y);
a.Z = Math.Max(a.Z, b.Z);
}
public static float Proximity(ref DbvtAabbMm a, ref DbvtAabbMm b)
{
IndexedVector3 d = (a._min + a._max) - (b._min + b._max);
return(Math.Abs(d.X) + Math.Abs(d.Y) + Math.Abs(d.Z));
}
public void SpawnRagdoll(ref IndexedVector3 startOffset, StreamWriter streamWriter)
{
RagDoll ragDoll = new RagDoll(this, m_dynamicsWorld, ref startOffset, streamWriter);
m_ragdolls.Add(ragDoll);
}
public static DbvtAabbMm FromCR(ref IndexedVector3 c, float r)
{
IndexedVector3 temp = new IndexedVector3(r);
return(FromCE(ref c, ref temp));
}
///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
//skip the box 'getAabb'
// FIXME - Don't think we can call on it directly as below
//PolyhedralConvexShape.getAabb(ref trans,ref aabbMin,ref aabbMax);
//base.getAabb(ref trans,ref aabbMin,ref aabbMax);
AabbUtil2.TransformAabb(GetHalfExtentsWithoutMargin(), GetMargin(), ref trans, out aabbMin, out aabbMax);
}
public void Expand(IndexedVector3 e)
{
_min -= e; _max += e;
}
public IndexedVector3 CylinderLocalSupportZ(IndexedVector3 halfExtents, IndexedVector3 v)
{
return(CylinderLocalSupportZ(ref halfExtents, ref v));
}
public int Classify(ref IndexedVector3 n, float o, int s)
{
IndexedVector3 pi, px;
switch (s)
{
case (0 + 0 + 0):
{
px = new IndexedVector3(_min.X, _min.Y, _min.Z);
pi = new IndexedVector3(_max.X, _max.Y, _max.Z);
break;
}
case (1 + 0 + 0):
{
px = new IndexedVector3(_max.X, _min.Y, _min.Z);
pi = new IndexedVector3(_min.X, _max.Y, _max.Z); break;
}
case (0 + 2 + 0):
{
px = new IndexedVector3(_min.X, _max.Y, _min.Z);
pi = new IndexedVector3(_max.X, _min.Y, _max.Z); break;
}
case (1 + 2 + 0):
{
px = new IndexedVector3(_max.X, _max.Y, _min.Z);
pi = new IndexedVector3(_min.X, _min.Y, _max.Z); break;
}
case (0 + 0 + 4):
{
px = new IndexedVector3(_min.X, _min.Y, _max.Z);
pi = new IndexedVector3(_max.X, _max.Y, _min.Z); break;
}
case (1 + 0 + 4):
{
px = new IndexedVector3(_max.X, _min.Y, _max.Z);
pi = new IndexedVector3(_min.X, _max.Y, _min.Z); break;
}
case (0 + 2 + 4):
{
px = new IndexedVector3(_min.X, _max.Y, _max.Z);
pi = new IndexedVector3(_max.X, _min.Y, _min.Z); break;
}
case (1 + 2 + 4):
{
px = new IndexedVector3(_max.X, _max.Y, _max.Z);
pi = new IndexedVector3(_min.X, _min.Y, _min.Z); break;
}
default:
{
px = new IndexedVector3();
pi = new IndexedVector3();
break;
}
}
if ((IndexedVector3.Dot(n, px) + o) < 0)
{
return(-1);
}
if ((IndexedVector3.Dot(n, pi) + o) >= 0)
{
return(+1);
}
return(0);
}
public CylinderShapeZ(IndexedVector3 halfExtents)
: this(ref halfExtents)
{ }
public static void Split(ObjectArray <DbvtNode> leaves, ObjectArray <DbvtNode> left, ObjectArray <DbvtNode> right, ref IndexedVector3 org, ref IndexedVector3 axis)
{
left.Resize(0);
right.Resize(0);
for (int i = 0, ni = leaves.Count; i < ni; ++i)
{
if (IndexedVector3.Dot(axis, leaves[i].volume.Center() - org) < 0)
{
left.Add(leaves[i]);
}
else
{
right.Add(leaves[i]);
}
}
}
public override IndexedVector3 LocalGetSupportingVertexWithoutMargin(ref IndexedVector3 vec)
{
return(CylinderLocalSupportZ(GetHalfExtentsWithoutMargin(), vec));
}
public static void RayTest(DbvtNode root,
ref IndexedVector3 rayFrom,
ref IndexedVector3 rayTo,
ICollide policy)
{
PooledType <DbvtStackDataBlock> objPool = BulletGlobals.DbvtStackDataBlockPool;
using (DbvtStackDataBlock stackDataBlock = objPool.Get())
{
if (root != null)
{
IndexedVector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
///what about division by zero? -. just set rayDirection[i] to INF/BT_LARGE_FLOAT
IndexedVector3 rayDirectionInverse = new IndexedVector3(
rayDir.X == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.X,
rayDir.Y == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Y,
rayDir.Z == 0.0f ? MathUtil.BT_LARGE_FLOAT : 1.0f / rayDir.Z);
stackDataBlock.signs[0] = rayDirectionInverse.X < 0.0f;
stackDataBlock.signs[1] = rayDirectionInverse.Y < 0.0f;
stackDataBlock.signs[2] = rayDirectionInverse.Z < 0.0f;
float lambda_max = IndexedVector3.Dot(rayDir, (rayTo - rayFrom));
int depth = 1;
int treshold = DOUBLE_STACKSIZE - 2;
stackDataBlock.stack.Resize(DOUBLE_STACKSIZE);
stackDataBlock.stack[0] = root;
do
{
DbvtNode node = stackDataBlock.stack[--depth];
stackDataBlock.bounds[0] = node.volume.Mins();
stackDataBlock.bounds[1] = node.volume.Maxs();
float tmin = 1.0f, lambda_min = 0.0f;
bool result1 = AabbUtil2.RayAabb2(ref rayFrom, ref rayDirectionInverse, stackDataBlock.signs, stackDataBlock.bounds, out tmin, lambda_min, lambda_max);
#if COMPARE_BTRAY_AABB2
float param = 1.0f;
bool result2 = AabbUtil.RayAabb(ref rayFrom, ref rayTo, node.volume.Mins(), node.volume.Maxs(), param, resultNormal);
Debug.Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if (result1)
{
if (node.IsInternal())
{
if (depth > treshold)
{
stackDataBlock.stack.Resize(stackDataBlock.stack.Count * 2);
treshold = stackDataBlock.stack.Count - 2;
}
stackDataBlock.stack[depth++] = node._children[0];
stackDataBlock.stack[depth++] = node._children[1];
}
else
{
policy.Process(node);
}
}
} while (depth != 0);
}
}
}
public static void DebugDrawConstraint(TypedConstraint constraint, IDebugDraw debugDraw)
{
bool drawFrames = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraints) != 0;
bool drawLimits = (debugDraw.GetDebugMode() & DebugDrawModes.DBG_DrawConstraintLimits) != 0;
float dbgDrawSize = constraint.GetDbgDrawSize();
if (dbgDrawSize <= 0f)
{
return;
}
switch (constraint.GetConstraintType())
{
case TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE:
{
Point2PointConstraint p2pC = constraint as Point2PointConstraint;
IndexedMatrix tr = IndexedMatrix.Identity;
IndexedVector3 pivot = p2pC.GetPivotInA();
pivot = p2pC.GetRigidBodyA().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
debugDraw.DrawTransform(ref tr, dbgDrawSize);
// that ideally should draw the same frame
pivot = p2pC.GetPivotInB();
pivot = p2pC.GetRigidBodyB().GetCenterOfMassTransform() * pivot;
tr._origin = pivot;
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
}
break;
case TypedConstraintType.HINGE_CONSTRAINT_TYPE:
{
HingeConstraint pHinge = constraint as HingeConstraint;
IndexedMatrix tr = pHinge.GetRigidBodyA().GetCenterOfMassTransform() * pHinge.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pHinge.GetRigidBodyB().GetCenterOfMassTransform() * pHinge.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
float minAng = pHinge.GetLowerLimit();
float maxAng = pHinge.GetUpperLimit();
if (minAng == maxAng)
{
break;
}
bool drawSect = true;
if (minAng > maxAng)
{
minAng = 0f;
maxAng = MathUtil.SIMD_2_PI;
drawSect = false;
}
if (drawLimits)
{
IndexedVector3 center = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(2);
IndexedVector3 axis = tr._basis.GetColumn(0);
IndexedVector3 zero = IndexedVector3.Zero;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, ref zero, drawSect);
}
}
break;
case TypedConstraintType.CONETWIST_CONSTRAINT_TYPE:
{
ConeTwistConstraint pCT = constraint as ConeTwistConstraint;
IndexedMatrix tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
//const float length = float(5);
float length = dbgDrawSize;
const int nSegments = 8 * 4;
float fAngleInRadians = MathUtil.SIMD_2_PI * (float)(nSegments - 1) / (float)nSegments;
IndexedVector3 pPrev = pCT.GetPointForAngle(fAngleInRadians, length);
pPrev = tr * pPrev;
for (int i = 0; i < nSegments; i++)
{
fAngleInRadians = MathUtil.SIMD_2_PI * (float)i / (float)nSegments;
IndexedVector3 pCur = pCT.GetPointForAngle(fAngleInRadians, length);
pCur = tr * pCur;
debugDraw.DrawLine(ref pPrev, ref pCur, ref zero);
if (i % (nSegments / 8) == 0)
{
IndexedVector3 origin = tr._origin;
debugDraw.DrawLine(ref origin, ref pCur, ref zero);
}
pPrev = pCur;
}
float tws = pCT.GetTwistSpan();
float twa = pCT.GetTwistAngle();
bool useFrameB = (pCT.GetRigidBodyB().GetInvMass() > 0f);
if (useFrameB)
{
tr = pCT.GetRigidBodyB().GetCenterOfMassTransform() * pCT.GetBFrame();
}
else
{
tr = pCT.GetRigidBodyA().GetCenterOfMassTransform() * pCT.GetAFrame();
}
IndexedVector3 pivot = tr._origin;
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
debugDraw.DrawArc(ref pivot, ref normal, ref axis, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, ref zero, true);
}
}
break;
case TypedConstraintType.D6_CONSTRAINT_TYPE:
case TypedConstraintType.D6_SPRING_CONSTRAINT_TYPE:
{
Generic6DofConstraint p6DOF = constraint as Generic6DofConstraint;
IndexedMatrix tr = p6DOF.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = p6DOF.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 center = p6DOF.GetCalculatedTransformB()._origin;
// up is axis 1 not 2 ?
IndexedVector3 up = tr._basis.GetColumn(1);
IndexedVector3 axis = tr._basis.GetColumn(0);
float minTh = p6DOF.GetRotationalLimitMotor(1).m_loLimit;
float maxTh = p6DOF.GetRotationalLimitMotor(1).m_hiLimit;
float minPs = p6DOF.GetRotationalLimitMotor(2).m_loLimit;
float maxPs = p6DOF.GetRotationalLimitMotor(2).m_hiLimit;
debugDraw.DrawSpherePatch(ref center, ref up, ref axis, dbgDrawSize * .9f, minTh, maxTh, minPs, maxPs, ref zero);
axis = tr._basis.GetColumn(1);
float ay = p6DOF.GetAngle(1);
float az = p6DOF.GetAngle(2);
float cy = (float)Math.Cos(ay);
float sy = (float)Math.Sin(ay);
float cz = (float)Math.Cos(az);
float sz = (float)Math.Sin(az);
IndexedVector3 ref1 = new IndexedVector3(
cy * cz * axis.X + cy * sz * axis.Y - sy * axis.Z,
-sz * axis.X + cz * axis.Y,
cz * sy * axis.X + sz * sy * axis.Y + cy * axis.Z);
tr = p6DOF.GetCalculatedTransformB();
IndexedVector3 normal = -tr._basis.GetColumn(0);
float minFi = p6DOF.GetRotationalLimitMotor(0).m_loLimit;
float maxFi = p6DOF.GetRotationalLimitMotor(0).m_hiLimit;
if (minFi > maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, -MathUtil.SIMD_PI, MathUtil.SIMD_PI, ref zero, false);
}
else if (minFi < maxFi)
{
debugDraw.DrawArc(ref center, ref normal, ref ref1, dbgDrawSize, dbgDrawSize, minFi, maxFi, ref zero, false);
}
tr = p6DOF.GetCalculatedTransformA();
IndexedVector3 bbMin = p6DOF.GetTranslationalLimitMotor().m_lowerLimit;
IndexedVector3 bbMax = p6DOF.GetTranslationalLimitMotor().m_upperLimit;
debugDraw.DrawBox(ref bbMin, ref bbMax, ref tr, ref zero);
}
}
break;
case TypedConstraintType.SLIDER_CONSTRAINT_TYPE:
{
SliderConstraint pSlider = constraint as SliderConstraint;
IndexedMatrix tr = pSlider.GetCalculatedTransformA();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
tr = pSlider.GetCalculatedTransformB();
if (drawFrames)
{
debugDraw.DrawTransform(ref tr, dbgDrawSize);
}
IndexedVector3 zero = IndexedVector3.Zero;
if (drawLimits)
{
IndexedMatrix tr2 = pSlider.GetCalculatedTransformA();
IndexedVector3 li_min = tr2 * new IndexedVector3(pSlider.GetLowerLinLimit(), 0f, 0f);
IndexedVector3 li_max = tr2 * new IndexedVector3(pSlider.GetUpperLinLimit(), 0f, 0f);
debugDraw.DrawLine(ref li_min, ref li_max, ref zero);
IndexedVector3 normal = tr._basis.GetColumn(0);
IndexedVector3 axis = tr._basis.GetColumn(1);
float a_min = pSlider.GetLowerAngLimit();
float a_max = pSlider.GetUpperAngLimit();
IndexedVector3 center = pSlider.GetCalculatedTransformB()._origin;
debugDraw.DrawArc(ref center, ref normal, ref axis, dbgDrawSize, dbgDrawSize, a_min, a_max, ref zero, true);
}
}
break;
default:
break;
}
return;
}
public static DbvtNode TopDown(Dbvt pdbvt, ObjectArray <DbvtNode> leaves, int bu_treshold)
{
if (leaves.Count > 1)
{
if (leaves.Count > bu_treshold)
{
DbvtAabbMm vol = Bounds(leaves);
IndexedVector3 org = vol.Center();
ObjectArray <DbvtNode>[] sets = { new ObjectArray <DbvtNode>(), new ObjectArray <DbvtNode>() };
int bestaxis = -1;
int bestmidp = leaves.Count;
int[] a1 = new int[] { 0, 0 };
int[] a2 = new int[] { 0, 0 };
int[] a3 = new int[] { 0, 0 };
int[][] splitcount = new int[][] { a1, a2, a3 };
int i;
for (i = 0; i < leaves.Count; ++i)
{
IndexedVector3 x = leaves[i].volume.Center() - org;
for (int j = 0; j < 3; ++j)
{
++splitcount[j][IndexedVector3.Dot(x, axis[j]) > 0 ? 1 : 0];
}
}
for (i = 0; i < 3; ++i)
{
if ((splitcount[i][0] > 0) && (splitcount[i][1] > 0))
{
int midp = (int)Math.Abs((splitcount[i][0] - splitcount[i][1]));
if (midp < bestmidp)
{
bestaxis = i;
bestmidp = midp;
}
}
}
if (bestaxis >= 0)
{
sets[0].EnsureCapacity(splitcount[bestaxis][0]);
sets[1].EnsureCapacity(splitcount[bestaxis][1]);
Split(leaves, sets[0], sets[1], ref org, ref axis[bestaxis]);
}
else
{
sets[0].EnsureCapacity(leaves.Count / 2 + 1);
sets[1].EnsureCapacity(leaves.Count / 2);
for (int i2 = 0, ni = leaves.Count; i2 < ni; ++i2)
{
sets[i2 & 1].Add(leaves[i2]);
}
}
DbvtNode node = CreateNode(pdbvt, null, ref vol, null);
node._children[0] = TopDown(pdbvt, sets[0], bu_treshold);
node._children[1] = TopDown(pdbvt, sets[1], bu_treshold);
node._children[0].parent = node;
node._children[1].parent = node;
return(node);
}
else
{
BottomUp(pdbvt, leaves);
return(leaves[0]);
}
}
return(leaves[0]);
}
public void Initialise(IDebugDraw debugDrawer, ref IndexedMatrix worldTrans, ref IndexedVector3 color)
{
m_debugDrawer = debugDrawer;
m_color = color;
m_worldTrans = worldTrans;
}
public void SetPivotA(ref IndexedVector3 pivotA)
{
m_pivotInA = pivotA;
}
