public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape, btVector3 localInertia)
{
m_mass = mass;
m_motionState = motionState;
m_collisionShape = collisionShape;
m_localInertia = localInertia;
m_linearDamping = 0f;
m_angularDamping = 0f;
m_friction = 0.5f;
m_restitution = 0f;
m_linearSleepingThreshold = 0.8f;
m_angularSleepingThreshold = 1f;
m_additionalDamping = false;
m_additionalDampingFactor = 0.005f;
m_additionalLinearDampingThresholdSqr = 0.01f;
m_additionalAngularDampingThresholdSqr = 0.01f;
m_additionalAngularDampingFactor = 0.01f;
m_startWorldTransform.setIdentity();
}
public RigidBody(float mass, IMotionState motionState, CollisionShape collisionShape, btVector3 localInertia)
{
setupRigidBody(new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia));
}
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;
}
}
}
}
