public override void InternalProcessTriangleIndex(ref Vector3 vertex0, ref Vector3 vertex1, ref Vector3 vertex2, int partId, int triangleIndex) { Vector3 scale = _meshShape.LocalScaling; Vector3 v0 = vertex0 * scale; Vector3 v1 = vertex1 * scale; Vector3 v2 = vertex2 * scale; Vector3 centroid = (v0 + v1 + v2) / 3; Vector3 normal = (v1 - v0).Cross(v2 - v0); normal.Normalize(); Vector3 rayFrom = centroid; Vector3 rayTo = centroid - normal * _depth; MyCallback cb = new MyCallback(ref rayFrom, ref rayTo, partId, triangleIndex); _meshShape.ProcessAllTrianglesRayRef(cb, ref rayFrom, ref rayTo); if (cb.HitFraction < 1) { rayTo = Vector3.Lerp(cb.From, cb.To, cb.HitFraction); //rayTo = cb.From; //gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0)); } BuSimplex1To4 tet = new BuSimplex1To4(v0, v1, v2, rayTo); _colShape.AddChildShape(Matrix.Identity, tet); }
public override void InternalProcessTriangleIndex(ref Vector3 vertex0, ref Vector3 vertex1, ref Vector3 vertex2, int partId, int triangleIndex) { Vector3 scale = _meshShape.LocalScaling; Vector3 v0 = vertex0 * scale; Vector3 v1 = vertex1 * scale; Vector3 v2 = vertex2 * scale; Vector3 centroid = (v0 + v1 + v2) / 3; Vector3 normal = (v1 - v0).Cross(v2 - v0); normal.Normalize(); Vector3 rayFrom = centroid; Vector3 rayTo = centroid - normal * _depth; using (var cb = new MyCallback(ref rayFrom, ref rayTo, partId, triangleIndex)) { _meshShape.ProcessAllTrianglesRayRef(cb, ref rayFrom, ref rayTo); if (cb.HitFraction < 1) { rayTo = Vector3.Lerp(cb.From, cb.To, cb.HitFraction); //rayTo = cb.From; //Vector3 to = centroid + normal; //debugDraw.DrawLine(ref centroid, ref to, ref _redColor); } } var triangle = new BuSimplex1To4(v0, v1, v2, rayTo); _collisionShape.AddChildShape(Matrix.Identity, triangle); }
public unsafe static void AddChildShape(this CompoundShape obj, ref OpenTK.Matrix4 localTransform, CollisionShape shape) { fixed(OpenTK.Matrix4 *localTransformPtr = &localTransform) { obj.AddChildShape(ref *(BulletSharp.Math.Matrix *)localTransformPtr, 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); }
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.TransformPoint(up); origin = colliders[i].transform.TransformPoint(origin); forward = colliders[i].transform.TransformPoint(forward); //to compound collider up = transform.InverseTransformPoint(up); origin = transform.InverseTransformPoint(origin); forward = transform.InverseTransformPoint(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); } } return collisionShapePtr; }
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; }
/// <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; }
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(); } }
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 }
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; }
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; } } }
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 }