public void SetUp()
{
conf = new DefaultCollisionConfiguration();
dispatcher = new CollisionDispatcher(conf);
broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, conf);
groundShape = new BoxShape(30, 1, 30);
ground = CreateBody(0, Matrix.Translation(0, -5, 0), groundShape);
ground.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
compoundShape = new CompoundShape();
boxShape = new BoxShape(1, 1, 1);
compoundShape.AddChildShape(Matrix.Identity, boxShape);
boxShape2 = new BoxShape(1, 1, 1);
compoundShape.AddChildShape(Matrix.Translation(0, -1, 0), boxShape2);
boxShape3 = new BoxShape(1, 1, 1);
compoundShape.AddChildShape(Matrix.Translation(0, -2, 0), boxShape3);
compoundShape2 = new CompoundShape();
compoundShape2.AddChildShape(Matrix.Identity, compoundShape);
compound = CreateBody(1, Matrix.Translation(0, 0, 0), compoundShape2);
ManifoldPoint.ContactAdded += ContactAdded;
}
void CreateRigidBodyStack(int count)
{
const float mass = 10.0f;
CompoundShape cylinderCompound = new CompoundShape();
CollisionShape cylinderShape = new CylinderShapeX(4, 1, 1);
CollisionShape boxShape = new BoxShape(4, 1, 1);
cylinderCompound.AddChildShape(Matrix.Identity, boxShape);
Quaternion orn = Quaternion.RotationYawPitchRoll((float)Math.PI / 2.0f, 0.0f, 0.0f);
Matrix localTransform = Matrix.RotationQuaternion(orn);
//localTransform *= Matrix.Translation(new Vector3(1,1,1));
cylinderCompound.AddChildShape(localTransform, cylinderShape);
CollisionShape[] shape =
{
cylinderCompound,
new BoxShape(new Vector3(1, 1, 1)),
new SphereShape(1.5f)
};
for (int i = 0; i < count; ++i)
{
LocalCreateRigidBody(mass, Matrix.Translation(0, 2 + 6 * i, 0), shape[i % shape.Length]);
}
}
private void _injectShape(ChVector pos, ChMatrix33 <double> rot, CollisionShape mshape)
{
bool centered = true;// (pos.IsNull() && rot.IsIdentity()); // FIX THIS !!!
// This is needed so later one can access ChModelBullet::GetSafeMargin and ChModelBullet::GetEnvelope
mshape.SetUserPointer(this);
// start_vector = || -- description is still empty
if (shapes.Count == 0)
{
if (centered)
{
shapes.Add(mshape);
bt_collision_object.SetCollisionShape(mshape);
// end_vector= | centered shape |
return;
}
else
{
CompoundShape mcompound = new CompoundShape();
shapes.Add(mcompound);
shapes.Add(mshape);
bt_collision_object.SetCollisionShape(mcompound);
IndexedMatrix mtransform = new IndexedMatrix();
ChPosMatrToBullet(pos, rot, ref mtransform);
mcompound.AddChildShape(ref mtransform, mshape);
// vector= | compound | not centered shape |
return;
}
}
// start_vector = | centered shape | ----just a single centered shape was added
if (shapes.Count == 1)
{
IndexedMatrix mtransform = new IndexedMatrix();
shapes.Add(shapes[0]);
shapes.Add(mshape);
CompoundShape mcompound = new CompoundShape(true);
shapes[0] = mcompound;
bt_collision_object.SetCollisionShape(mcompound);
//mtransform.setIdentity();
mcompound.AddChildShape(ref mtransform, shapes[1]);
ChPosMatrToBullet(pos, rot, ref mtransform);
mcompound.AddChildShape(ref mtransform, shapes[2]);
// vector= | compound | old centered shape | new shape | ...
return;
}
// vector= | compound | old | old.. | ----already working with compounds..
if (shapes.Count > 1)
{
IndexedMatrix mtransform = new IndexedMatrix();
shapes.Add(mshape);
ChPosMatrToBullet(pos, rot, ref mtransform);
CollisionShape mcom = shapes[0];
((CompoundShape)mcom).AddChildShape(ref mtransform, mshape);
// vector= | compound | old | old.. | new shape | ...
return;
}
}
static void UpdateShapes(ref RigidbodyInternal rb)
{
if (rb.ownsCollisionShape && rb.CollisionShape != null)
{
rb.CollisionShape.Dispose();
}
CollisionShape resultShape;
var collisionShapes = rb.CollisionShapes;
if (collisionShapes.Count == 0)
{
//EmptyShape
resultShape = new EmptyShape();
}
else
{
//CompoundShape
var compoundShape = new CompoundShape();
for (int i = 0; i < collisionShapes.Count; i++)
{
compoundShape.AddChildShape(Matrix4x4.Identity, collisionShapes[i]);
}
resultShape = compoundShape;
}
rb.ownsCollisionShape = true;
rb.BulletRigidbody.CollisionShape = resultShape;
rb.updateShapes = false;
rb.updateMass = true;
}
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);
RigidBody body = LocalCreateRigidBody(mass, transform, shape);
body.LinearFactor = Vector3.Zero;
return(body);
}
//Generate and return an optimized Bullet ConvexHullShape
private void GenerateCompoundCollider()
{
//Get all child collision shapes
_childShapes = GetComponentsInChildren <BulletCollisionShape>().ToList();
//Warn the user if no child colliders
if (_childShapes.Count < 2)
{
Debug.LogError("Bullet Compound Shape requires at least two or more child collision shapes!\n" +
"Please add two or more primitive collision shapes as child objects.");
return;
}
//Generate the compound collider
_compoundShape = new CompoundShape {
LocalScaling = transform.localScale.ToBullet()
};
_childTransform = new Matrix();
foreach (var child in _childShapes)
{
if (child == this)
{
continue;
}
_childTransform.Origin = child.transform.localPosition.ToBullet();
_childTransform.SetOrientation(child.transform.rotation.ToBullet());
_compoundShape.AddChildShape(_childTransform, child.GetCollisionShape());
}
//Assign the compound collider to inherited member "Shape"
Shape = _compoundShape;
}
/// <summary>
/// Adds a <see cref="CollisionShape"/> to the multi hull shape.
/// </summary>
/// <param name="hullShape"></param>
/// <param name="offset"></param>
public void AddShape(CollisionShape hullShape, BulletSharp.Math.Matrix offset)
{
if (hullShapes.Contains(hullShape))
{
return;
}
hullShapes.Add(hullShape);
compoundShape.AddChildShape(offset, hullShape);
}
private CompoundShape CreateCompoundShape(Hacd hacd, Vector3 localScaling)
{
var wavefrontWriter = new WavefrontWriter("file_convex.obj");
var convexDecomposition = new ConvexDecomposition(wavefrontWriter)
{
LocalScaling = localScaling
};
for (int c = 0; c < hacd.NClusters; c++)
{
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
if (trianglesLen == 0)
{
continue;
}
var triangles = new long[trianglesLen];
int nVertices = hacd.GetNPointsCH(c);
var points = new double[nVertices * 3];
hacd.GetCH(c, points, triangles);
var verticesArray = new Vector3[nVertices];
int vi3 = 0;
for (int vi = 0; vi < nVertices; vi++)
{
verticesArray[vi] = new Vector3(
(float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
vi3 += 3;
}
convexDecomposition.Result(verticesArray, triangles);
}
wavefrontWriter.Dispose();
// Combine convex shapes into a compound shape
var compoundShape = new CompoundShape();
for (int i = 0; i < convexDecomposition.ConvexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.ConvexCentroids[i];
var convexShape = convexDecomposition.ConvexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (_enableSat)
{
convexShape.InitializePolyhedralFeatures();
}
compoundShape.AddChildShape(trans, convexShape);
PhysicsHelper.CreateBody(1.0f, trans, convexShape, World);
}
return(compoundShape);
}
/// <summary>
/// Adds a child shape.
/// </summary>
/// <param name="localTransform">The local transform.</param>
/// <param name="shape">The shape.</param>
public void AddChildShape(float4x4 localTransform, IBoxShapeImp shape)
{
Debug.WriteLine("AddBox");
var btHalfExtents = Translator.Float3ToBtVector3(shape.HalfExtents);
var btChildShape = new BoxShape(btHalfExtents);
var btLocalTransform = Translator.Float4X4ToBtMatrix(localTransform);
BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
}
void CreateGear(Vector3 pos, float speed)
{
Matrix startTransform = Matrix.Translation(pos);
CompoundShape shape = new CompoundShape();
#if true
shape.AddChildShape(Matrix.Identity, new BoxShape(5, 1, 6));
shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(5, 1, 6));
#else
shape.AddChildShape(Matrix.Identity, new CylinderShapeZ(5, 1, 7));
shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(4, 1, 8));
#endif
RigidBody body = LocalCreateRigidBody(10, startTransform, shape);
body.Friction = 1;
HingeConstraint hinge = new HingeConstraint(body, Matrix.Identity);
if (speed != 0)
{
hinge.EnableAngularMotor(true, speed, 3);
}
World.AddConstraint(hinge);
}
protected override CollisionShape CreateShape()
{
CompoundShape shape = new CompoundShape();
foreach (AbstractRigidShapeDefinition shapedef in this.children)
{
ShapeCustomData sc = new ShapeCustomData();
sc.Id = 0;
sc.ShapeDef = shapedef;
shape.AddChildShape((Matrix)shapedef.Pose, shapedef.GetShape(sc));
}
return(shape);
}
public void SetCollisionConvexMesh(Mesh mesh, Vector3 centerOfMass, Vector3 scale)
{
if (mesh == null)
{
Log.Error("Could not apply mesh to rigidbody! GameObject: " + GameObject.Name);
}
else
{
ConvexHullShape hull = new ConvexHullShape(mesh.GetPoints());
CompoundShape cs = new CompoundShape();
cs.AddChildShape(Matrix4.CreateTranslation(-centerOfMass), hull);
SetCollisionShape(cs, centerOfMass, scale);
}
}
protected override CollisionShape CreateShape()
{
CompoundShape shape = new CompoundShape();
foreach (AbstractRigidShapeDefinition shapedef in this.children)
{
ShapeCustomData sc = new ShapeCustomData();
sc.Id = 0;
sc.ShapeDef = shapedef;
Matrix tr = Matrix.Translation(shapedef.Translation);
Matrix rot = Matrix.RotationQuaternion(shapedef.Rotation);
shape.AddChildShape(Matrix.Multiply(rot, tr), shapedef.GetShape(sc));
}
return(shape);
}
private CompoundShape CreateCompoundShape(Hacd hacd, Vector3 localScaling)
{
var wavefrontWriter = new WavefrontWriter("file_convex.obj");
var convexDecomposition = new ConvexDecomposition(wavefrontWriter)
{
LocalScaling = localScaling
};
for (int c = 0; c < hacd.NClusters; c++)
{
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
if (trianglesLen == 0)
{
continue;
}
Vector3[] points;
int[] triangles;
hacd.GetCH(c, out points, out triangles);
convexDecomposition.Result(points, triangles);
}
wavefrontWriter.Dispose();
// Combine convex shapes into a compound shape
var compoundShape = new CompoundShape();
for (int i = 0; i < convexDecomposition.ConvexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.ConvexCentroids[i];
var convexShape = convexDecomposition.ConvexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (_enableSat)
{
convexShape.InitializePolyhedralFeatures();
}
compoundShape.AddChildShape(trans, convexShape);
LocalCreateRigidBody(1.0f, trans, convexShape);
}
return(compoundShape);
}
/// <summary>
/// Turns a BXDA mesh into a CompoundShape centered around the origin
/// </summary>
/// <param name="mesh"></param>
/// <returns></returns>
private static CompoundShape GetShape(BXDAMesh mesh)
{
CompoundShape shape = new CompoundShape();
Vector3[] meshVertices = mesh.AllColliderVertices().ToArray();
for (int i = 0; i < mesh.colliders.Count; i++)
{
BXDAMesh.BXDASubMesh sub = mesh.colliders[i];
Vector3[] vertices = sub.GetVertexData();
StridingMeshInterface sMesh = MeshUtilities.CenteredBulletShapeFromSubMesh(sub);
//Add the shape at a location relative to the compound shape such that the compound shape is centered at (0, 0) but child shapes are properly placed
shape.AddChildShape(Matrix4.CreateTranslation(MeshUtilities.MeshCenterRelative(sub, mesh)), new ConvexTriangleMeshShape(sMesh));
Console.WriteLine("Successfully created and added sub shape");
}
return(shape);
}
protected virtual CompoundShape _CreateCompoundShape(bool copyChildren)
{
//TODO
// some of the collider types (non-finite and other compound colliders) are probably not
// can only be added to game object with rigid body attached.
// allowed should check for these.
// what about scaling not sure if it is handled correctly
CompoundShape cs = new CompoundShape();
CollisionShapeWithTransform[] collisionShapes = GetSubCollisionShapes(copyChildren);
for (int i = 0; i < collisionShapes.Length; i++)
{
CollisionShape chcs = collisionShapes[i].Shape;
Vector3 up = Vector3.up;
Vector3 origin = Vector3.zero;
Vector3 forward = Vector3.forward;
//to world
up = collisionShapes[i].Transform.TransformDirection(up);
origin = collisionShapes[i].Transform.TransformPoint(origin);
forward = collisionShapes[i].Transform.TransformDirection(forward);
//to compound collider
up = transform.InverseTransformDirection(up);
origin = transform.InverseTransformPoint(origin);
forward = transform.InverseTransformDirection(forward);
Quaternion q = Quaternion.LookRotation(forward, up);
/*
* Some collision shapes can have local scaling applied. Use
* btCollisionShape::setScaling(vector3).Non uniform scaling with different scaling
* values for each axis, can be used for btBoxShape, btMultiSphereShape,
* btConvexShape, btTriangleMeshShape.Note that a non - uniform scaled
* sphere can be created by using a btMultiSphereShape with 1 sphere.
*/
BulletSharp.Math.Matrix m = BulletSharp.Math.Matrix.AffineTransformation(1f, q.ToBullet(), origin.ToBullet());
cs.AddChildShape(m, chcs);
}
cs.LocalScaling = m_localScaling.ToBullet();
cs.Margin = m_Margin;
return(cs);
}
protected virtual CompoundShape _CreateCompoundShape(bool copyChildren)
{
//TODO
// some of the collider types (non-finite and other compound colliders) are probably not
// can only be added to game object with rigid body attached.
// allowed should check for these.
// what about scaling not sure if it is handled correctly
CompoundShape cs = new CompoundShape();
CollisionShapeWithTransform[] collisionShapes = GetSubCollisionShapes(copyChildren);
for (int i = 0; i < collisionShapes.Length; i++)
{
CollisionShape chcs = collisionShapes[i].Shape;
// we need to invert the scale
BulletSharp.Math.Matrix m = (this.transform.worldToLocalMatrix * collisionShapes[i].Transform.localToWorldMatrix * Matrix4x4.Scale(collisionShapes[i].Transform.lossyScale).inverse).ToBullet();
cs.AddChildShape(m, chcs);
}
cs.LocalScaling = m_localScaling.ToBullet();
cs.Margin = m_Margin;
return(cs);
}
public Physics(VehicleDemo game)
{
CollisionShape groundShape = new BoxShape(50, 3, 50);
CollisionShapes.Add(groundShape);
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Solver = new SequentialImpulseConstraintSolver();
Vector3 worldMin = new Vector3(-10000, -10000, -10000);
Vector3 worldMax = new Vector3(10000, 10000, 10000);
Broadphase = new AxisSweep3(worldMin, worldMax);
//Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
int i;
Matrix tr;
Matrix vehicleTr;
if (UseTrimeshGround)
{
const float scale = 20.0f;
//create a triangle-mesh ground
int vertStride = Vector3.SizeInBytes;
int indexStride = 3 * sizeof(int);
const int NUM_VERTS_X = 20;
const int NUM_VERTS_Y = 20;
const int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;
const int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalVerts, vertStride, totalTriangles, indexStride, PhyScalarType.Int32, PhyScalarType.Single);
BulletSharp.DataStream data = mesh.LockVerts();
for (i = 0; i < NUM_VERTS_X; i++)
{
for (int j = 0; j < NUM_VERTS_Y; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
data.Write((i - NUM_VERTS_X * 0.5f) * scale);
data.Write(height);
data.Write((j - NUM_VERTS_Y * 0.5f) * scale);
}
}
int index = 0;
IntArray idata = mesh.TriangleIndices;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = j * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
vertexArray.AddIndexedMesh(mesh);
groundShape = new BvhTriangleMeshShape(vertexArray, true);
tr = Matrix.Identity;
vehicleTr = Matrix.Translation(0, -2, 0);
}
else
{
// Use HeightfieldTerrainShape
int width = 40, length = 40;
//int width = 128, length = 128; // Debugging is too slow for this
float maxHeight = 10.0f;
float heightScale = maxHeight / 256.0f;
Vector3 scale = new Vector3(20.0f, maxHeight, 20.0f);
//PhyScalarType scalarType = PhyScalarType.PhyUChar;
//FileStream file = new FileStream(heightfieldFile, FileMode.Open, FileAccess.Read);
// Use float data
PhyScalarType scalarType = PhyScalarType.Single;
byte[] terr = new byte[width * length * 4];
MemoryStream file = new MemoryStream(terr);
BinaryWriter writer = new BinaryWriter(file);
for (i = 0; i < width; i++)
{
for (int j = 0; j < length; j++)
{
writer.Write((float)((maxHeight / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
}
}
writer.Flush();
file.Position = 0;
HeightfieldTerrainShape heightterrainShape = new HeightfieldTerrainShape(width, length,
file, heightScale, 0, maxHeight, upIndex, scalarType, false);
heightterrainShape.SetUseDiamondSubdivision(true);
groundShape = heightterrainShape;
groundShape.LocalScaling = new Vector3(scale.X, 1, scale.Z);
tr = Matrix.Translation(new Vector3(-scale.X / 2, scale.Y / 2, -scale.Z / 2));
vehicleTr = Matrix.Translation(new Vector3(20, 3, -3));
// Create graphics object
file.Position = 0;
BinaryReader reader = new BinaryReader(file);
int totalTriangles = (width - 1) * (length - 1) * 2;
int totalVerts = width * length;
game.groundMesh = new Mesh(game.Device, totalTriangles, totalVerts,
MeshFlags.SystemMemory | MeshFlags.Use32Bit, VertexFormat.Position | VertexFormat.Normal);
SlimDX.DataStream data = game.groundMesh.LockVertexBuffer(LockFlags.None);
for (i = 0; i < width; i++)
{
for (int j = 0; j < length; j++)
{
float height;
if (scalarType == PhyScalarType.Single)
{
// heightScale isn't applied internally for float data
height = reader.ReadSingle();
}
else if (scalarType == PhyScalarType.Byte)
{
height = file.ReadByte() * heightScale;
}
else
{
height = 0.0f;
}
data.Write((j - length * 0.5f) * scale.X);
data.Write(height);
data.Write((i - width * 0.5f) * scale.Z);
// Normals will be calculated later
data.Position += 12;
}
}
game.groundMesh.UnlockVertexBuffer();
file.Close();
data = game.groundMesh.LockIndexBuffer(LockFlags.None);
for (i = 0; i < width - 1; i++)
{
for (int j = 0; j < length - 1; j++)
{
// Using diamond subdivision
if ((j + i) % 2 == 0)
{
data.Write(j * width + i);
data.Write((j + 1) * width + i + 1);
data.Write(j * width + i + 1);
data.Write(j * width + i);
data.Write((j + 1) * width + i);
data.Write((j + 1) * width + i + 1);
}
else
{
data.Write(j * width + i);
data.Write((j + 1) * width + i);
data.Write(j * width + i + 1);
data.Write(j * width + i + 1);
data.Write((j + 1) * width + i);
data.Write((j + 1) * width + i + 1);
}
/*
* // Not using diamond subdivision
* data.Write(j * width + i);
* data.Write((j + 1) * width + i);
* data.Write(j * width + i + 1);
*
* data.Write(j * width + i + 1);
* data.Write((j + 1) * width + i);
* data.Write((j + 1) * width + i + 1);
*/
}
}
game.groundMesh.UnlockIndexBuffer();
game.groundMesh.ComputeNormals();
}
CollisionShapes.Add(groundShape);
//create ground object
RigidBody ground = LocalCreateRigidBody(0, tr, groundShape);
ground.UserObject = "Ground";
CollisionShape chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);
CollisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
CollisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
RigidBody carChassis = LocalCreateRigidBody(800, Matrix.Identity, compound);
carChassis.UserObject = "Chassis";
//carChassis.SetDamping(0.2f, 0.2f);
//CylinderShapeX wheelShape = new CylinderShapeX(wheelWidth, wheelRadius, wheelRadius);
// clientResetScene();
// create vehicle
RaycastVehicle.VehicleTuning tuning = new RaycastVehicle.VehicleTuning();
IVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel = true;
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
Vector3 connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
WheelInfo a = vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
isFrontWheel = false;
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
for (i = 0; i < vehicle.NumWheels; i++)
{
WheelInfo wheel = vehicle.GetWheelInfo(i);
wheel.SuspensionStiffness = suspensionStiffness;
wheel.WheelsDampingRelaxation = suspensionDamping;
wheel.WheelsDampingCompression = suspensionCompression;
wheel.FrictionSlip = wheelFriction;
wheel.RollInfluence = rollInfluence;
}
vehicle.RigidBody.WorldTransform = vehicleTr;
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
WavefrontObj wo = new WavefrontObj();
int tcount = wo.LoadObj("data/file.obj");
if (tcount > 0)
{
TriangleMesh trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = vertices[index0] * localScaling;
Vector3 vertex1 = vertices[index1] * localScaling;
Vector3 vertex2 = vertices[index2] * localScaling;
trimesh.AddTriangle(vertex0, vertex1, vertex2);
}
ConvexShape tmpConvexShape = new ConvexTriangleMeshShape(trimesh);
//create a hull approximation
ShapeHull hull = new ShapeHull(tmpConvexShape);
float margin = tmpConvexShape.Margin;
hull.BuildHull(margin);
tmpConvexShape.UserObject = hull;
ConvexHullShape convexShape = new ConvexHullShape();
foreach (Vector3 v in hull.Vertices)
{
convexShape.AddPoint(v);
}
if (sEnableSAT)
{
convexShape.InitializePolyhedralFeatures();
}
tmpConvexShape.Dispose();
//hull.Dispose();
CollisionShapes.Add(convexShape);
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
const bool useQuantization = true;
CollisionShape concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// Bullet Convex Decomposition
FileStream outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
StreamWriter writer = new StreamWriter(outputFile);
DecompDesc desc = new DecompDesc
{
mVertices = wo.Vertices.ToArray(),
mTcount = tcount,
mIndices = wo.Indices.ToArray(),
mDepth = 5,
mCpercent = 5,
mPpercent = 15,
mMaxVertices = 16,
mSkinWidth = 0.0f
};
MyConvexDecomposition convexDecomposition = new MyConvexDecomposition(writer, this);
desc.mCallback = convexDecomposition;
// HACD
Hacd myHACD = new Hacd();
myHACD.SetPoints(wo.Vertices);
myHACD.SetTriangles(wo.Indices);
myHACD.CompacityWeight = 0.1;
myHACD.VolumeWeight = 0.0;
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
int nClusters = 2;
const double concavity = 100;
//bool invert = false;
const bool addExtraDistPoints = false;
const bool addNeighboursDistPoints = false;
const bool addFacesPoints = false;
myHACD.NClusters = nClusters; // minimum number of clusters
myHACD.VerticesPerConvexHull = 100; // max of 100 vertices per convex-hull
myHACD.Concavity = concavity; // maximum concavity
myHACD.AddExtraDistPoints = addExtraDistPoints;
myHACD.AddNeighboursDistPoints = addNeighboursDistPoints;
myHACD.AddFacesPoints = addFacesPoints;
myHACD.Compute();
nClusters = myHACD.NClusters;
myHACD.Save("output.wrl", false);
if (true)
{
CompoundShape compound = new CompoundShape();
CollisionShapes.Add(compound);
Matrix trans = Matrix.Identity;
for (int c = 0; c < nClusters; c++)
{
//generate convex result
Vector3[] points;
int[] triangles;
myHACD.GetCH(c, out points, out triangles);
ConvexResult r = new ConvexResult(points, triangles);
convexDecomposition.ConvexDecompResult(r);
}
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
trans = Matrix.Translation(centroid);
ConvexHullShape convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
compound.AddChildShape(trans, convexShape2);
RigidBody body = LocalCreateRigidBody(1.0f, trans, convexShape2);
}
#if true
mass = 10.0f;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
RigidBody body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
trans = Matrix.Translation(-convexDecompositionObjectOffset);
body2 = LocalCreateRigidBody(mass, trans, compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
writer.Dispose();
outputFile.Dispose();
}
}
public Character(DiscreteDynamicsWorld world, Vehiculo vehiculo, TGCVector3 position, float rotation, GameModel gameModel)
{
//Cargar sonido
turboSound = new TgcStaticSound();
turboSound.loadSound(Game.Default.MediaDirectory + Game.Default.FXDirectory + "turbo.wav", gameModel.DirectSound.DsDevice);
this.vehiculo = vehiculo;
var loader = new TgcSceneLoader();
this.mesh = loader.loadSceneFromFile(vehiculo.ChassisXmlPath).Meshes[0];
this.wheel = loader.loadSceneFromFile(vehiculo.WheelsXmlPath).Meshes[0];
Vehiculo.ChangeTextureColor(this.mesh, vehiculo.Color);
this.mesh.AutoTransform = false;
this.wheel.AutoTransform = false;
maxHitPoints = float.Parse(mesh.UserProperties["maxHitPoints"], CultureInfo.InvariantCulture);
engineForce = -float.Parse(mesh.UserProperties["engineForce"], CultureInfo.InvariantCulture);
brakeForce = float.Parse(mesh.UserProperties["brakeForce"], CultureInfo.InvariantCulture);
steeringAngle = -float.Parse(mesh.UserProperties["steeringAngle"], CultureInfo.InvariantCulture);
turboImpulse = float.Parse(mesh.UserProperties["turboImpulse"], CultureInfo.InvariantCulture);
frictionSlip = float.Parse(mesh.UserProperties["frictionSlip"], CultureInfo.InvariantCulture);
rollInfluence = float.Parse(mesh.UserProperties["rollInfluence"], CultureInfo.InvariantCulture);
rearWheelsHeight = float.Parse(mesh.UserProperties["rearWheelsHeight"], CultureInfo.InvariantCulture);
frontWheelsHeight = float.Parse(mesh.UserProperties["frontWheelsHeight"], CultureInfo.InvariantCulture);
suspensionRestLength = float.Parse(mesh.UserProperties["suspensionRestLength"], CultureInfo.InvariantCulture);
suspensionStiffness = float.Parse(mesh.UserProperties["suspensionStiffness"], CultureInfo.InvariantCulture);
dampingCompression = float.Parse(mesh.UserProperties["dampingCompression"], CultureInfo.InvariantCulture);
dampingRelaxation = float.Parse(mesh.UserProperties["dampingRelaxation"], CultureInfo.InvariantCulture);
meshAxisRadius = this.mesh.BoundingBox.calculateAxisRadius().ToBsVector;
var wheelRadius = this.wheel.BoundingBox.calculateAxisRadius().Y;
//The btBoxShape is centered at the origin
CollisionShape chassisShape = new BoxShape(meshAxisRadius.X, meshRealHeight, meshAxisRadius.Z);
//A compound shape is used so we can easily shift the center of gravity of our vehicle to its bottom
//This is needed to make our vehicle more stable
CompoundShape compound = new CompoundShape();
//The center of gravity of the compound shape is the origin. When we add a rigidbody to the compound shape
//it's center of gravity does not change. This way we can add the chassis rigidbody one unit above our center of gravity
//keeping it under our chassis, and not in the middle of it
var localTransform = Matrix.Translation(0, (meshAxisRadius.Y * 1.75f) - (meshRealHeight / 2f), 0);
compound.AddChildShape(localTransform, chassisShape);
//Creates a rigid body
this.rigidBody = CreateChassisRigidBodyFromShape(compound, position, rotation);
//Adds the vehicle chassis to the world
world.AddRigidBody(this.rigidBody);
worldID = world.CollisionObjectArray.IndexOf(this.rigidBody);
//RaycastVehicle
DefaultVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(world);
VehicleTuning tuning = new VehicleTuning();
//Creates a new instance of the raycast vehicle
vehicle = new RaycastVehicle(tuning, this.rigidBody, vehicleRayCaster);
//Never deactivate the vehicle
this.rigidBody.ActivationState = ActivationState.DisableDeactivation;
//Adds the vehicle to the world
world.AddAction(vehicle);
//Adds the wheels to the vehicle
AddWheels(meshAxisRadius, vehicle, tuning, wheelRadius);
//Inicializo puntos
hitPoints = maxHitPoints;
specialPoints = maxSpecialPoints;
timerMachineGun = 0f;
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 3, 50));
//CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new SimpleBroadphase(100, null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//m_dynamicsWorld.setGravity(new IndexedVector3(0,0,0));
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
//either use heightfield or triangle mesh
//create ground object
LocalCreateRigidBody(0f, ref tr, groundShape);
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
//m_carChassis = LocalCreateRigidBody(800f, ref tr, chassisShape);//chassisShape);
//CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
//m_collisionShapes.Add(liftShape);
//m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
//IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
//m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
//IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
//IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
//m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
//// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
//m_liftHinge.SetLimit(0.0f, 0.0f);
//m_dynamicsWorld.AddConstraint(m_liftHinge, true);
//CompoundShape forkCompound = new CompoundShape();
//m_collisionShapes.Add(forkCompound);
//IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
//CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
//m_collisionShapes.Add(forkShapeA);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
//CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeB);
//forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
//CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeC);
//forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
//m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
//IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
//m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
//localA = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
//IndexedVector3 col0 = MathUtil.matrixColumn(ref localA, 0);
//IndexedVector3 col1 = MathUtil.matrixColumn(ref localA, 1);
//IndexedVector3 col2 = MathUtil.matrixColumn(ref localA, 2);
////localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
//m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
//m_forkSlider.SetLowerLinLimit(0.1f);
//m_forkSlider.SetUpperLinLimit(0.1f);
//// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
//// m_forkSlider.setUpperAngLimit(LIFT_EPS);
//m_forkSlider.SetLowerAngLimit(0.0f);
//m_forkSlider.SetUpperAngLimit(0.0f);
//IndexedMatrix localAVec = IndexedMatrix.Identity;
//IndexedMatrix localBVec = IndexedMatrix.Identity;
//m_forkSlider2 = new HingeConstraint(m_liftBody, m_forkBody, ref localAVec, ref localBVec);
//m_dynamicsWorld.AddConstraint(m_forkSlider, true);
//m_dynamicsWorld.addConstraint(m_forkSlider2, true);
CompoundShape loadCompound = new CompoundShape(true);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#if false
{
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = localCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge.setLimit(0.0f, 0.0f);
m_dynamicsWorld.addConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.addChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = localCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.setLowerLinLimit(0.1f);
m_forkSlider.setUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.setLowerAngLimit(0.0f);
m_forkSlider.setUpperAngLimit(0.0f);
m_dynamicsWorld.addConstraint(m_forkSlider, true);
CompoundShape loadCompound = new CompoundShape();
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.addChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = localCreateRigidBody(4f, ref loadTrans, loadCompound);
}
#endif
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
SetCameraDistance(26.0f);
SetTexturing(true);
SetShadows(true);
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
convexDecompositionObjectOffset = new Vector3(10, 0, 0);
// Load wavefront file
var wo = new WavefrontObj();
int tcount = wo.LoadObj("data/file.obj");
if (tcount == 0)
{
return;
}
// Convert file data to TriangleMesh
var trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = vertices[index0] * localScaling;
Vector3 vertex1 = vertices[index1] * localScaling;
Vector3 vertex2 = vertices[index2] * localScaling;
trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
}
// Create a hull approximation
ConvexHullShape convexShape;
using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
{
using (var hull = new ShapeHull(tmpConvexShape))
{
hull.BuildHull(tmpConvexShape.Margin);
convexShape = new ConvexHullShape(hull.Vertices);
}
}
if (sEnableSAT)
{
convexShape.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape);
// Add non-moving body to world
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// HACD
var hacd = new Hacd();
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.CompacityWeight = 0.1;
hacd.VolumeWeight = 0.0;
// Recommended HACD parameters: 2 100 false false false
hacd.NClusters = 2; // minimum number of clusters
hacd.Concavity = 100; // maximum concavity
hacd.AddExtraDistPoints = false;
hacd.AddNeighboursDistPoints = false;
hacd.AddFacesPoints = false;
hacd.NumVerticesPerConvexHull = 100; // max of 100 vertices per convex-hull
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer, this);
convexDecomposition.LocalScaling = localScaling;
for (int c = 0; c < hacd.NClusters; c++)
{
Vector3[] points;
int[] triangles;
hacd.GetCH(c, out points, out triangles);
convexDecomposition.ConvexDecompResult(points, triangles);
}
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
Matrix trans = Matrix.Translation(centroid);
var convexShape2 = convexDecomposition.convexShapes[i] as ConvexHullShape;
if (sEnableSAT)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
writer.Dispose();
outputFile.Dispose();
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
private void CreateVehicle(Matrix transform)
{
var chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);
var compound = new CompoundShape();
//localTrans effectively shifts the center of mass with respect to the chassis
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
RigidBody carChassis = PhysicsHelper.CreateBody(800, Matrix.Identity, compound, World);
carChassis.UserObject = "Chassis";
//carChassis.SetDamping(0.2f, 0.2f);
var tuning = new VehicleTuning();
var vehicleRayCaster = new DefaultVehicleRaycaster(World);
//vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
_vehicle = new CustomVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(_vehicle);
const float connectionHeight = 1.2f;
// choose coordinate system
_vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
Vector3 wheelDirection = Vector3.Zero;
Vector3 wheelAxle = Vector3.Zero;
wheelDirection[upIndex] = -1;
wheelAxle[rightIndex] = -1;
bool isFrontWheel = true;
var connectionPoint = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
_vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPoint = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
_vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
isFrontWheel = false;
connectionPoint = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
_vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPoint = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
_vehicle.AddWheel(connectionPoint, wheelDirection, wheelAxle, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
for (int i = 0; i < _vehicle.NumWheels; i++)
{
WheelInfo wheel = _vehicle.GetWheelInfo(i);
wheel.SuspensionStiffness = suspensionStiffness;
wheel.WheelsDampingRelaxation = suspensionDamping;
wheel.WheelsDampingCompression = suspensionCompression;
wheel.FrictionSlip = wheelFriction;
wheel.RollInfluence = rollInfluence;
}
_vehicle.RigidBody.WorldTransform = transform;
}
CompoundShape _CreateCompoundShape(bool copyChildren)
{
BCollisionShape[] css = GetComponentsInChildren <BCollisionShape>();
colliders = new BCollisionShape[css.Length - 1];
int ii = 0;
for (int i = 0; i < css.Length; i++)
{
if (css[i] == this)
{
//skip
}
else
{
colliders[ii] = css[i];
ii++;
}
}
if (colliders.Length == 0)
{
Debug.LogError("Compound collider");
}
//TODO
// some of the collider types (non-finite and other compound colliders) are probably not
// can only be added to game object with rigid body attached.
// allowed should check for these.
// what about scaling not sure if it is handled correctly
CompoundShape cs = new CompoundShape();
for (int i = 0; i < colliders.Length; i++)
{
CollisionShape chcs;
if (copyChildren == true)
{
chcs = colliders[i].CopyCollisionShape();
}
else
{
chcs = colliders[i].GetCollisionShape();
}
Vector3 up = Vector3.up;
Vector3 origin = Vector3.zero;
Vector3 forward = Vector3.forward;
//to world
up = colliders[i].transform.TransformDirection(up);
origin = colliders[i].transform.TransformPoint(origin);
forward = colliders[i].transform.TransformDirection(forward);
//to compound collider
up = transform.InverseTransformDirection(up);
origin = transform.InverseTransformPoint(origin);
forward = transform.InverseTransformDirection(forward);
Quaternion q = Quaternion.LookRotation(forward, up);
/*
* Some collision shapes can have local scaling applied. Use
* btCollisionShape::setScaling(vector3).Non uniform scaling with different scaling
* values for each axis, can be used for btBoxShape, btMultiSphereShape,
* btConvexShape, btTriangleMeshShape.Note that a non - uniform scaled
* sphere can be created by using a btMultiSphereShape with 1 sphere.
*/
BulletSharp.Math.Matrix m = BulletSharp.Math.Matrix.AffineTransformation(1f, q.ToBullet(), origin.ToBullet());
cs.AddChildShape(m, chcs);
}
cs.LocalScaling = m_localScaling.ToBullet();
return(cs);
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
convexDecompositionObjectOffset = new Vector3(10, 0, 0);
// Load wavefront file
var wo = new WavefrontObj();
//string filename = UnityEngine.Application.dataPath + "/BulletUnity/Examples/Scripts/BulletSharpDemos/ConvexDecompositionDemo/data/file.obj";
UnityEngine.TextAsset bytes = (UnityEngine.TextAsset)UnityEngine.Resources.Load("file.obj");
System.IO.Stream byteStream = new System.IO.MemoryStream(bytes.bytes);
int tcount = wo.LoadObj(byteStream);
if (tcount == 0)
{
return;
}
// Convert file data to TriangleMesh
var trimesh = new TriangleMesh();
trimeshes.Add(trimesh);
Vector3 localScaling = new Vector3(6, 6, 6);
List <int> indices = wo.Indices;
List <Vector3> vertices = wo.Vertices;
int i;
for (i = 0; i < tcount; i++)
{
int index0 = indices[i * 3];
int index1 = indices[i * 3 + 1];
int index2 = indices[i * 3 + 2];
Vector3 vertex0 = vertices[index0] * localScaling;
Vector3 vertex1 = vertices[index1] * localScaling;
Vector3 vertex2 = vertices[index2] * localScaling;
trimesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
}
// Create a hull approximation
ConvexHullShape convexShape;
using (var tmpConvexShape = new ConvexTriangleMeshShape(trimesh))
{
using (var hull = new ShapeHull(tmpConvexShape))
{
hull.BuildHull(tmpConvexShape.Margin);
convexShape = new ConvexHullShape(hull.Vertices);
}
}
if (sEnableSAT)
{
convexShape.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape);
// Add non-moving body to world
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(trimesh, useQuantization);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
CollisionShapes.Add(concaveShape);
// HACD
var hacd = new Hacd();
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.CompacityWeight = 0.1;
hacd.VolumeWeight = 0.0;
// Recommended HACD parameters: 2 100 false false false
hacd.NClusters = 2; // minimum number of clusters
hacd.Concavity = 100; // maximum concavity
hacd.AddExtraDistPoints = false;
hacd.AddNeighboursDistPoints = false;
hacd.AddFacesPoints = false;
hacd.NVerticesPerCH = 100; // max of 100 vertices per convex-hull
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer, this);
convexDecomposition.LocalScaling = localScaling;
for (int c = 0; c < hacd.NClusters; c++)
{
int nVertices = hacd.GetNPointsCH(c);
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
double[] points = new double[nVertices * 3];
long[] triangles = new long[trianglesLen];
hacd.GetCH(c, points, triangles);
if (trianglesLen == 0)
{
continue;
}
Vector3[] verticesArray = new Vector3[nVertices];
int vi3 = 0;
for (int vi = 0; vi < nVertices; vi++)
{
verticesArray[vi] = new Vector3(
(float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
vi3 += 3;
}
int[] trianglesInt = new int[trianglesLen];
for (int ti = 0; ti < trianglesLen; ti++)
{
trianglesInt[ti] = (int)triangles[ti];
}
convexDecomposition.ConvexDecompResult(verticesArray, trianglesInt);
}
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
var convexShape2 = convexDecomposition.convexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (sEnableSAT)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
writer.Dispose();
outputFile.Dispose();
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
public override void InitializeDemo()
{
m_cameraDistance = 10.0f;
//string filename = @"e:\users\man\bullet\gimpact-demo-xna.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
/// Init Bullet
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
//m_broadphase = new btSimpleBroadphase();
int maxProxies = 1024;
IndexedVector3 worldAabbMin = new IndexedVector3(-10000, -10000, -10000);
IndexedVector3 worldAabbMax = new IndexedVector3(10000, 10000, 10000);
//m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new SimpleBroadphase(16384, null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//create trimesh model and shape
InitGImpactCollision();
/// Create Scene
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.Identity;
CollisionShape staticboxShape1 = new BoxShape(new IndexedVector3(200, 1, 200));//floor
staticboxShape1.SetUserPointer("Floor");
CollisionShape staticboxShape2 = new BoxShape(new IndexedVector3(1, 50, 200));//left wall
staticboxShape1.SetUserPointer("LeftWall");
CollisionShape staticboxShape3 = new BoxShape(new IndexedVector3(1, 50, 200));//right wall
staticboxShape1.SetUserPointer("RightWall");
CollisionShape staticboxShape4 = new BoxShape(new IndexedVector3(200, 50, 1));//front wall
staticboxShape1.SetUserPointer("FrontWall");
CollisionShape staticboxShape5 = new BoxShape(new IndexedVector3(200, 50, 1));//back wall
staticboxShape1.SetUserPointer("BackWall");
CompoundShape staticScenario = new CompoundShape();//static scenario
startTransform._origin = new IndexedVector3(0, 0, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape1);
startTransform._origin = new IndexedVector3(-200, 25, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape2);
startTransform._origin = new IndexedVector3(200, 25, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape3);
startTransform._origin = new IndexedVector3(0, 25, 200);
staticScenario.AddChildShape(ref startTransform, staticboxShape4);
startTransform._origin = new IndexedVector3(0, 25, -200);
staticScenario.AddChildShape(ref startTransform, staticboxShape5);
startTransform._origin = new IndexedVector3(0, 0, 0);
//RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticScenario);
RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticboxShape1);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//static plane
IndexedVector3 normal = new IndexedVector3(0.4f, 1.5f, -0.4f);
normal.Normalize();
CollisionShape staticplaneShape6 = new StaticPlaneShape(ref normal, 0.0f); // A plane
startTransform._origin = IndexedVector3.Zero;
RigidBody staticBody2 = LocalCreateRigidBody(mass, startTransform, staticplaneShape6);
staticBody2.SetCollisionFlags(staticBody2.GetCollisionFlags() | CollisionFlags.CF_STATIC_OBJECT);
startTransform = IndexedMatrix.Identity;
/// Create Dynamic Boxes
{
int numBoxes = 1;
for (int i = 0; i < numBoxes; i++)
{
CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
//CollisionShape mesh = new BvhTriangleMeshShape(m_indexVertexArrays2,true,true);
startTransform._origin = new IndexedVector3(2 * i - (numBoxes - 1), 2, -3);
//startTransform._origin = new IndexedVector3(2 * i - 5, 10, -3);
//LocalCreateRigidBody(1, startTransform, m_trimeshShape2);
LocalCreateRigidBody(1, startTransform, boxShape);
}
}
}
public void Evaluate(int SpreadMax)
{
IRigidBulletWorld inputWorld = this.worldInput[0];
SpreadMax = 1;
if (inputWorld != null)
{
this.persistedList.UpdateWorld(inputWorld);
if (this.chassisShape.IsConnected)
{
for (int i = 0; i < SpreadMax; i++)
{
if (this.doCreate[i])
{
RigidBodyPose initialPose = this.initialPoseInput.IsConnected ? this.initialPoseInput[i] : RigidBodyPose.Default;
RigidBodyProperties properties = this.initialProperties.IsConnected ? this.initialProperties[i] : RigidBodyProperties.Default;
ShapeCustomData shapeData = new ShapeCustomData();
DynamicShapeDefinitionBase chassisShapeDefinition = this.chassisShape[i];
CollisionShape chassisShape = chassisShapeDefinition.GetShape(shapeData);
shapeData.ShapeDef = chassisShapeDefinition;
RaycastVehicle vehicle;
CompoundShape compoundShape = new CompoundShape();
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compoundShape.AddChildShape(localTrans, chassisShape);
//Build mass for dynamic object
Vector3 localInertia = Vector3.Zero;
if (chassisShapeDefinition.Mass > 0.0f)
{
compoundShape.CalculateLocalInertia(chassisShapeDefinition.Mass, out localInertia);
}
Tuple <RigidBody, int> createBodyResult = inputWorld.CreateRigidBody(chassisShape, ref initialPose, ref properties, ref localInertia, chassisShapeDefinition.Mass, this.customString[i]);
RigidBody carChassis = createBodyResult.Item1;
RaycastVehicle.VehicleTuning tuning = new RaycastVehicle.VehicleTuning();
DefaultVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(inputWorld.World);
vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
carChassis.ActivationState = ActivationState.DisableDeactivation;
inputWorld.World.AddAction(vehicle);
int wheelCount = this.wheelConstruction.SliceCount;
//Add wheels
for (int j = 0; j < this.wheelConstruction[i].SliceCount; j++)
{
WheelConstructionProperties wcs = this.wheelConstruction[i][j];
Vector3 connectionPointCS0 = wcs.localPosition.ToBulletVector();
WheelInfo wheel = vehicle.AddWheel(connectionPointCS0, wcs.wheelDirection.ToBulletVector(), wcs.wheelAxis.ToBulletVector(), wcs.SuspensionRestLength, wcs.WheelRadius, tuning, wcs.isFrontWheel);
}
//Set Wheel Properties
WheelProperties wis = this.wheelInfoSettings[i] != null ? this.wheelInfoSettings[i] : new WheelProperties();
for (int j = 0; j < vehicle.NumWheels; j++)
{
WheelInfo wheel = vehicle.GetWheelInfo(j);
wheel.SuspensionStiffness = wis.SuspensionStiffness;
wheel.WheelsDampingRelaxation = wis.WheelsDampingRelaxation;
wheel.WheelsDampingCompression = wis.WheelsDampingCompression;
wheel.FrictionSlip = wis.FrictionSlip;
wheel.RollInfluence = wis.RollInfluence;
}
BodyCustomData bd = (BodyCustomData)carChassis.UserObject;
bd.Vehicle = vehicle;
this.persistedList.Append(createBodyResult.Item1, createBodyResult.Item2);
}
}
List <RigidBody> bodies = this.persistedList.Bodies;
this.vehicleOutput.SliceCount = bodies.Count;
this.chassisOutput.SliceCount = bodies.Count;
for (int i = 0; i < bodies.Count; i++)
{
BodyCustomData bd = (BodyCustomData)bodies[i].UserObject;
this.vehicleOutput[i] = bd.Vehicle;
this.chassisOutput[i] = bodies[i];
}
}
}
else
{
this.vehicleOutput.SliceCount = 0;
this.chassisOutput.SliceCount = 0;
}
}
public override void Evaluate(int SpreadMax)
{
for (int i = 0; i < SpreadMax; i++)
{
if (this.CanCreate(i))
{
wheelRadius = FwheelRadius[0];
wheelWidth = FwheelWidth[0];
CUBE_HALF_EXTENTS = FwheelDistance[0];
RaycastVehicle vehicle;
AbstractRigidShapeDefinition shapedef = this.FShapes[i];
ShapeCustomData sc = new ShapeCustomData();
sc.ShapeDef = shapedef;
CompoundShape compound = new CompoundShape();
CollisionShape chassisShape = shapedef.GetShape(sc);
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
float mass = shapedef.Mass;
bool isDynamic = (mass != 0.0f);
isFrontWheel = true;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
{
chassisShape.CalculateLocalInertia(mass, out localInertia);
}
Vector3D pos = this.FPosition[i];
Vector4D rot = this.FRotation[i];
DefaultMotionState ms = BulletUtils.CreateMotionState(pos.x, pos.y, pos.z, rot.x, rot.y, rot.z, rot.w);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, ms, compound, localInertia);
RigidBody carChassis = new RigidBody(rbInfo);
BodyCustomData bd = new BodyCustomData();
carChassis.UserObject = bd;
bd.Id = this.FWorld[0].GetNewBodyId();
bd.Custom = this.FCustom[i];
this.FWorld[0].Register(carChassis);
RaycastVehicle.VehicleTuning tuning = new RaycastVehicle.VehicleTuning();
VehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(this.FWorld[0].World);
vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
this.FWorld[0].World.AddAction(vehicle);
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
Vector3 connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), FconnectionHeight[0], 2 * CUBE_HALF_EXTENTS - wheelRadius);
WheelInfo a = vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), FconnectionHeight[0], 2 * CUBE_HALF_EXTENTS - wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);
isFrontWheel = false;
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), FconnectionHeight[0], -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), FconnectionHeight[0], -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, FsuspensionRestLength[0], wheelRadius, tuning, isFrontWheel);
for (i = 0; i < vehicle.NumWheels; i++)
{
WheelInfo wheel = vehicle.GetWheelInfo(i);
wheel.SuspensionStiffness = FsuspensionStiffness[0];
wheel.WheelDampingRelaxation = FDampingRelaxation[0];
wheel.WheelDampingCompression = FDampingCompression[0];
wheel.FrictionSlip = FwheelFriction[0];
wheel.RollInfluence = FrollInfluence[0];
wheel.MaxSuspensionTravelCm = FmaxSuspensionTravelCm[0];
wheel.MaxSuspensionForce = FmaxSuspensionForce[0];
}
FOutVehicle.SliceCount = 1;
FOutVehicle[0] = vehicle;
}
}
}
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;
}
protected override void OnInitializePhysics()
{
ManifoldPoint.ContactAdded += MyContactCallback;
SetupEmptyDynamicsWorld();
CreateGround();
//CompoundCollisionAlgorithm.CompoundChildShapePairCallback = MyCompoundChildShapeCallback;
var wo = WavefrontObj.Load("data/file.obj");
if (wo.Indices.Count == 0)
{
return;
}
var localScaling = new Vector3(6, 6, 6);
triangleMesh = CreateTriangleMesh(wo.Indices, wo.Vertices, localScaling);
// Convex hull approximation
ConvexHullShape convexShape = CreateHullApproximation(triangleMesh);
CollisionShapes.Add(convexShape);
float mass = 1.0f;
LocalCreateRigidBody(mass, Matrix.Translation(0, 2, 14), convexShape);
// Non-moving body
Vector3 convexDecompositionObjectOffset = new Vector3(10, 0, 0);
const bool useQuantization = true;
var concaveShape = new BvhTriangleMeshShape(triangleMesh, useQuantization);
CollisionShapes.Add(concaveShape);
LocalCreateRigidBody(0, Matrix.Translation(convexDecompositionObjectOffset), concaveShape);
var hacd = new Hacd()
{
VerticesPerConvexHull = 100,
CompacityWeight = 0.1,
VolumeWeight = 0,
// Recommended HACD parameters
NClusters = 2,
Concavity = 100,
AddExtraDistPoints = false,
AddFacesPoints = false,
AddNeighboursDistPoints = false
};
hacd.SetPoints(wo.Vertices);
hacd.SetTriangles(wo.Indices);
hacd.Compute();
hacd.Save("output.wrl", false);
// Generate convex result
var outputFile = new FileStream("file_convex.obj", FileMode.Create, FileAccess.Write);
var writer = new StreamWriter(outputFile);
var convexDecomposition = new ConvexDecomposition(writer)
{
LocalScaling = localScaling
};
for (int c = 0; c < hacd.NClusters; c++)
{
int nVertices = hacd.GetNPointsCH(c);
int trianglesLen = hacd.GetNTrianglesCH(c) * 3;
double[] points = new double[nVertices * 3];
long[] triangles = new long[trianglesLen];
hacd.GetCH(c, points, triangles);
if (trianglesLen == 0)
{
continue;
}
Vector3[] verticesArray = new Vector3[nVertices];
int vi3 = 0;
for (int vi = 0; vi < nVertices; vi++)
{
verticesArray[vi] = new Vector3(
(float)points[vi3], (float)points[vi3 + 1], (float)points[vi3 + 2]);
vi3 += 3;
}
int[] trianglesInt = new int[trianglesLen];
for (int ti = 0; ti < trianglesLen; ti++)
{
trianglesInt[ti] = (int)triangles[ti];
}
convexDecomposition.Result(verticesArray, trianglesInt);
}
writer.Dispose();
outputFile.Dispose();
// Combine convex shapes into a compound shape
var compound = new CompoundShape();
for (int i = 0; i < convexDecomposition.convexShapes.Count; i++)
{
Vector3 centroid = convexDecomposition.convexCentroids[i];
var convexShape2 = convexDecomposition.convexShapes[i];
Matrix trans = Matrix.Translation(centroid);
if (enableSat)
{
convexShape2.InitializePolyhedralFeatures();
}
CollisionShapes.Add(convexShape2);
compound.AddChildShape(trans, convexShape2);
LocalCreateRigidBody(1.0f, trans, convexShape2);
}
CollisionShapes.Add(compound);
#if true
mass = 10.0f;
var body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = 6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
convexDecompositionObjectOffset.Z = -6;
body2 = LocalCreateRigidBody(mass, Matrix.Translation(-convexDecompositionObjectOffset), compound);
body2.CollisionFlags |= CollisionFlags.CustomMaterialCallback;
#endif
}
