private void AddVertexGraph(VertexGraph graph) { Individual = graph.Vertices.Select(vertex => { var mass = 10000; var motionState = new DefaultMotionState(); var collisionShape = new BoxShape(1); var info = new RigidBodyConstructionInfo(mass, motionState, collisionShape); var rigidBody = new VertexBoundRigidBody(vertex, info); return rigidBody; }).ToList(); foreach (var body in Individual) { // Select the 3 nearest vertices, excluding this one var nearest = Individual.OrderBy(a => a.Binding.DistanceTo(body.Binding)) .Where(a => a != body) .Take(3); foreach (var other in nearest) { // TODO: What are these matrices supposed to be? var frameInA = body.MotionState.WorldTransform; var frameInB = other.MotionState.WorldTransform; // TODO: How do you specify the spring's springiness? var constraint = new Generic6DofSpringConstraint(body, other, frameInA, frameInB, true); // TODO: Now how do I apply this to the bodies? body.AddConstraintRef(constraint); other.AddConstraintRef(constraint); } } }
public unsafe static void GetLocalInertia(this RigidBodyConstructionInfo obj, out OpenTK.Vector3 value) { fixed(OpenTK.Vector3 *valuePtr = &value) { *(BulletSharp.Math.Vector3 *)valuePtr = obj.LocalInertia; } }
public unsafe static void SetStartWorldTransform(this RigidBodyConstructionInfo obj, ref OpenTK.Matrix4 value) { fixed(OpenTK.Matrix4 *valuePtr = &value) { obj.StartWorldTransform = *(BulletSharp.Math.Matrix *)valuePtr; } }
public Physics() { // collision configuration contains default setup for memory, collision setup collisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(collisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf); World.Gravity = new Vector3(0, -10, 0); CollisionShapes = new List<CollisionShape>(); // create the ground CollisionShape groundShape = new BoxShape(50, 1, 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; CollisionShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float start_x = StartPosX - ArraySizeX / 2; float start_y = StartPosY; float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.CreateTranslation( new Vector3( 2*i + start_x, 2*k + start_y, 2*j + start_z ) ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } }
public void SetUpBulletPhysicsBody(float mass, BulletSharp.MotionState motionState, BulletSharp.CollisionShape collisionShape, Vector3 localInertia) { BulletSharpPhysics.RigidBodyConstructionInfo rbInfo = new BulletSharpPhysics.RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia); RigidBody = new BulletSharpPhysics.RigidBody(rbInfo); bulletPhysics.World.AddRigidBody(RigidBody, GetCollisionFlags(), GetCollisionMask()); }
static RaycastVehicle() { using (var ci = new RigidBodyConstructionInfo(0, null, null)) { fixedBody = new RigidBody(ci); fixedBody.SetMassProps(0, Vector3.Zero); } }
public void SetUpBulletPhysicsBody(float mass, BulletSharp.MotionState motionState, BulletSharp.CollisionShape collisionShape, Vector3 localInertia) { BulletSharpPhysics.RigidBodyConstructionInfo rbInfo = new BulletSharpPhysics.RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia); RigidBody = new BulletSharpPhysics.RigidBody(rbInfo); bulletPhysics.World.AddRigidBody(RigidBody,GetCollisionFlags(),GetCollisionMask()); }
/// <summary> /// Applies friction, bounciness, angular damping, and linear damping /// </summary> public void ApplyMaterial(RigidBodyConstructionInfo info, string material) { PhysicsMaterial mat = GetMaterial(material); info.Friction = mat.Friction; info.Restitution = mat.Bounciness; info.AngularDamping = mat.AngularDamping; info.LinearDamping = mat.LinearDamping; }
public RigidBody(RigidBodyConstructionInfo constructionInfo) : base(ConstructionInfo.Null) { IntPtr native = btRigidBody_new(constructionInfo.Native); InitializeCollisionObject(native); _collisionShape = constructionInfo.CollisionShape; _motionState = constructionInfo.MotionState; }
public static RigidBody GetFixedBody() { if (_fixedBody == null) { using (var cinfo = new RigidBodyConstructionInfo(0, null, null)) { _fixedBody = new RigidBody(cinfo); _fixedBody.SetMassProps(0, Vector3.Zero); } } return(_fixedBody); }
public void AlignmentTest() { const float mass = 1.0f; for (int i = 0; i < 100; i++) { // RigidBodyConstructionInfo without the optional localInertia parameter will // cause the default value to be passed, which is not aligned to 16 bytes in C++/CLI. // If BulletSharp doesn't explicitly pass an aligned value and SSE is used, // an AccessViolationException occurs. var info = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), boxShape); // , Vector3.Zero info.Dispose(); } }
RigidBody CreateBody(float mass, CollisionShape shape, Vector3 offset) { using (var info = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), shape, Vector3.Zero)) { if (mass != 0.0f) { info.LocalInertia = info.CollisionShape.CalculateLocalInertia(mass); } var collisionObject = new RigidBody(info); collisionObject.Translate(offset); world.AddRigidBody(collisionObject); return collisionObject; } }
protected RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape) { bool isDynamic = (mass != 0.0f); Vector3 localInertia = Vector3.Zero; if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia); body = new RigidBody(rbInfo); return body; }
static RigidBody CreateBody(float mass, CollisionShape shape, Vector3 offset) { var constInfo = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), shape, Vector3.Zero); if (mass != 0.0f) { constInfo.LocalInertia = constInfo.CollisionShape.CalculateLocalInertia(mass); } var collisionObject = new RigidBody(constInfo); collisionObject.Translate(offset); world.AddRigidBody(collisionObject); AddToDisposeQueue(constInfo); AddToDisposeQueue(constInfo.MotionState); AddToDisposeQueue(collisionObject); AddToDisposeQueue(shape); return collisionObject; }
BulletSharp.RigidBody createRigidBody (BulletSharp.CollisionShape shape, UnityEngine.Transform transform, float mass) { Matrix4x4 unityMatrix = Matrix4x4.TRS (transform.position, transform.rotation, UnityEngine.Vector3.one); BulletSharp.Matrix bulletMatrix = new BulletSharp.Matrix ( unityMatrix.m00, unityMatrix.m10, unityMatrix.m20, unityMatrix.m30, unityMatrix.m01, unityMatrix.m11, unityMatrix.m21, unityMatrix.m31, unityMatrix.m02, unityMatrix.m12, unityMatrix.m22, unityMatrix.m32, unityMatrix.m03, unityMatrix.m13, unityMatrix.m23, unityMatrix.m33); BulletSharp.MotionState motionState = new BulletSharp.DefaultMotionState (bulletMatrix); BulletSharp.Vector3 inertia = new BulletSharp.Vector3 (0f, 0f, 0f); if (!Mathf.Approximately (mass, 0f)) { shape.CalculateLocalInertia (mass, out inertia); } BulletSharp.RigidBodyConstructionInfo myRigidBodyCI = new BulletSharp.RigidBodyConstructionInfo (mass, motionState, shape, inertia); BulletSharp.RigidBody myRigidBody = new BulletSharp.RigidBody (myRigidBodyCI); myRigidBodyCI.Dispose (); return myRigidBody; }
public override RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape) { //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.0f); Vector3 localInertia = Vector3.Zero; if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, null, shape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); body.ContactProcessingThreshold = defaultContactProcessingThreshold; body.WorldTransform = startTransform; World.AddRigidBody(body); return body; }
public RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape) { //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.0f); Vector3 localInertia = Vector3.Zero; if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBody body; using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia)) { body = new RigidBody(rbInfo); } ownerWorld.AddRigidBody(body); return body; }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create the ground CollisionShape groundShape = new BoxShape(20, 50, 10); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.RotationAxis(new Vector3(0, 0, 1), (float)Math.PI * 0.03f) * Matrix.Translation(0, -50, 0), groundShape); ground.Friction = 1; ground.RollingFriction = 1; ground.UserObject = "Ground"; groundShape = new BoxShape(100, 50, 100); CollisionShapes.Add(groundShape); ground = LocalCreateRigidBody(0, Matrix.Translation(0, -54, 0), groundShape); ground.Friction = 1; ground.RollingFriction = 1; ground.UserObject = "Ground"; // create a few dynamic rigidbodies CollisionShape[] colShapes = { new SphereShape(1), new CapsuleShape(0.5f,1), new CapsuleShapeX(0.5f,1), new CapsuleShapeZ(0.5f,1), new ConeShape(0.5f,1), new ConeShapeX(0.5f,1), new ConeShapeZ(0.5f,1), new CylinderShape(new Vector3(0.5f,1,0.5f)), new CylinderShapeX(new Vector3(1,0.5f,0.5f)), new CylinderShapeZ(new Vector3(0.5f,0.5f,1)), }; foreach (var collisionShape in colShapes) { CollisionShapes.Add(collisionShape); } const float mass = 1.0f; CollisionShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); var rbInfo = new RigidBodyConstructionInfo(mass, null, null, localInertia); const float startX = StartPosX - ArraySizeX / 2; const float startY = StartPosY; const float startZ = StartPosZ - ArraySizeZ / 2; int shapeIndex = 0; for (int k = 0; k < ArraySizeY; k++) { for (int i = 0; i < ArraySizeX; i++) { for (int j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 2 * i + startX, 2 * k + startY + 20, 2 * j + startZ ); shapeIndex++; // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects rbInfo.MotionState = new DefaultMotionState(startTransform); rbInfo.CollisionShape = colShapes[shapeIndex % colShapes.Length]; RigidBody body = new RigidBody(rbInfo); body.Friction = 1; body.RollingFriction = 0.3f; body.SetAnisotropicFriction(colShape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.RollingFriction); World.AddRigidBody(body); } } } rbInfo.Dispose(); }
public static OpenTK.Vector3 GetLocalInertia(this RigidBodyConstructionInfo obj) { OpenTK.Vector3 value; GetLocalInertia(obj, out value); return(value); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create the ground BoxShape groundShape = new BoxShape(50, 1, 50); //groundShape.InitializePolyhedralFeatures(); //CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies const float mass = 1.0f; BoxShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); const float startX = StartPosX - ArraySizeX / 2; const float startY = StartPosY; const float startZ = StartPosZ - ArraySizeZ / 2; RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia); int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 2 * i + startX, 2 * k + startY, 2 * j + startZ ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects rbInfo.MotionState = new DefaultMotionState(startTransform); RigidBody body = new RigidBody(rbInfo); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } rbInfo.Dispose(); }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); Solver = new MultiBodyConstraintSolver(); World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create a few basic rigid bodies BoxShape groundShape = new BoxShape(50, 50, 50); //groundShape.InitializePolyhedralFeatures(); //CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies const float mass = 1.0f; BoxShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); const float start_x = StartPosX - ArraySizeX / 2; const float start_y = StartPosY; const float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 3 * i + start_x, 3 * k + start_y, 3 * j + start_z ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia)) { var body = new RigidBody(rbInfo); World.AddRigidBody(body); } } } } var settings = new MultiBodySettings() { BasePosition = new Vector3(60, 29.5f, -2) * Scaling, CanSleep = true, CreateConstraints = true, DisableParentCollision = true, // the self-collision has conflicting/non-resolvable contact normals IsFixedBase = false, NumLinks = 2, UsePrismatic = true }; var multiBodyA = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.NumLinks = 10; settings.BasePosition = new Vector3(0, 29.5f, -settings.NumLinks * 4); settings.IsFixedBase = true; settings.UsePrismatic = false; var multiBodyB = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.BasePosition = new Vector3(-20 * Scaling, 29.5f * Scaling, -settings.NumLinks * 4 * Scaling); settings.IsFixedBase = false; var multiBodyC = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); settings.BasePosition = new Vector3(-20, 9.5f, -settings.NumLinks * 4); settings.IsFixedBase = true; settings.UsePrismatic = true; settings.DisableParentCollision = true; var multiBodyPrim = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings); }
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1) { Debug.WriteLine("MyTickCallBack"); int numManifolds = BtWorld.Dispatcher.NumManifolds; RigidBodyImp myRb; //Debug.WriteLine("numManifolds: " + numManifolds); for (int i = 0; i < numManifolds; i++) { PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i); int numContacts = contactManifold.NumContacts; if (numContacts > 0) { CollisionObject obA = (CollisionObject) contactManifold.Body0; CollisionObject obB = (CollisionObject) contactManifold.Body1; // Debug.WriteLine(numContacts); var pnA = obA.UserObject; for (int j = 0; j < numContacts; j++) { ManifoldPoint pt = contactManifold.GetContactPoint(j); } } } }*/ public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape/*, float3 intertia*/) { // Use bullet to do what needs to be done: var btMatrix = Matrix.RotationX(orientation.x) * Matrix.RotationY(orientation.y) * Matrix.RotationZ(orientation.z) * Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z); var btMotionState = new DefaultMotionState(btMatrix); var shapeType = colShape.GetType().ToString(); CollisionShape btColShape; var isStatic = false; switch (shapeType) { //Primitives case "Fusee.Engine.BoxShapeImp": var box = (BoxShapeImp) colShape; var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents); btColShape = new BoxShape(btBoxHalfExtents); break; case "Fusee.Engine.CapsuleShapeImp": var capsule = (CapsuleShapeImp) colShape; btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight); break; case "Fusee.Engine.ConeShapeImp": var cone = (ConeShapeImp) colShape; btColShape = new ConeShape(cone.Radius, cone.Height); break; case "Fusee.Engine.CylinderShapeImp": var cylinider = (CylinderShapeImp) colShape; var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents); btColShape = new CylinderShape(btCylinderHalfExtents); break; case "Fusee.Engine.MultiSphereShapeImp": var multiSphere = (MultiSphereShapeImp) colShape; var btPositions = new Vector3[multiSphere.SphereCount]; var btRadi = new float[multiSphere.SphereCount]; for (int i = 0; i < multiSphere.SphereCount; i++) { var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i)); btPositions[i] = pos; btRadi[i] = multiSphere.GetSphereRadius(i); } btColShape = new MultiSphereShape(btPositions, btRadi); break; case "Fusee.Engine.SphereShapeImp": var sphere = (SphereShapeImp) colShape; var btRadius = sphere.Radius; btColShape = new SphereShape(btRadius); break; //Misc case "Fusee.Engine.CompoundShapeImp": var compShape = (CompoundShapeImp) colShape; btColShape = new CompoundShape(true); btColShape = compShape.BtCompoundShape; break; case "Fusee.Engine.EmptyShapeImp": btColShape = new EmptyShape(); break; //Meshes case "Fusee.Engine.ConvexHullShapeImp": var convHull = (ConvexHullShapeImp) colShape; var btPoints= new Vector3[convHull.GetNumPoints()]; for (int i = 0; i < convHull.GetNumPoints(); i++) { var point = convHull.GetScaledPoint(i); btPoints[i] = Translater.Float3ToBtVector3(point); } btColShape = new ConvexHullShape(btPoints); //btColShape.LocalScaling = new Vector3(3,3,3); break; case "Fusee.Engine.StaticPlaneShapeImp": var staticPlane = (StaticPlaneShapeImp) colShape; Debug.WriteLine("staticplane: " + staticPlane.Margin); var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal); btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant); isStatic = true; //btColShape.Margin = 0.04f; //Debug.WriteLine("btColshape" + btColShape.Margin); break; case "Fusee.Engine.GImpactMeshShapeImp": var gImpMesh = (GImpactMeshShapeImp)colShape; gImpMesh.BtGImpactMeshShape.UpdateBound(); var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface); btGimp.UpdateBound(); btColShape = btGimp; break; //Default default: Debug.WriteLine("defaultImp"); btColShape = new EmptyShape(); break; } var btLocalInertia = btColShape.CalculateLocalInertia(mass); // btLocalInertia *= (10.0f*10); RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape, btLocalInertia); var btRigidBody = new RigidBody(btRbcInfo); btRigidBody.Restitution = 0.2f; btRigidBody.Friction = 0.2f; btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback; BtWorld.AddRigidBody(btRigidBody); btRbcInfo.Dispose(); var retval = new RigidBodyImp(); retval._rbi = btRigidBody; btRigidBody.UserObject = retval; return retval; }
public static void SetStartWorldTransform(this RigidBodyConstructionInfo obj, OpenTK.Matrix4 value) { SetStartWorldTransform(obj, ref value); }
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; } } }
void AddBoxes() { // create a few dynamic rigidbodies const float mass = 1.0f; BoxShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); const float startX = StartPosX - ArraySizeX / 2; const float startY = StartPosY; const float startZ = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 3 * i + startX, 3 * k + startY, 3 * j + startZ ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia)) { var body = new RigidBody(rbInfo); World.AddRigidBody(body); } } } } }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); // Use the default collision dispatcher. For parallel processing you can use a diffent dispatcher. Dispatcher = new CollisionDispatcher(CollisionConf); VoronoiSimplexSolver simplex = new VoronoiSimplexSolver(); MinkowskiPenetrationDepthSolver pdSolver = new MinkowskiPenetrationDepthSolver(); Convex2DConvex2DAlgorithm.CreateFunc convexAlgo2d = new Convex2DConvex2DAlgorithm.CreateFunc(simplex, pdSolver); Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2d); Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2d); Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Box2DShape, convexAlgo2d); Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Box2DShape, new Box2DBox2DCollisionAlgorithm.CreateFunc()); Broadphase = new DbvtBroadphase(); // the default constraint solver. Solver = new SequentialImpulseConstraintSolver(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf); World.Gravity = new Vector3(0, -10, 0); // create a few basic rigid bodies CollisionShape groundShape = new BoxShape(150, 7, 150); CollisionShapes.Add(groundShape); RigidBody ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies // Re-using the same collision is better for memory usage and performance float u = 0.96f; Vector3[] points = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) }; ConvexShape childShape0 = new BoxShape(1, 1, Depth); ConvexShape colShape = new Convex2DShape(childShape0); ConvexShape childShape1 = new ConvexHullShape(points); ConvexShape colShape2 = new Convex2DShape(childShape1); ConvexShape childShape2 = new CylinderShapeZ(1, 1, Depth); ConvexShape colShape3 = new Convex2DShape(childShape2); CollisionShapes.Add(colShape); CollisionShapes.Add(colShape2); CollisionShapes.Add(colShape3); CollisionShapes.Add(childShape0); CollisionShapes.Add(childShape1); CollisionShapes.Add(childShape2); colShape.Margin = 0.03f; float mass = 1.0f; Vector3 localInertia = colShape.CalculateLocalInertia(mass); Matrix startTransform; Vector3 x = new Vector3(-ArraySizeX, 8, -20); Vector3 y = Vector3.Zero; Vector3 deltaX = new Vector3(1, 2, 0); Vector3 deltaY = new Vector3(2, 0, 0); int i, j; for (i = 0; i < ArraySizeY; i++) { y = x; for (j = 0; j < ArraySizeX; j++) { startTransform = Matrix.Translation(y - new Vector3(-10, 0, 0)); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo; switch (j % 3) { case 0: rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); break; case 1: rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape3, localInertia); break; default: rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape2, localInertia); break; } RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); //body.ActivationState = ActivationState.IslandSleeping; body.LinearFactor = new Vector3(1, 1, 0); body.AngularFactor = new Vector3(0, 0, 1); World.AddRigidBody(body); y += deltaY; } x += deltaX; } }
public virtual RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape, bool isKinematic = false) { //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.0f); Vector3 localInertia = Vector3.Zero; if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia); RigidBody body = new RigidBody(rbInfo); if (isKinematic) { body.CollisionFlags = body.CollisionFlags | CollisionFlags.KinematicObject; body.ActivationState = ActivationState.DisableDeactivation; } rbInfo.Dispose(); _world.AddRigidBody(body); return body; }
public static void SetLocalInertia(this RigidBodyConstructionInfo obj, OpenTK.Vector3 value) { SetLocalInertia(obj, ref value); }
public bool Build( FeatureUpdateContext updateContext, [ParentModel] Point[] Points, double Size) { //-- Upkeep //-- if( physics == null ) { physics = new Sutd.Physics( ); } else { Regenerate( ); physics.Reset( ); } //-- Set Gravity //-- physics.world.Gravity = new Vec3D( 0, 0, -10 ); //-- Create Ground //-- var body = new Sutd.Physics.Body( ); { //-- Define Infinite Plane //-- var shape = new StaticPlaneShape( new Vec3D( 0, 0, 1 ), 0 ); physics.shapes.Add( shape ); //-- Set Physics State / Bullet //-- Fixed bodies have zero mass and inertia //-- var param = new RigidBodyConstructionInfo( mass: 0.0f, motionState: new DefaultMotionState( Mat4D.Identity ), collisionShape: shape, localInertia: Vec3D.Zero ); body.rigid = new RigidBody( param ); param.Dispose( ); physics.world.AddRigidBody( body.rigid ); body.matrix = body.rigid.WorldTransform; //-- Set Visual State / Rhino //-- Create a very thin but wide finite box //-- var transform = DTransform3d.Identity; AddBox( transform, new DVector3d( 50, 50, 0.01 ) ); body.solid = geometry[geometry.Count - 1]; } physics.bodies.Add( body ); //-- Create 3D Grid of Boxes //-- float half = (float)( Size * 0.5 ); foreach( var point in Points ) { body = new Sutd.Physics.Body( ); { //-- Collision Shape //-- var shape = new BoxShape( half, half, half ); physics.shapes.Add( shape ); //-- Mass Properties //-- var inertia = Vec3D.Zero; shape.CalculateLocalInertia( mass: 1.0f, inertia: out inertia ); //-- Physics State //-- var param = new RigidBodyConstructionInfo( mass: 1.0f, motionState: new DefaultMotionState( Mat4D.Identity ), collisionShape: shape, localInertia: inertia ); body.rigid = new RigidBody( param ); param.Dispose( ); physics.world.AddRigidBody( body.rigid ); body.rigid.Translate( new Vec3D( (float)point.X, (float)point.Y, (float)point.Z ) ); body.matrix = body.rigid.WorldTransform; //-- Visual State //-- var transform = DTransform3d.Identity; transform.Translation = point.DPoint3d; AddBox( transform, new DVector3d( Size, Size, Size ) ); body.solid = geometry[geometry.Count - 1]; } physics.bodies.Add( body ); } return true; }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf); World.Gravity = new Vector3(0, -10, 0); GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher); string bulletFile; string[] args = Environment.GetCommandLineArgs(); if (args.Length == 1) { bulletFile = "testFile.bullet"; } else { bulletFile = args[1]; } fileLoader = new CustomBulletWorldImporter(World); if (!fileLoader.LoadFile(bulletFile)) { CollisionShape groundShape = new BoxShape(50); CollisionShapes.Add(groundShape); RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; Vector3[] positions = new Vector3[2] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) }; float[] radi = new float[2] { 0.3f, 0.4f }; CollisionShape colShape = new MultiSphereShape(positions, radi); //CollisionShape colShape = new CapsuleShapeZ(1, 1); //CollisionShape colShape = new CylinderShapeZ(1, 1, 1); //CollisionShape colShape = new BoxShape(1); //CollisionShape colShape = new SphereShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); float start_x = StartPosX - ArraySizeX / 2; float start_y = StartPosY; float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix startTransform = Matrix.Translation( 2 * i + start_x, 2 * k + start_y, 2 * j + start_z ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } DefaultSerializer serializer = new DefaultSerializer(); serializer.RegisterNameForObject(ground, "GroundName"); for (i = 0; i < CollisionShapes.Count; i++) serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString()); Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0)); World.AddConstraint(p2p); serializer.RegisterNameForObject(p2p, "constraintje"); World.Serialize(serializer); BulletSharp.DataStream data = serializer.LockBuffer(); byte[] dataBytes = new byte[data.Length]; data.Read(dataBytes, 0, dataBytes.Length); FileStream file = new FileStream("testFile.bullet", FileMode.Create); file.Write(dataBytes, 0, dataBytes.Length); file.Close(); } }
public RigidBody(RigidBodyConstructionInfo constructionInfo) : base(btRigidBody_new(constructionInfo._native)) { _collisionShape = constructionInfo.CollisionShape; _motionState = constructionInfo.MotionState; }
public static OpenTK.Matrix4 GetStartWorldTransform(this RigidBodyConstructionInfo obj) { OpenTK.Matrix4 value; GetStartWorldTransform(obj, out value); return(value); }
static RaycastVehicle() { using (var ci = new RigidBodyConstructionInfo(0, null, null)) { fixedBody = new RigidBody(ci); fixedBody.SetMassProps(0, Vector3.Zero); } }
/// <inheritdoc/> public override BulletSharp.RigidBody CreateRigidBody(float mass) { var ppm = DD.Physics.PhysicsSimulator.PPM; var mstate = new DefaultMotionState (); var shape = new BulletSharp.CapsuleShape (radius/ppm, halfHeight/ppm); var info = new BulletSharp.RigidBodyConstructionInfo (mass, mstate, shape); return new BulletSharp.RigidBody (info); }
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; }
public virtual RigidBody CreateRigidBody(bool isDynamic, float mass, ref Matrix startTransform, CollisionShape shape, string bodyName) { Vector3 localInertia; if (mass != 0.0f) { shape.CalculateLocalInertia(mass, out localInertia); } else { localInertia = Vector3.Zero; } RigidBodyConstructionInfo info = new RigidBodyConstructionInfo(mass, null, shape, localInertia); RigidBody body = new RigidBody(info); info.Dispose(); body.WorldTransform = startTransform; if (_dynamicsWorld != null) { _dynamicsWorld.AddRigidBody(body); } if (bodyName != null) { _objectNameMap.Add(body, bodyName); _nameBodyMap.Add(bodyName, body); } _allocatedRigidBodies.Add(body); return body; }
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings) { int nLinks = settings.NumLinks; float mass = 13.5f * Scaling; Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling; var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep); //body.HasSelfCollision = false; //Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI); Quaternion orn = new Quaternion(0, 0, 0, 1); body.BasePosition = settings.BasePosition; body.WorldToBaseRot = orn; body.BaseVelocity = Vector3.Zero; Vector3 joint_axis_hinge = new Vector3(1, 0, 0); Vector3 joint_axis_prismatic = new Vector3(0, 0, 1); Quaternion parent_to_child = orn.Inverse(); Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic); Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge); int this_link_num = -1; int link_num_counter = 0; Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling; Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling; for (int i = 0; i < nLinks; i++) { float initial_joint_angle = 0.3f; if (i > 0) initial_joint_angle = -0.06f; int child_link_num = link_num_counter++; if (settings.UsePrismatic) // i == (nLinks - 1)) { body.SetupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision); } else { body.SetupRevolute(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision); } body.SetJointPos(child_link_num, initial_joint_angle); this_link_num = i; /*if (false) //!useGroundShape && i == 4) { Vector3 pivotInAworld = new Vector3(0, 20, 46); Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld); Vector3 pivotInBworld = pivotInAworld; MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); }*/ if (settings.UsePrismatic) { //MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3); if (settings.CreateConstraints) { MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } } else { //if (true) { var con = new MultiBodyJointMotor(body, i, 0, 50000); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2); } } // Add a collider for the base Quaternion[] worldToLocal = new Quaternion[nLinks + 1]; Vector3[] localOrigin = new Vector3[nLinks + 1]; worldToLocal[0] = body.WorldToBaseRot; localOrigin[0] = body.BasePosition; //Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f); Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f); float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 }; //float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W }; //if (true) { CollisionShape box = new BoxShape(halfExtents * Scaling); var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia); RigidBody bodyB = new RigidBody(bodyInfo); var collider = new MultiBodyLinkCollider(body, -1); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse()) * Matrix.Translation(localOrigin[0]); collider.WorldTransform = tr; bodyB.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.BaseCollider = collider; } for (int i = 0; i < body.NumLinks; i++) { int parent = body.GetParent(i); worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1]; localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse().Rotate(body.GetRVector(i))); } for (int i = 0; i < body.NumLinks; i++) { CollisionShape box = new BoxShape(halfExtents * Scaling); var collider = new MultiBodyLinkCollider(body, i); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse()) * Matrix.Translation(localOrigin[i + 1]); collider.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.GetLink(i).Collider = collider; //World.DebugDrawer.DrawBox(halfExtents, pos, quat); } (World as MultiBodyDynamicsWorld).AddMultiBody(body); return body; }
public static RigidBody GetFixedBody() { if (_fixedBody == null) { using (var cinfo = new RigidBodyConstructionInfo(0, null, null)) { _fixedBody = new RigidBody(cinfo); _fixedBody.SetMassProps(0, Vector3.Zero); } } return _fixedBody; }
public RigidBody LocalCreateRigidBody(float mass, Matrix4 startTransform, CollisionShape shape) { bool isDynamic = (mass != 0.0f); Vector3 localInertia = Vector3.ZERO; if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); DefaultMotionState myMotionState = new DefaultMotionState(startTransform); RigidBody body; using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia)) { body = new RigidBody(rbInfo); } World.AddRigidBody(body); return body; }
static CustomVehicle() { var ci = new RigidBodyConstructionInfo(0, null, null); fixedBody = new RigidBody(ci); fixedBody.SetMassProps(0, Vector3.Zero); ci.Dispose(); }
public Physics(SceneManager sceneMgr) { // collision configuration contains default setup for memory, collision setup collisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(collisionConf); Broadphase = new DbvtBroadphase(); World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf); World.Gravity = new Vector3(0, -10, 0); // create the ground CollisionShape groundShape = new BoxShape(50, 1, 50); CollisionShapes.Add(groundShape); CollisionObject ground = LocalCreateRigidBody(0, Matrix4.IDENTITY, groundShape); ground.UserObject = "Ground"; // create a few dynamic rigidbodies float mass = 1.0f; CollisionShape colShape = new BoxShape(1); CollisionShapes.Add(colShape); Vector3 localInertia = colShape.CalculateLocalInertia(mass); var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia); float start_x = StartPosX - ArraySizeX / 2; float start_y = StartPosY; float start_z = StartPosZ - ArraySizeZ / 2; int k, i, j; for (k = 0; k < ArraySizeY; k++) { for (i = 0; i < ArraySizeX; i++) { for (j = 0; j < ArraySizeZ; j++) { Matrix4 startTransform = new Matrix4(); startTransform.MakeTrans( new Vector3( 2*i + start_x, 2*k + start_y, 2*j + start_z ) ); // using motionstate is recommended, it provides interpolation capabilities // and only synchronizes 'active' objects int index = (k * ArraySizeX + i) * ArraySizeZ + j; Entity box = sceneMgr.CreateEntity("Box" + index.ToString(), "box.mesh"); box.SetMaterialName("BoxMaterial/Active"); SceneNode boxNode = sceneMgr.RootSceneNode.CreateChildSceneNode("BoxNode" + index.ToString()); boxNode.AttachObject(box); boxNode.Scale(new Vector3(2, 2, 2)); var mogreMotionState = new MogreMotionState(box, boxNode, startTransform); rbInfo.MotionState = mogreMotionState; RigidBody body = new RigidBody(rbInfo); mogreMotionState.Body = body; // make it drop from a height body.Translate(new Vector3(0, 20, 0)); World.AddRigidBody(body); } } } rbInfo.Dispose(); }