public override CollisionShape GetCollisionShape()
{
if (collisionShapePtr == null) {
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();
collisionShapePtr = cs;
for (int i = 0; i < colliders.Length; i++) {
CollisionShape 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 collisionShapePtr;
}
public static CompoundShape Create(GImpactMeshShape gImpactMesh, float depth)
{
CompoundShape colShape = new CompoundShape();
using (var cb = new MyInternalTriangleIndexCallback(colShape, gImpactMesh, depth))
{
Vector3 aabbMin, aabbMax;
gImpactMesh.GetAabb(Matrix.Identity, out aabbMin, out aabbMax);
gImpactMesh.MeshInterface.InternalProcessAllTriangles(cb, aabbMin, aabbMax);
}
return colShape;
}
static CollisionShape CreateShape(float size)
{
CompoundShape shape = new CompoundShape();
shape.AddChildShape(Matrix4.CreateTranslation(-size, 0.0f, 0.0f), new StaticPlaneShape(new Vector3(1.0f, 0.0f, 0.0f), 0.0f));
shape.AddChildShape(Matrix4.CreateTranslation(size, 0.0f, 0.0f), new StaticPlaneShape(new Vector3(-1.0f, 0.0f, 0.0f), 0.0f));
shape.AddChildShape(Matrix4.CreateTranslation(0.0f, -size, 0.0f), new StaticPlaneShape(new Vector3(0.0f, 1.0f, 0.0f), 0.0f));
shape.AddChildShape(Matrix4.CreateTranslation(0.0f, size, 0.0f), new StaticPlaneShape(new Vector3(0.0f, -1.0f, 0.0f), 0.0f));
shape.AddChildShape(Matrix4.CreateTranslation(0.0f, 0.0f, -size), new StaticPlaneShape(new Vector3(0.0f, 0.0f, 1.0f), 0.0f));
shape.AddChildShape(Matrix4.CreateTranslation(0.0f, 0.0f, size), new StaticPlaneShape(new Vector3(0.0f, 0.0f, -1.0f), 0.0f));
return shape;
}
CollisionShape _AddFlipperCylinders(FlipperBehavior flipper)
{
float r1 = flipper.data.BaseRadius;
float r2 = flipper.data.EndRadius;
float h = flipper.data.Height;
float l = flipper.data.FlipperRadius;
var hh = h * 0.5f; // half height
var cs = new BulletSharp.CompoundShape();
cs.AddChildShape(
Matrix.Translation(0, 0, hh),
new CylinderShapeZ(r1, r1, hh));
cs.AddChildShape(
Matrix.Translation(0, -l, hh),
new CylinderShapeZ(r2, r2, hh));
// we can't add Triangle Mesh Shape to Compound Shape. Add one or two boxes
float hbl = new Vector2(l, r1 - r2).magnitude * 0.5f;
Vector3 n = new Vector3(l, r1 - r2, 0); n.Normalize();
Vector3 beg = new Vector3(0, 0, hh) + n * (r1 - r2);
Vector3 beg2 = new Vector3(-beg.X, beg.Y, beg.Z);
Vector3 end = new Vector3(0, -l, hh);
float angle = math.atan2(n.Y, n.X);
bool onlyFront = true;
bool rev = (flipper.data.StartAngle < 0 | flipper.data.StartAngle > 180);
if (!onlyFront || rev)
{
cs.AddChildShape(
Matrix.RotationZ(-angle) *
Matrix.Translation((beg + end) * 0.5f),
new BoxShape(Mathf.Min(r1, r2), hbl, hh));
}
if (!onlyFront || !rev)
{
cs.AddChildShape(
Matrix.RotationZ(angle) *
Matrix.Translation((beg2 + end) * 0.5f),
new BoxShape(Mathf.Min(r1, r2), hbl, hh));
}
return(cs);
}
/// <summary>
/// Creates a CollisionShape from the ShapeComponents of the given thing. If the shape already exists, we'll just return that instead.
/// </summary>
public CollisionShape CreateAndRegisterShape(LThing thing, ThingDefinition def)
{
CollisionShape shape;
if (!Shapes.TryGetValue(thing.Name, out shape)) {
// create the shape
bool forceCompound = def.GetBoolProperty("forcecompound", false);
// if we only have one shape we don't have to do as much
if (thing.ShapeComponents.Count == 1) {
// force us to use a compound shape?
if (forceCompound) {
CompoundShape comp = new CompoundShape();
comp.AddChildShape(thing.ShapeComponents[0].Transform, CreateShapeForComponent(thing.ShapeComponents[0]));
shape = comp;
}
// one component, no compound is the easiest
else {
shape = CreateShapeForComponent(thing.ShapeComponents[0]);
}
}
// otherwise, make all of our shapes and stick them in a compound shape
else {
CompoundShape comp = new CompoundShape();
foreach (ShapeComponent component in thing.ShapeComponents) {
comp.AddChildShape(component.Transform, CreateShapeForComponent(component));
}
shape = comp;
}
// then put the shape in our dictionary
Shapes.Add(thing.Name, shape);
}
return shape;
}
public Model(params GeometricPrimitive[] Primitives)
{
this.Primitives = Primitives.ToList();
CollisionShape compound = null;
CastShadow = true;
List<Vector3> points = new List<Vector3>();
if (Primitives.Length > 1)
{
compound = new CompoundShape(true);
foreach (GeometricPrimitive prim in Primitives)
{
(compound as CompoundShape).AddChildShape(Matrix.Identity, prim.CollisionShape);
points.AddRange(prim.GeometryData.Positions);
}
}
else
{
compound = Primitives[0].CollisionShape;
points.AddRange(Primitives[0].GeometryData.Positions);
}
mass = 10;
body = new RigidBody(new RigidBodyConstructionInfo(mass, new DefaultMotionState(), compound, compound.CalculateLocalInertia(mass)));
bbox = BoundingBox.FromPoints(points.ToArray());
}
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1)
{
Debug.WriteLine("MyTickCallBack");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
RigidBodyImp myRb;
//Debug.WriteLine("numManifolds: " + numManifolds);
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
CollisionObject obA = (CollisionObject) contactManifold.Body0;
CollisionObject obB = (CollisionObject) contactManifold.Body1;
// Debug.WriteLine(numContacts);
var pnA = obA.UserObject;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
}
}
}
}*/
public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape/*, float3 intertia*/)
{
// Use bullet to do what needs to be done:
var btMatrix = Matrix.RotationX(orientation.x)
* Matrix.RotationY(orientation.y)
* Matrix.RotationZ(orientation.z)
* Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z);
var btMotionState = new DefaultMotionState(btMatrix);
var shapeType = colShape.GetType().ToString();
CollisionShape btColShape;
var isStatic = false;
switch (shapeType)
{
//Primitives
case "Fusee.Engine.BoxShapeImp":
var box = (BoxShapeImp) colShape;
var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents);
btColShape = new BoxShape(btBoxHalfExtents);
break;
case "Fusee.Engine.CapsuleShapeImp":
var capsule = (CapsuleShapeImp) colShape;
btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight);
break;
case "Fusee.Engine.ConeShapeImp":
var cone = (ConeShapeImp) colShape;
btColShape = new ConeShape(cone.Radius, cone.Height);
break;
case "Fusee.Engine.CylinderShapeImp":
var cylinider = (CylinderShapeImp) colShape;
var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents);
btColShape = new CylinderShape(btCylinderHalfExtents);
break;
case "Fusee.Engine.MultiSphereShapeImp":
var multiSphere = (MultiSphereShapeImp) colShape;
var btPositions = new Vector3[multiSphere.SphereCount];
var btRadi = new float[multiSphere.SphereCount];
for (int i = 0; i < multiSphere.SphereCount; i++)
{
var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i));
btPositions[i] = pos;
btRadi[i] = multiSphere.GetSphereRadius(i);
}
btColShape = new MultiSphereShape(btPositions, btRadi);
break;
case "Fusee.Engine.SphereShapeImp":
var sphere = (SphereShapeImp) colShape;
var btRadius = sphere.Radius;
btColShape = new SphereShape(btRadius);
break;
//Misc
case "Fusee.Engine.CompoundShapeImp":
var compShape = (CompoundShapeImp) colShape;
btColShape = new CompoundShape(true);
btColShape = compShape.BtCompoundShape;
break;
case "Fusee.Engine.EmptyShapeImp":
btColShape = new EmptyShape();
break;
//Meshes
case "Fusee.Engine.ConvexHullShapeImp":
var convHull = (ConvexHullShapeImp) colShape;
var btPoints= new Vector3[convHull.GetNumPoints()];
for (int i = 0; i < convHull.GetNumPoints(); i++)
{
var point = convHull.GetScaledPoint(i);
btPoints[i] = Translater.Float3ToBtVector3(point);
}
btColShape = new ConvexHullShape(btPoints);
//btColShape.LocalScaling = new Vector3(3,3,3);
break;
case "Fusee.Engine.StaticPlaneShapeImp":
var staticPlane = (StaticPlaneShapeImp) colShape;
Debug.WriteLine("staticplane: " + staticPlane.Margin);
var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal);
btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant);
isStatic = true;
//btColShape.Margin = 0.04f;
//Debug.WriteLine("btColshape" + btColShape.Margin);
break;
case "Fusee.Engine.GImpactMeshShapeImp":
var gImpMesh = (GImpactMeshShapeImp)colShape;
gImpMesh.BtGImpactMeshShape.UpdateBound();
var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface);
btGimp.UpdateBound();
btColShape = btGimp;
break;
//Default
default:
Debug.WriteLine("defaultImp");
btColShape = new EmptyShape();
break;
}
var btLocalInertia = btColShape.CalculateLocalInertia(mass);
// btLocalInertia *= (10.0f*10);
RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape,
btLocalInertia);
var btRigidBody = new RigidBody(btRbcInfo);
btRigidBody.Restitution = 0.2f;
btRigidBody.Friction = 0.2f;
btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback;
BtWorld.AddRigidBody(btRigidBody);
btRbcInfo.Dispose();
var retval = new RigidBodyImp();
retval._rbi = btRigidBody;
btRigidBody.UserObject = retval;
return retval;
}
public ICompoundShapeImp AddCompoundShape(bool enableDynamicAabbTree)
{
var btCompoundShape = new CompoundShape(enableDynamicAabbTree);
BtCollisionShapes.Add(btCompoundShape);
var retval = new CompoundShapeImp();
retval.BtCompoundShape = btCompoundShape;
btCompoundShape.UserObject = retval;
return retval;
}
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);
}
void Create_RbUpStack(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 = new CollisionShape[]{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]);
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
IsDebugDrawEnabled = true;
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI/4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2/L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
}
else
{
pivotInB = pivotInA;
}
Vector3 axisInB;
if (body1 != null)
{
Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
axisInB = Vector3.TransformCoordinate(axisInA, transform);
}
else
{
axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
}
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
protected override void OnInitializePhysics()
{
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
const int NumVertsX = 20;
const int NumVertsY = 20;
const int totalVerts = NumVertsX * NumVertsY;
const int totalTriangles = 2 * (NumVertsX - 1) * (NumVertsY - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts);
mesh.NumTriangles = totalTriangles;
mesh.NumVertices = totalVerts;
mesh.TriangleIndexStride = 3 * sizeof(int);
mesh.VertexStride = Vector3.SizeInBytes;
using (var indicesStream = mesh.GetTriangleStream())
{
var indices = new BinaryWriter(indicesStream);
for (i = 0; i < NumVertsX - 1; i++)
{
for (int j = 0; j < NumVertsY - 1; j++)
{
indices.Write(j * NumVertsX + i);
indices.Write(j * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write(j * NumVertsX + i);
indices.Write((j + 1) * NumVertsX + i + 1);
indices.Write((j + 1) * NumVertsX + i);
}
}
indices.Dispose();
}
using (var vertexStream = mesh.GetVertexStream())
{
var vertices = new BinaryWriter(vertexStream);
for (i = 0; i < NumVertsX; i++)
{
for (int j = 0; j < NumVertsY; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
vertices.Write((i - NumVertsX * 0.5f) * scale);
vertices.Write(height);
vertices.Write((j - NumVertsY * 0.5f) * scale);
}
}
vertices.Dispose();
}
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.PhyFloat;
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.PhyFloat)
{
// heightScale isn't applied internally for float data
height = reader.ReadSingle();
}
else if (scalarType == PhyScalarType.PhyUChar)
{
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
VehicleTuning tuning = new VehicleTuning();
IVehicleRaycaster 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;
bool isFrontWheel = true;
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
BulletSharp.Math.Vector3 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);
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;
}
public CompoundShape CreateCompoundShape()
{
CompoundShape shape = new CompoundShape();
_allocatedCollisionShapes.Add(shape);
return shape;
}
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];
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
}
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();
}
}
//private readonly static Vector3 _redColor = new Vector3(1, 0, 0);
public MyInternalTriangleIndexCallback(CompoundShape collisionShape, GImpactMeshShape meshShape, Scalar depth)
{
_collisionShape = collisionShape;
_depth = depth;
_meshShape = meshShape;
}
public MyInternalTriangleIndexCallback(CompoundShape colShape, GImpactMeshShape meshShape, float depth)
{
_colShape = colShape;
_depth = depth;
_meshShape = meshShape;
}
public void GenerateBody()
{
if (body != null)
body.Dispose();
CollisionShape compound = null;
if (Primitives.Count > 1)
{
compound = new CompoundShape(true);
foreach (GeometricPrimitive prim in Primitives)
{
(compound as CompoundShape).AddChildShape((Matrix.Identity), prim.CollisionShape);
}
}
else
{
compound = Primitives[0].CollisionShape;
}
mass = 10;
body = new RigidBody(new RigidBodyConstructionInfo(mass, new DefaultMotionState(), compound, compound.CalculateLocalInertia(mass)));
}
public void GImpactVsCompoundShape(CollisionObjectWrapper body0Wrap, CollisionObjectWrapper body1Wrap,
GImpactShapeInterface shape0, CompoundShape shape1, bool swapped)
{
btGImpactCollisionAlgorithm_gimpact_vs_compoundshape(_native, body0Wrap._native,
body1Wrap._native, shape0._native, shape1._native, swapped);
}
override protected BulletSharp.CollisionShape createShape ()
{
BulletSharp.CompoundShape compound = new BulletSharp.CompoundShape (enableDynamicAABBTree);
return compound;
}
public MyInternalTriangleIndexCallback(CompoundShape colShape, GImpactMeshShape meshShape, float depth)
{
_colShape = colShape;
_depth = depth;
_meshShape = meshShape;
}
float wheelFriction = 1000; //BT_LARGE_FLOAT;
#endregion Fields
#region Methods
public override void Evaluate(int SpreadMax)
{
for (int i = 0; i < SpreadMax; i++)
{
if (this.CanCreate(i))
{
RaycastVehicle vehicle;
AbstractRigidShapeDefinition shapedef = this.FShapes[i];
ShapeCustomData sc = new ShapeCustomData();
sc.ShapeDef = shapedef;
CompoundShape compound = new CompoundShape();
//List<AbstractRigidShapeDefinition> children = new List<AbstractRigidShapeDefinition>();
CollisionShape chassisShape = shapedef.GetShape(sc);
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
float mass = shapedef.Mass;
bool isDynamic = (mass != 0.0f);
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);
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;
}
FOutVehicle.SliceCount = 1;
FOutVehicle[0] = vehicle;
}
}
}
public void GImpactVsCompoundShape(CollisionObjectWrapper body0Wrap, CollisionObjectWrapper body1Wrap,
GImpactShapeInterface shape0, CompoundShape shape1, bool swapped)
{
btGImpactCollisionAlgorithm_gimpact_vs_compoundshape(Native, body0Wrap.Native,
body1Wrap.Native, shape0.Native, shape1.Native, swapped);
}
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
}
