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; } } } }