public override CollisionShape GetCollisionShape()
{
if (collisionShapePtr == null)
{
collisionShapePtr = new CylinderShape(halfExtent.ToBullet());
}
return collisionShapePtr;
}
public override CollisionShape GetCollisionShape()
{
if (collisionShapePtr == null)
{
collisionShapePtr = new CylinderShape(halfExtent.ToBullet());
((CylinderShape)collisionShapePtr).LocalScaling = m_localScaling.ToBullet();
}
return collisionShapePtr;
}
public override void InitPhysics()
{
int i;
shootBoxInitialSpeed = 4000;
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.BoxShape, BroadphaseNativeType.BoxShape,
CollisionConf.GetCollisionAlgorithmCreateFunc(BroadphaseNativeType.ConvexShape, BroadphaseNativeType.ConvexShape));
Broadphase = new DbvtBroadphase();
// the default constraint solver.
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SplitImpulse = 1;
World.SolverInfo.NumIterations = 20;
World.DispatchInfo.UseContinuous = ccdMode;
World.Gravity = new Vector3(0, -10, 0);
BoxShape ground = new BoxShape(200, 1, 200);
// ground.InitializePolyhedralFeatures();
CollisionShapes.Add(ground);
RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);
body.UserObject = "Ground";
CollisionShape shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
CollisionShapes.Add(shape);
int numObjects = 120;
for (i = 0; i < numObjects; i++)
{
//stack them
int colsize = 10;
int row = (int)((i * CubeHalfExtents * 2) / (colsize * 2 * CubeHalfExtents));
int row2 = row;
int col = (i) % (colsize) - colsize / 2;
if (col > 3)
{
col = 11;
row2 |= 1;
}
Matrix trans = Matrix.Translation(col * 2 * CubeHalfExtents + (row2 % 2) * CubeHalfExtents,
row * 2 * CubeHalfExtents + CubeHalfExtents + ExtraHeight, 0);
body = LocalCreateRigidBody(1, trans, shape);
if (ccdMode)
{
body.CcdMotionThreshold = CubeHalfExtents;
body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
}
}
}
ShapeData CreateCylinderShape(CylinderShape shape)
{
int up = shape.UpAxis;
float radius = shape.Radius;
float halfHeight = shape.HalfExtentsWithoutMargin[up] + shape.Margin;
int numSteps = 10;
float angleStep = (2 * (float)Math.PI) / numSteps;
ShapeData shapeData = new ShapeData();
shapeData.VertexCount = 2 + 6 * numSteps;
shapeData.IndexCount = (4 * numSteps + 2) * 3;
Vector3[] vertices = new Vector3[shapeData.VertexCount * 2];
ushort[] indices = new ushort[shapeData.IndexCount];
int i = 0, v = 0;
ushort index = 0;
ushort baseIndex;
Vector3 normal;
// Draw two sides
for (int side = 1; side != -3; side -= 2)
{
normal = GetVectorByAxis(side * Vector3.UnitY, up);
baseIndex = index;
vertices[v++] = GetVectorByAxis(new Vector3(0, side * halfHeight, 0), up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(new Vector3(0, side * halfHeight, radius), up);
vertices[v++] = normal;
index += 2;
for (int j = 1; j < numSteps; j++)
{
float x = radius * (float)Math.Sin(j * angleStep);
float z = radius * (float)Math.Cos(j * angleStep);
vertices[v++] = GetVectorByAxis(new Vector3(x, side * halfHeight, z), up);
vertices[v++] = normal;
indices[i++] = baseIndex;
if (side == 1)
{
indices[i++] = (ushort)(index - 1);
indices[i++] = index++;
}
else
{
indices[i++] = index;
indices[i++] = (ushort)(index - 1);
index++;
}
}
indices[i++] = baseIndex;
if (side == 1)
{
indices[i++] = (ushort)(index - 1);
indices[i++] = (ushort)(baseIndex + 1);
}
else
{
indices[i++] = (ushort)(baseIndex + 1);
indices[i++] = (ushort)(index - 1);
}
}
normal = GetVectorByAxis(new Vector3(0, 0, radius), up);
normal.Normalize();
baseIndex = index;
vertices[v++] = GetVectorByAxis(new Vector3(0, halfHeight, radius), up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(new Vector3(0, -halfHeight, radius), up);
vertices[v++] = normal;
index += 2;
for (int j = 1; j < numSteps + 1; j++)
{
float x = radius * (float)Math.Sin(j * angleStep);
float z = radius * (float)Math.Cos(j * angleStep);
normal = GetVectorByAxis(new Vector3(x, 0, z), up);
normal.Normalize();
vertices[v++] = GetVectorByAxis(new Vector3(x, halfHeight, z), up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(new Vector3(x, -halfHeight, z), up);
vertices[v++] = normal;
indices[i++] = (ushort)(index - 2);
indices[i++] = (ushort)(index - 1);
indices[i++] = index;
indices[i++] = index;
indices[i++] = (ushort)(index - 1);
indices[i++] = (ushort)(index + 1);
index += 2;
}
indices[i++] = (ushort)(index - 2);
indices[i++] = (ushort)(index - 1);
indices[i++] = baseIndex;
indices[i++] = baseIndex;
indices[i++] = (ushort)(index - 1);
indices[i] = (ushort)(baseIndex + 1);
shapeData.SetVertexBuffer(device, vertices);
shapeData.SetIndexBuffer(device, indices);
return shapeData;
}
void Init_ClusterRobot()
{
Vector3 basePos = new Vector3(0, 25, 8);
SoftBody psb0 = Init_ClusterRobot_CreateBall(basePos + new Vector3(-8, 0, 0));
SoftBody psb1 = Init_ClusterRobot_CreateBall(basePos + new Vector3(+8, 0, 0));
SoftBody psb2 = Init_ClusterRobot_CreateBall(basePos + new Vector3(0, 0, +8 * (float)Math.Sqrt(2)));
Vector3 ctr = (psb0.ClusterCom(0) + psb1.ClusterCom(0) + psb2.ClusterCom(0)) / 3;
CylinderShape pshp = new CylinderShape(new Vector3(8, 1, 8));
RigidBody prb = LocalCreateRigidBody(50, Matrix.Translation(ctr + new Vector3(0, 5, 0)), pshp);
LJoint.Specs ls = new LJoint.Specs();
ls.Erp = 0.5f;
Body prbBody = new Body(prb);
ls.Position = psb0.ClusterCom(0); psb0.AppendLinearJoint(ls, prbBody);
ls.Position = psb1.ClusterCom(0); psb1.AppendLinearJoint(ls, prbBody);
ls.Position = psb2.ClusterCom(0); psb2.AppendLinearJoint(ls, prbBody);
BoxShape pbox = new BoxShape(20, 1, 40);
RigidBody pgrn = LocalCreateRigidBody(0, Matrix.RotationZ(-(float)Math.PI / 4), pbox);
}
public static OpenTK.Vector3 GetHalfExtentsWithoutMargin(this CylinderShape obj)
{
OpenTK.Vector3 value;
GetHalfExtentsWithoutMargin(obj, out value);
return(value);
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
IsDebugDrawEnabled = true;
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI/4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2/L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
public static void CreateCylinder(CylinderShape cs, Mesh mesh)
{
mesh.Clear();
//float r = cs.Radius;
//todo this is a cube
mesh.Clear();
BulletSharp.Math.Vector3 ext = cs.HalfExtentsWithMargin;
float length = ext.X * 2f;
float width = ext.Y * 2f;
float height = ext.Z * 2f;
UnityEngine.Vector3 p0 = new UnityEngine.Vector3(-length * .5f, -width * .5f, height * .5f);
UnityEngine.Vector3 p1 = new UnityEngine.Vector3(length * .5f, -width * .5f, height * .5f);
UnityEngine.Vector3 p2 = new UnityEngine.Vector3(length * .5f, -width * .5f, -height * .5f);
UnityEngine.Vector3 p3 = new UnityEngine.Vector3(-length * .5f, -width * .5f, -height * .5f);
UnityEngine.Vector3 p4 = new UnityEngine.Vector3(-length * .5f, width * .5f, height * .5f);
UnityEngine.Vector3 p5 = new UnityEngine.Vector3(length * .5f, width * .5f, height * .5f);
UnityEngine.Vector3 p6 = new UnityEngine.Vector3(length * .5f, width * .5f, -height * .5f);
UnityEngine.Vector3 p7 = new UnityEngine.Vector3(-length * .5f, width * .5f, -height * .5f);
UnityEngine.Vector3[] vertices = new UnityEngine.Vector3[]
{
// Bottom
p0, p1, p2, p3,
// Left
p7, p4, p0, p3,
// Front
p4, p5, p1, p0,
// Back
p6, p7, p3, p2,
// Right
p5, p6, p2, p1,
// Top
p7, p6, p5, p4
};
UnityEngine.Vector3 up = UnityEngine.Vector3.up;
UnityEngine.Vector3 down = UnityEngine.Vector3.down;
UnityEngine.Vector3 front = UnityEngine.Vector3.forward;
UnityEngine.Vector3 back = UnityEngine.Vector3.back;
UnityEngine.Vector3 left = UnityEngine.Vector3.left;
UnityEngine.Vector3 right = UnityEngine.Vector3.right;
UnityEngine.Vector3[] normales = new UnityEngine.Vector3[]
{
// Bottom
down, down, down, down,
// Left
left, left, left, left,
// Front
front, front, front, front,
// Back
back, back, back, back,
// Right
right, right, right, right,
// Top
up, up, up, up
};
Vector2 _00 = new Vector2(0f, 0f);
Vector2 _10 = new Vector2(1f, 0f);
Vector2 _01 = new Vector2(0f, 1f);
Vector2 _11 = new Vector2(1f, 1f);
Vector2[] uvs = new Vector2[]
{
// Bottom
_11, _01, _00, _10,
// Left
_11, _01, _00, _10,
// Front
_11, _01, _00, _10,
// Back
_11, _01, _00, _10,
// Right
_11, _01, _00, _10,
// Top
_11, _01, _00, _10,
};
int[] triangles = new int[]
{
// Bottom
3, 1, 0,
3, 2, 1,
// Left
3 + 4 * 1, 1 + 4 * 1, 0 + 4 * 1,
3 + 4 * 1, 2 + 4 * 1, 1 + 4 * 1,
// Front
3 + 4 * 2, 1 + 4 * 2, 0 + 4 * 2,
3 + 4 * 2, 2 + 4 * 2, 1 + 4 * 2,
// Back
3 + 4 * 3, 1 + 4 * 3, 0 + 4 * 3,
3 + 4 * 3, 2 + 4 * 3, 1 + 4 * 3,
// Right
3 + 4 * 4, 1 + 4 * 4, 0 + 4 * 4,
3 + 4 * 4, 2 + 4 * 4, 1 + 4 * 4,
// Top
3 + 4 * 5, 1 + 4 * 5, 0 + 4 * 5,
3 + 4 * 5, 2 + 4 * 5, 1 + 4 * 5,
};
mesh.vertices = vertices;
mesh.normals = normales;
mesh.uv = uvs;
mesh.triangles = triangles;
mesh.RecalculateBounds();
mesh.Optimize();
}
public static UnityEngine.Vector3[] CreateCylinder(CylinderShape shape, out int[] indices)
{
int up = shape.UpAxis;
float radius = shape.Radius;
float halfHeight = shape.HalfExtentsWithoutMargin[up] + shape.Margin;
const int numSteps = 10;
const float angleStep = (2 * (float)Math.PI) / numSteps;
const int vertexCount = 2 + 6 * numSteps;
const int indexCount = (4 * numSteps + 2) * 3;
UnityEngine.Vector3[] vertices = new UnityEngine.Vector3[vertexCount * 2];
indices = new int[indexCount];
int i = 0, v = 0;
int index = 0;
int baseIndex;
UnityEngine.Vector3 normal;
// Draw two sides
for (int side = 1; side != -3; side -= 2)
{
normal = GetVectorByAxis(side * UnityEngine.Vector3.up, up);
baseIndex = index;
vertices[v++] = GetVectorByAxis(0, side * halfHeight, 0, up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(0, side * halfHeight, radius, up);
vertices[v++] = normal;
index += 2;
for (int j = 1; j < numSteps; j++)
{
float x = radius * (float)Math.Sin(j * angleStep);
float z = radius * (float)Math.Cos(j * angleStep);
vertices[v++] = GetVectorByAxis(x, side * halfHeight, z, up);
vertices[v++] = normal;
indices[i++] = baseIndex;
if (side == 1)
{
indices[i++] = index - 1;
indices[i++] = index++;
}
else
{
indices[i++] = index;
indices[i++] = index - 1;
index++;
}
}
indices[i++] = baseIndex;
if (side == 1)
{
indices[i++] = index - 1;
indices[i++] = baseIndex + 1;
}
else
{
indices[i++] = baseIndex + 1;
indices[i++] = index - 1;
}
}
normal = GetVectorByAxis(0, 0, radius, up);
normal.Normalize();
baseIndex = index;
vertices[v++] = GetVectorByAxis(0, halfHeight, radius, up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(0, -halfHeight, radius, up);
vertices[v++] = normal;
index += 2;
for (int j = 1; j < numSteps + 1; j++)
{
float x = radius * (float)Math.Sin(j * angleStep);
float z = radius * (float)Math.Cos(j * angleStep);
normal = GetVectorByAxis(x, 0, z, up);
normal.Normalize();
vertices[v++] = GetVectorByAxis(x, halfHeight, z, up);
vertices[v++] = normal;
vertices[v++] = GetVectorByAxis(x, -halfHeight, z, up);
vertices[v++] = normal;
indices[i++] = index - 2;
indices[i++] = index - 1;
indices[i++] = index;
indices[i++] = index;
indices[i++] = index - 1;
indices[i++] = index + 1;
index += 2;
}
indices[i++] = index - 2;
indices[i++] = index - 1;
indices[i++] = baseIndex;
indices[i++] = baseIndex;
indices[i++] = index - 1;
indices[i] = baseIndex + 1;
return vertices;
}
public CollisionShape CreateCylinderShapeY(float radius, float height)
{
CylinderShape shape = new CylinderShape(radius, height, radius);
_allocatedCollisionShapes.Add(shape);
return shape;
}
Mesh CreateCylinderShape(CylinderShape shape)
{
int upAxis = shape.UpAxis;
float radius = shape.Radius;
float halfHeight = shape.HalfExtentsWithoutMargin[upAxis] + shape.Margin;
Mesh mesh = Mesh.CreateCylinder(device, radius, radius, halfHeight * 2, 16, 1);
if (upAxis == 0)
{
Matrix[] transform = new Matrix[] { Matrix.RotationY((float)Math.PI / 2) };
Mesh meshRotated = Mesh.Concatenate(device, new Mesh[] { mesh }, MeshFlags.Managed, transform, null);
mesh.Dispose();
mesh = meshRotated;
}
else if (upAxis == 1)
{
Matrix[] transform = new Matrix[] { Matrix.RotationX((float)Math.PI / 2) };
Mesh cylinderMeshRot = Mesh.Concatenate(device, new Mesh[] { mesh }, MeshFlags.Managed, transform, null);
mesh.Dispose();
mesh = cylinderMeshRot;
}
shapes.Add(shape, mesh);
return mesh;
}