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