/*
* void MyContactCallback(object sender, ContactAddedEventArgs e)
* {
* if (e.CollisionObject0Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
* {
* CompoundShape compound = e.CollisionObject0Wrapper.CollisionObject.CollisionShape as CompoundShape;
* CollisionShape childShape = compound.GetChildShape(e.Index0);
* }
*
* if (e.CollisionObject1Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
* {
* CompoundShape compound = e.CollisionObject1Wrapper.CollisionObject.CollisionShape as CompoundShape;
* CollisionShape childShape = compound.GetChildShape(e.Index1);
* }
*
* e.IsContactModified = true;
* }
*/
public void SetupEmptyDynamicsWorld()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
convexDecompositionObjectOffset = new Vector3(10, 0, 0);
Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
//Broadphase = new SimpleBroadphase();
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
// create the ground
CollisionShape groundShape = new BoxShape(30, 2, 30);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
}
public Physics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(50, 1, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
}
public Physics()
{
CLStuff.InitCL();
cloths = new Cloth[numFlags];
for (int flagIndex = 0; flagIndex < numFlags; ++flagIndex)
{
cloths[flagIndex] = new Cloth();
cloths[flagIndex].CreateBuffers(clothWidth, clothHeight);
}
gSolver = new OpenCLSoftBodySolver(CLStuff.commandQueue, CLStuff.cxMainContext);
softBodyOutput = new SoftBodySolverOutputCLToCpu();
// collision configuration contains default setup for memory, collision setup
CollisionConf = new SoftBodyRigidBodyCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new SequentialImpulseConstraintSolver();
World = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf, gSolver);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(50, 50, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -60, 0), groundShape);
ground.UserObject = "Ground";
SoftWorld.WorldInfo.AirDensity = 1.2f;
SoftWorld.WorldInfo.WaterDensity = 0;
SoftWorld.WorldInfo.WaterOffset = 0;
SoftWorld.WorldInfo.WaterNormal = Vector3.Zero;
SoftWorld.WorldInfo.Gravity = new Vector3(0, -10, 0);
CreateFlag(clothWidth, clothHeight, out flags);
// Create output buffer descriptions for ecah flag
// These describe where the simulation should send output data to
for (int flagIndex = 0; flagIndex < flags.Count; ++flagIndex)
{
// flags[flagIndex].WindVelocity = new Vector3(0, 0, 15.0f);
// In this case we have a DX11 output buffer with a vertex at index 0, 8, 16 and so on as well as a normal at 3, 11, 19 etc.
// Copies will be performed GPU-side directly into the output buffer
CpuVertexBufferDescriptor vertexBufferDescriptor = new CpuVertexBufferDescriptor(cloths[flagIndex].CpuBuffer, 0, 8, 3, 8);
cloths[flagIndex].VertexBufferDescriptor = vertexBufferDescriptor;
}
gSolver.Optimize(SoftWorld.SoftBodyArray);
World.StepSimulation(1.0f / 60.0f, 0);
}
// Create a door using a hinge constraint attached to the world
private void CreateDoor()
{
const float mass = 1.0f;
var doorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(doorShape);
RigidBody doorBody = LocalCreateRigidBody(mass, Matrix.Translation(-5.0f, -2.0f, 0.0f), doorShape);
doorBody.ActivationState = ActivationState.DisableDeactivation;
var pivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
var axisA = Vector3.UnitY; // pointing upwards, aka Y-axis
var hinge = new HingeConstraint(doorBody, pivotA, axisA);
hinge.DebugDrawSize = 5;
//hinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//hinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//hinge.SetLimit(1, -1);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
hinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//hinge.SetLimit(0, 0);
World.AddConstraint(hinge);
}
private void CreateBoxes()
{
const float mass = 1.0f;
var colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
for (int y = 0; y < ArraySizeY; y++)
{
for (int x = 0; x < ArraySizeX; x++)
{
for (int z = 0; z < ArraySizeZ; z++)
{
Vector3 position = startPosition + 2 * new Vector3(x, y, z);
// make it drop from a height
position += new Vector3(0, 10, 0);
// using MotionState is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(Matrix.Translation(position));
var body = new RigidBody(rbInfo);
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
//World.DispatchInfo.UseConvexConservativeDistanceUtil = true;
//World.DispatchInfo.ConvexConservativeDistanceThreshold = 0.01f;
// Setup a big ground box
CollisionShape groundShape = new BoxShape(100, 10, 100);
CollisionShapes.Add(groundShape);
Matrix groundTransform = Matrix.Translation(0, -10, 0);
RigidBody ground = LocalCreateRigidBody(0, groundTransform, groundShape);
ground.UserObject = "Ground";
// Spawn one ragdoll
SpawnRagdoll(new Vector3(1, 0.5f, 0));
SpawnRagdoll(new Vector3(-1, 0.5f, 0));
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Vector3 worldAabbMin = new Vector3(-10000, -10000, -10000);
Vector3 worldAabbMax = new Vector3(10000, 10000, 10000);
Broadphase = new AxisSweep3(worldAabbMin, worldAabbMax);
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
World.SetInternalTickCallback(MotorPreTickCallback, this, true);
// create the ground
CollisionShape groundShape = new BoxShape(200, 10, 200);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -10, 0), groundShape);
ground.UserObject = "Ground";
fCyclePeriod = 2000.0f;
fMuscleStrength = 0.5f;
m_Time = 0;
SpawnTestRig(new Vector3(1, 0.5f, 0), false);
SpawnTestRig(new Vector3(-2, 0.5f, 0), true);
}
protected override void Initialize()
{
base.Initialize();
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
World.DebugDrawer = DebugDrawer;
// create the ground
_groundShape = new BoxShape(50, 1, 50);
LoadModel("ground", _groundShape);
CollisionShapes.Add(_groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, _groundShape);
// create a few dynamic rigidbodies
const float mass = 1.0f;
_boxShape = new BoxShape(1);
LoadModel("cube", _boxShape);
CollisionShapes.Add(_boxShape);
var rbInfo = new RigidBodyConstructionInfo(mass, null, _boxShape);
rbInfo.LocalInertia = _boxShape.CalculateLocalInertia(mass);
for (int k = 0; k < ArraySizeY; k++)
{
for (int i = 0; i < ArraySizeX; i++)
{
for (int j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.CreateTranslation(
_start + new Vector3(2 * i, 2 * k, 2 * j));
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
var body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
FreeLook.SetEyeTarget(ref _eye, ref _target);
}
private void CreateGround()
{
var groundShape = new BoxShape(30, 2, 30);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);
ground.UserObject = "Ground";
}
private void CreateGround()
{
var groundShape = new BoxShape(250, 50, 250);
CollisionShapes.Add(groundShape);
var ground = base.LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
}
private void CreateGround()
{
var groundShape = new BoxShape(150, 7, 150);
CollisionShapes.Add(groundShape);
var ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
}
private void CreateGround()
{
var groundShape = new BoxShape(50, 1, 50);
//var groundShape = new StaticPlaneShape(Vector3.UnitY, 1);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
}
private void CreateGround()
{
BoxShape ground = new BoxShape(200, 1, 200);
ground.InitializePolyhedralFeatures();
CollisionShapes.Add(ground);
RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);
body.Friction = 0.5f;
//body.RollingFriction = 0.3f;
body.UserObject = "Ground";
}
private void CreateGround()
{
var groundShape = new BoxShape(50, 1, 50);
//groundShape.InitializePolyhedralFeatures();
//var groundShape = new StaticPlaneShape(Vector3.UnitY, 1);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
}
private void CreateBoxes()
{
var colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
for (int i = 0; i < 10; i++)
{
//CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
Matrix startTransform = Matrix.Translation(2 * i, 10, 1);
LocalCreateRigidBody(1.0f, startTransform, colShape);
}
}
private void CreateBoxes()
{
var colShape = new BoxShape(1);
//var colShape = new CapsuleShape(0.5f, 2.0f);//boxShape = new SphereShape(1.0f);
CollisionShapes.Add(colShape);
for (int j = 0; j < NumDynamicBoxesX; j++)
{
for (int i = 0; i < NumDynamicBoxesY; i++)
{
Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
LocalCreateRigidBody(1.0f, startTransform, colShape);
}
}
}
private void CreateGround()
{
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
const int triangleIndexStride = 3 * sizeof(int);
const int vertexStride = Vector3.SizeInBytes;
var mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts, triangleIndexStride, vertexStride);
var indicesStream = mesh.GetTriangleStream();
using (var indices = new BinaryWriter(indicesStream))
{
for (int x = 0; x < NumVertsX - 1; x++)
{
for (int y = 0; y < NumVertsY - 1; y++)
{
int row1Index = x * NumVertsX + y;
int row2Index = row1Index + NumVertsX;
indices.Write(row1Index);
indices.Write(row1Index + 1);
indices.Write(row2Index + 1);
indices.Write(row1Index);
indices.Write(row2Index + 1);
indices.Write(row2Index);
}
}
}
indexVertexArrays = new TriangleIndexVertexArray();
indexVertexArrays.AddIndexedMesh(mesh);
SetVertexPositions(waveHeight, 0.0f);
const bool useQuantizedAabbCompression = true;
groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
CollisionShapes.Add(groundShape);
staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
staticBody.CollisionFlags |= CollisionFlags.StaticObject;
staticBody.UserObject = "Ground";
}
/*
* void MyContactCallback(object sender, ContactAddedEventArgs e)
* {
* if (e.CollisionObject0Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
* {
* CompoundShape compound = e.CollisionObject0Wrapper.CollisionObject.CollisionShape as CompoundShape;
* CollisionShape childShape = compound.GetChildShape(e.Index0);
* }
*
* if (e.CollisionObject1Wrapper.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
* {
* CompoundShape compound = e.CollisionObject1Wrapper.CollisionObject.CollisionShape as CompoundShape;
* CollisionShape childShape = compound.GetChildShape(e.Index1);
* }
*
* e.IsContactModified = true;
* }
*/
public void SetupEmptyDynamicsWorld()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new AxisSweep3(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000));
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
// create the ground
CollisionShape groundShape = new BoxShape(30, 2, 30);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -4.5f, 0), groundShape);
ground.UserObject = "Ground";
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new AxisSweep3(
new Vector3(-1000, -1000, -1000),
new Vector3(1000, 1000, 1000));
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
renderCallback = new DrawingResult(World);
var boxA = new BoxShape(1.0f)
{
Margin = 0
};
var boxB = new BoxShape(0.5f)
{
Margin = 0
};
CollisionShapes.Add(boxA);
CollisionShapes.Add(boxB);
Quaternion rotA = new Quaternion(0.739f, -0.204f, 0.587f, 0.257f);
rotA.Normalize();
objectA = new CollisionObject
{
CollisionShape = boxA,
WorldTransform = Matrix.RotationQuaternion(rotA) * Matrix.Translation(0, 3, 0)
};
objectB = new CollisionObject
{
CollisionShape = boxB,
WorldTransform = Matrix.Translation(0, 4.248f, 0)
};
//World.AddCollisionObject(objectA);
World.AddCollisionObject(objectB);
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Vector3 worldMin = new Vector3(-1000, -1000, -1000);
Vector3 worldMax = new Vector3(1000, 1000, 1000);
Broadphase = new AxisSweep3(worldMin, worldMax);
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SplitImpulse = 1;
World.Gravity = new Vector3(0, -10, 0);
IsDebugDrawEnabled = true;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
for (int j = 0; j < NumDynamicBoxesX; j++)
{
for (int i = 0; i < NumDynamicBoxesY; i++)
{
//CollisionShape colShape = new CapsuleShape(0.5f,2.0f);//boxShape = new SphereShape(1.0f);
Matrix startTransform = Matrix.Translation(5 * (i - NumDynamicBoxesX / 2), 10, 5 * (j - NumDynamicBoxesY / 2));
LocalCreateRigidBody(1.0f, startTransform, colShape);
}
}
SetVertexPositions(WaveHeight, 0.0f);
const bool useQuantizedAabbCompression = true;
groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
CollisionShapes.Add(groundShape);
staticBody = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape);
staticBody.CollisionFlags |= CollisionFlags.StaticObject;
staticBody.UserObject = "Ground";
}
private RigidBody CreateGear(float radius, Matrix transform)
{
const float mass = 6.28f;
var shape = new CompoundShape();
var axle = new CylinderShape(0.2f, 0.25f, 0.2f);
var wheel = new CylinderShape(radius, 0.025f, radius);
shape.AddChildShape(Matrix.Identity, axle);
shape.AddChildShape(Matrix.Identity, wheel);
CollisionShapes.Add(shape);
CollisionShapes.Add(axle);
CollisionShapes.Add(wheel);
RigidBody body = LocalCreateRigidBody(mass, transform, shape);
body.LinearFactor = Vector3.Zero;
return(body);
}
protected override void OnInitializePhysics()
{
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
IsDebugDrawEnabled = true;
Vector3 a = Vector3.Zero;
World.DebugDrawWorld();
// ground
CollisionShape groundShape = new BoxShape(50, 1, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// Objects
//colShape = new BoxShape(1);
Vector3[] points0 = new Vector3[] {
new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1)
};
Vector3[] points1 = new Vector3[] {
new Vector3(1, 0, 0), new Vector3(0, 1, 0), new Vector3(0, 0, 1), new Vector3(0, 0, -1), new Vector3(-1, -1, 0)
};
colShape0 = new ConvexHullShape(points0);
colShape1 = new ConvexHullShape(points1);
CollisionShapes.Add(colShape0);
CollisionShapes.Add(colShape1);
body2 = LocalCreateRigidBody(0, body2Position, colShape1);
rotBody = LocalCreateRigidBody(0, rotBodyPosition, colShape0);
rotBody.CollisionFlags |= CollisionFlags.KinematicObject;
rotBody.ActivationState = ActivationState.DisableDeactivation;
}
public void BeginContact(Contact contact)
{
if (!contact.AreTouching)
{
return;
}
CollisionData temp = new CollisionData();
temp.BodyA = contact.FixtureA.Body;
temp.BodyB = contact.FixtureB.Body;
WorldManifold manifold;
contact.GetWorldManifold(out manifold);
for (int i = 0; i < contact.Manifold.PointCount; i++)
{
temp.Points.Add(manifold.Points[i].ToAsdVector());
}
CollisionShapes.Add(temp);
}
void AddBoxes()
{
// create a few dynamic rigidbodies
const float mass = 1.0f;
BoxShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
const float startX = StartPosX - ArraySizeX / 2;
const float startY = StartPosY;
const float startZ = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
3 * i + startX,
3 * k + startY,
3 * j + startZ
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia))
{
var body = new RigidBody(rbInfo);
World.AddRigidBody(body);
}
}
}
}
}
void InitializeDemo()
{
motorControl.Goal = 0;
motorControl.MaxTorque = 0;
CollisionShape groundShape = new BoxShape(50, 50, 50);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -62, 0), groundShape);
body.UserObject = "Ground";
softBodyWorldInfo.SparseSdf.Reset();
softBodyWorldInfo.AirDensity = 1.2f;
softBodyWorldInfo.WaterDensity = 0;
softBodyWorldInfo.WaterOffset = 0;
softBodyWorldInfo.WaterNormal = Vector3.Zero;
softBodyWorldInfo.Gravity = new Vector3(0, -10, 0);
demos[demo]();
}
private void CreateBoxStack()
{
//var shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
var shape = new BoxShape(CubeHalfExtents);
CollisionShapes.Add(shape);
const int numObjects = 120;
for (int i = 0; i < numObjects; i++)
{
//stack them
const 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);
RigidBody body = LocalCreateRigidBody(1, trans, shape);
body.SetAnisotropicFriction(shape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.AnisotropicRollingFriction);
body.Friction = 0.5f;
//body.RollingFriction = 0.3f;
if (ccdMode)
{
body.CcdMotionThreshold = 1e-7f;
body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
}
}
}
protected override void OnInitializePhysics()
{
SetupDynamicsWorld();
CreateGround();
cubeShape = new BoxShape(CubeHalfExtent);
CollisionShapes.Add(cubeShape);
CreateGears();
CreateHingedBoxes();
CreateMotorHinge();
CreateMotorHinge2();
CreateSlider();
CreateD6Slider();
CreateDoor();
CreateGeneric6DofConstraint();
CreateConeTwist();
CreateUniversalConstraint();
CreateGeneric6DofSpringConstraint();
CreateHinge();
CreateHinge2();
}
protected override void OnInitializePhysics()
{
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.SolverMode |= SolverModes.Use2FrictionDirections | SolverModes.RandomizeOrder;
//World.SolverInfo.SplitImpulse = 0;
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.Friction = 0.5f;
//body.RollingFriction = 0.3f;
body.UserObject = "Ground";
//CollisionShape shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
CollisionShape shape = new BoxShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
CollisionShapes.Add(shape);
const int numObjects = 120;
for (i = 0; i < numObjects; i++)
{
//stack them
const 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);
body.SetAnisotropicFriction(shape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.AnisotropicRollingFriction);
body.Friction = 0.5f;
//body.RollingFriction = 0.3f;
if (ccdMode)
{
body.CcdMotionThreshold = 1e-7f;
body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
}
}
}
private void Create2dBodies()
{
// Re-using the same collision is better for memory usage and performance
float u = 0.96f;
Vector3[] points = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) };
var childShape0 = new BoxShape(1, 1, Depth);
var colShape = new Convex2DShape(childShape0);
var childShape1 = new ConvexHullShape(points);
var colShape2 = new Convex2DShape(childShape1);
var childShape2 = new CylinderShapeZ(1, 1, Depth);
var colShape3 = new Convex2DShape(childShape2);
CollisionShapes.Add(colShape);
CollisionShapes.Add(colShape2);
CollisionShapes.Add(colShape3);
CollisionShapes.Add(childShape0);
CollisionShapes.Add(childShape1);
CollisionShapes.Add(childShape2);
colShape.Margin = 0.03f;
float mass = 1.0f;
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
Vector3 x = new Vector3(-ArraySizeX, 8, -20);
Vector3 y = Vector3.Zero;
Vector3 deltaX = new Vector3(1, 2, 0);
Vector3 deltaY = new Vector3(2, 0, 0);
for (int i = 0; i < ArraySizeY; i++)
{
y = x;
for (int j = 0; j < ArraySizeX; j++)
{
Matrix startTransform = Matrix.Translation(y - new Vector3(-10, 0, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
switch (j % 3)
{
case 0:
rbInfo.CollisionShape = colShape;
break;
case 1:
rbInfo.CollisionShape = colShape3;
break;
default:
rbInfo.CollisionShape = colShape2;
break;
}
var body = new RigidBody(rbInfo)
{
//ActivationState = ActivationState.IslandSleeping,
LinearFactor = new Vector3(1, 1, 0),
AngularFactor = new Vector3(0, 0, 1)
};
World.AddRigidBody(body);
y += deltaY;
}
x += deltaX;
}
rbInfo.Dispose();
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
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;
}