public FeatherStoneDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); _solver = new MultiBodyConstraintSolver(); MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration); CreateGround(); int numLinks = 5; bool spherical = true; bool floatingBase = false; var basePosition = new Vector3(-0.4f, 3.0f, 0.0f); var baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var multiBody = CreateFeatherstoneMultiBody(MultiBodyWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase); bool damping = true; if (damping) { multiBody.LinearDamping = 0.1f; multiBody.AngularDamping = 0.9f; } else { multiBody.LinearDamping = 0; multiBody.AngularDamping = 0; } if (numLinks > 0) { float q0 = 45.0f * (float)Math.PI / 180.0f; if (spherical) { Quaternion quat0 = Quaternion.RotationAxis(Vector3.Normalize(new Vector3(1, 1, 0)), q0); quat0.Normalize(); multiBody.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }); } else { multiBody.SetJointPosMultiDof(0, new float[] { q0 }); } } CreateColliders(multiBody, baseHalfExtents, linkHalfExtents); CreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f)); }
/// <summary> /// Default Constructor /// </summary> public Physics() { DantzigSolver mlcp = new DantzigSolver(); collisionConf = new SoftBodyRigidBodyCollisionConfiguration(); dispatcher = new CollisionDispatcher(new DefaultCollisionConfiguration()); solver = new DefaultSoftBodySolver(); ConstraintSolver cSolver = new MultiBodyConstraintSolver(); broadphase = new DbvtBroadphase(); World = new SoftRigidDynamicsWorld(dispatcher, broadphase, cSolver, collisionConf, solver); //Actual scaling is unknown, this gravity is probably not right World.Gravity = new Vector3(0, -98.1f, 0); World.SetInternalTickCallback(new DynamicsWorld.InternalTickCallback((w, f) => DriveJoints.UpdateAllMotors(Skeleton, cachedArgs))); //Roobit RigidNode_Base.NODE_FACTORY = (Guid guid) => new BulletRigidNode(guid); string RobotPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Robots\"; string dir = RobotPath; GetFromDirectory(RobotPath, s => { Skeleton = (BulletRigidNode)BXDJSkeleton.ReadSkeleton(s + "skeleton.bxdj"); dir = s; }); List <RigidNode_Base> nodes = Skeleton.ListAllNodes(); for (int i = 0; i < nodes.Count; i++) { BulletRigidNode bNode = (BulletRigidNode)nodes[i]; bNode.CreateRigidBody(dir + bNode.ModelFileName); bNode.CreateJoint(); if (bNode.joint != null) { World.AddConstraint(bNode.joint, true); } World.AddCollisionObject(bNode.BulletObject); collisionShapes.Add(bNode.BulletObject.CollisionShape); } //Field string fieldPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Fields\"; GetFromDirectory(fieldPath, s => f = BulletFieldDefinition.FromFile(s)); foreach (RigidBody b in f.Bodies) { World.AddRigidBody(b); collisionShapes.Add(b.CollisionShape); } }
//GUIHelperInterface m_guiHelper; public CommonMultiBodyBase() { //m_filterCallback = null; m_pairCache = null; m_broadphase = null; m_dispatcher = null; m_solver = null; m_collisionConfiguration = null; m_dynamicsWorld = null; //m_pickedBody = null; //m_pickedConstraint = null; //m_pickingMultiBodyPoint2Point = null; m_prevCanSleep = false; //m_guiHelper = null; }
public virtual void createEmptyDynamicsWorld() { ///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 (see Extras/BulletMultiThreaded) Dispatcher = new CollisionDispatcher(CollisionConf); m_pairCache = new HashedOverlappingPairCache(); Broadphase = new DbvtBroadphase(m_pairCache); m_solver = new MultiBodyConstraintSolver(); World = m_dynamicsWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, m_solver, CollisionConf); m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0)); }
public virtual void createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new DefaultCollisionConfiguration(); //m_collisionConfiguration.setConvexConvexMultipointIterations(); //m_filterCallback = new MyOverlapFilterCallback2(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_dispatcher = new CollisionDispatcher(m_collisionConfiguration); m_pairCache = new HashedOverlappingPairCache(); //m_pairCache.OverlapFilterCallback = (m_filterCallback); m_broadphase = new DbvtBroadphase(m_pairCache); //btSimpleBroadphase(); m_solver = new MultiBodyConstraintSolver(); m_dynamicsWorld = new MultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0)); }
/* * Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation. */ public bool CreatePhysicsWorld(out CollisionWorld world, out CollisionConfiguration collisionConfig, out CollisionDispatcher dispatcher, out BroadphaseInterface broadphase, out ConstraintSolver solver, out SoftBodyWorldInfo softBodyWorldInfo) { bool success = true; if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching"); m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf; success = false; } collisionConfig = null; if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf) { collisionConfig = new DefaultCollisionConfiguration(); } else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf) { collisionConfig = new SoftBodyRigidBodyCollisionConfiguration(); } dispatcher = new CollisionDispatcher(collisionConfig); if (m_broadphaseType == BroadphaseType.DynamicAABBBroadphase) { broadphase = new DbvtBroadphase(); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase) { broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase_32bit) { broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies); } else { broadphase = null; } world = null; softBodyWorldInfo = null; solver = null; if (m_worldType == WorldType.CollisionOnly) { world = new CollisionWorld(dispatcher, broadphase, collisionConfig); } else if (m_worldType == WorldType.RigidBodyDynamics) { switch (solverType) { case SolverType.MLCP: DantzigSolver dtsolver = new DantzigSolver(); solver = new MlcpSolver(dtsolver); break; default: break; } world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfig); } else if (m_worldType == WorldType.MultiBodyWorld) { MultiBodyConstraintSolver mbConstraintSolver = null; switch (solverType) { /* case SolverType.MLCP: * DantzigSolver dtsolver = new DantzigSolver(); * mbConstraintSolver = new MultiBodyMLCPConstraintSolver(dtsolver); * break;*/ default: mbConstraintSolver = new MultiBodyConstraintSolver(); break; } world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig); } else if (m_worldType == WorldType.SoftBodyAndRigidBody) { SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = sequentialImpulseConstraintSolverRandomSeed; m_world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, siConstraintSolver, CollisionConf); _ddWorld = (DiscreteDynamicsWorld)m_world;; SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)m_world; m_world.DispatchInfo.EnableSpu = true; _sworld.WorldInfo.SparseSdf.Initialize(); _sworld.WorldInfo.SparseSdf.Reset(); _sworld.WorldInfo.AirDensity = 1.2f; _sworld.WorldInfo.WaterDensity = 0; _sworld.WorldInfo.WaterOffset = 0; _sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; _sworld.WorldInfo.Gravity = m_gravity.ToBullet(); } if (world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet(); } if (_doDebugDraw) { DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = _debugDrawMode; world.DebugDrawer = db; } return(success); }
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, -51.55f, 0), groundShape); ground.UserObject = "Ground"; int numLinks = 5; bool spherical = true; bool floatingBase = false; Vector3 basePosition = new Vector3(-0.4f, 3.0f, 0.0f); Vector3 baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); Vector3 linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f); var mb = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase); floatingBase = !floatingBase; mb.CanSleep = true; mb.HasSelfCollision = false; mb.UseGyroTerm = true; bool damping = true; if (damping) { mb.LinearDamping = 0.1f; mb.AngularDamping = 0.9f; } else { mb.LinearDamping = 0; mb.AngularDamping = 0; } if (numLinks > 0) { float q0 = 45.0f * (float)Math.PI / 180.0f; if (spherical) { Quaternion quat0 = Quaternion.RotationAxis(new Vector3(1, 1, 0).Normalized, q0); quat0.Normalize(); mb.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }); } else { mb.SetJointPosMultiDof(0, new float[] { q0 }); } } AddColliders(mb, baseHalfExtents, linkHalfExtents); LocalCreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f)); }
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, -9.81f, 0); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); //Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); Vector3 baseInertiaDiag = Vector3.Zero; const float baseMass = 0; multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); //multiBody.UseRK4Integration = true; //multiBody.BaseWorldTransform = Matrix.Identity; //init the links Vector3 hingeJointAxis = new Vector3(1, 0, 0); //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; for (int i = 0; i < numLinks; i++) { const float linkMass = 10; Vector3 linkInertiaDiag = Vector3.Zero; using (var shape = new SphereShape(radius)) { shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); } multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } multiBody.FinalizeMultiDof(); (World as MultiBodyDynamicsWorld).AddMultiBody(multiBody); multiBody.CanSleep = canSleep; multiBody.HasSelfCollision = selfCollide; multiBody.UseGyroTerm = gyro; #if PENDULUM_DAMPING multiBody.LinearDamping = 0.1f; multiBody.AngularDamping = 0.9f; #else multiBody.LinearDamping = 0; multiBody.AngularDamping = 0; #endif for (int i = 0; i < numLinks; i++) { var shape = new SphereShape(radius); CollisionShapes.Add(shape); var col = new MultiBodyLinkCollider(multiBody, i); col.CollisionShape = shape; //const bool isDynamic = true; CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }
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); RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia); RigidBody body = new RigidBody(rbInfo); rbInfo.Dispose(); 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); }
public virtual void exitPhysics() { removePickingConstraint(); //cleanup in the reverse order of creation/initialization //remove the rigidbodies from the dynamics world and delete them if (m_dynamicsWorld == null) { int i; for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--) { m_dynamicsWorld.RemoveConstraint(m_dynamicsWorld.GetConstraint(i)); } for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i); m_dynamicsWorld.RemoveMultiBodyConstraint(mbc); mbc.Dispose(); } for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--) { MultiBody mb = m_dynamicsWorld.GetMultiBody(i); m_dynamicsWorld.RemoveMultiBody(mb); mb.Dispose(); } for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i]; RigidBody body = RigidBody.Upcast(obj); if (body != null && body.MotionState != null) { body.MotionState.Dispose(); } m_dynamicsWorld.RemoveCollisionObject(obj); obj.Dispose(); } } //delete collision shapes for (int j = 0; j < m_collisionShapes.Count; j++) { CollisionObject shape = m_collisionShapes[j]; shape.Dispose(); } m_collisionShapes.Clear(); m_dynamicsWorld.Dispose(); m_dynamicsWorld = null; m_solver.Dispose(); m_solver = null; m_broadphase.Dispose(); m_broadphase = null; m_dispatcher.Dispose(); m_dispatcher = null; m_pairCache.Dispose(); m_pairCache = null; //m_filterCallback.Dispose(); //m_filterCallback = null; m_collisionConfiguration.Dispose(); m_collisionConfiguration = null; }
public override void ExitPhysics() { if (m_inverseModel != null) { Debug.Log("Dispose inverse model " + m_inverseModel.NumBodies); m_inverseModel.Dispose(); } Debug.Log("InverseDynamicsExitPhysics"); //cleanup in the reverse order of creation/initialization //remove the rigidbodies from the dynamics world and delete them if (m_dynamicsWorld == null) { int i; for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--) { TypedConstraint tc = m_dynamicsWorld.GetConstraint(i); m_dynamicsWorld.RemoveConstraint(tc); tc.Dispose(); } for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--) { MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i); m_dynamicsWorld.RemoveMultiBodyConstraint(mbc); mbc.Dispose(); } for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--) { MultiBody mb = m_dynamicsWorld.GetMultiBody(i); m_dynamicsWorld.RemoveMultiBody(mb); mb.Dispose(); } for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--) { CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i]; RigidBody body = RigidBody.Upcast(obj); if (body != null && body.MotionState != null) { body.MotionState.Dispose(); } m_dynamicsWorld.RemoveCollisionObject(obj); obj.Dispose(); } } if (m_multiBody != null) { m_multiBody.Dispose(); } //delete collision shapes for (int j = 0; j < CollisionShapes.Count; j++) { CollisionShape shape = CollisionShapes[j]; shape.Dispose(); } CollisionShapes.Clear(); m_dynamicsWorld.Dispose(); m_dynamicsWorld = null; m_solver.Dispose(); m_solver = null; Broadphase.Dispose(); Broadphase = null; Dispatcher.Dispose(); Dispatcher = null; m_pairCache.Dispose(); m_pairCache = null; CollisionConf.Dispose(); CollisionConf = null; Debug.Log("After dispose B"); }
//Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation. public void CreatePhysicsWorld ( out CollisionWorld world, out CollisionConfiguration collisionConfig, out CollisionDispatcher dispatcher, out BroadphaseInterface broadphase, out ConstraintSolver solver, out SoftBodyWorldInfo softBodyWorldInfo ) { collisionConfig = null; switch (physicWorldParameters.collisionType) { default: case CollisionConfType.DefaultDynamicsWorldCollisionConf: collisionConfig = new DefaultCollisionConfiguration(); break; case CollisionConfType.SoftBodyRigidBodyCollisionConf: collisionConfig = new SoftBodyRigidBodyCollisionConfiguration(); break; } dispatcher = new CollisionDispatcher(collisionConfig); switch (physicWorldParameters.broadphaseType) { default: case BroadphaseType.DynamicAABBBroadphase: broadphase = new DbvtBroadphase(); break; case BroadphaseType.Axis3SweepBroadphase: broadphase = new AxisSweep3( physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies ); break; case BroadphaseType.Axis3SweepBroadphase_32bit: broadphase = new AxisSweep3_32Bit( physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies ); break; } world = null; softBodyWorldInfo = null; solver = null; switch (physicWorldParameters.worldType) { case WorldType.CollisionOnly: world = new CollisionWorld(dispatcher, broadphase, collisionConfig); break; default: case WorldType.RigidBodyDynamics: world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig); break; case WorldType.MultiBodyWorld: MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver(); constraintSolver = mbConstraintSolver; world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig); break; case WorldType.SoftBodyAndRigidBody: SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed; world = new SoftRigidDynamicsWorld(this.dispatcher, this.broadphase, siConstraintSolver, collisionConf); world.DispatchInfo.EnableSpu = true; SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)world; _sworld.WorldInfo.SparseSdf.Initialize(); _sworld.WorldInfo.SparseSdf.Reset(); _sworld.WorldInfo.AirDensity = 1.2f; _sworld.WorldInfo.WaterDensity = 0; _sworld.WorldInfo.WaterOffset = 0; _sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; _sworld.WorldInfo.Gravity = Gravity.ToBullet(); break; } if (world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)world).Gravity = Gravity.ToBullet(); } }
private void CreateWorld() { if (physicWorldParameters.worldType == WorldType.SoftBodyAndRigidBody && physicWorldParameters.collisionType != CollisionConfType.SoftBodyRigidBodyCollisionConf) { Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType = SoftBodyRigidBodyCollisionConf."); return; } isDisposed = false; switch (physicWorldParameters.collisionType) { case CollisionConfType.DefaultDynamicsWorldCollisionConf: collisionConf = new DefaultCollisionConfiguration(); break; case CollisionConfType.SoftBodyRigidBodyCollisionConf: collisionConf = new SoftBodyRigidBodyCollisionConfiguration(); break; } dispatcher = new CollisionDispatcher(collisionConf); switch (physicWorldParameters.broadphaseType) { default: case BroadphaseType.DynamicAABBBroadphase: broadphase = new DbvtBroadphase(); break; case BroadphaseType.Axis3SweepBroadphase: broadphase = new AxisSweep3(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies); break; case BroadphaseType.Axis3SweepBroadphase_32bit: broadphase = new AxisSweep3_32Bit(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies); break; } switch (physicWorldParameters.worldType) { case WorldType.CollisionOnly: World = new CollisionWorld(dispatcher, broadphase, collisionConf); break; case WorldType.RigidBodyDynamics: World = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConf); break; case WorldType.MultiBodyWorld: MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver(); constraintSolver = mbConstraintSolver; World = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConf); break; case WorldType.SoftBodyAndRigidBody: SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver(); constraintSolver = siConstraintSolver; siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed; World = new SoftRigidDynamicsWorld(dispatcher, broadphase, siConstraintSolver, collisionConf); _world.DispatchInfo.EnableSpu = true; SoftWorld.WorldInfo.SparseSdf.Initialize(); SoftWorld.WorldInfo.SparseSdf.Reset(); SoftWorld.WorldInfo.AirDensity = 1.2f; SoftWorld.WorldInfo.WaterDensity = 0; SoftWorld.WorldInfo.WaterOffset = 0; SoftWorld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero; SoftWorld.WorldInfo.Gravity = physicWorldParameters.gravity; break; } if (_world is DiscreteDynamicsWorld) { ((DiscreteDynamicsWorld)_world).Gravity = physicWorldParameters.gravity; } Debug.LogFormat("Physic World of type {0} Instanced", physicWorldParameters.worldType); if (physicWorldParameters.debug) { Debug.Log("Physic World Debug is active"); DebugDrawUnity db = new DebugDrawUnity(); db.DebugMode = physicWorldParameters.debugDrawMode; _world.DebugDrawer = db; } }
public PendulumDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); _solver = new MultiBodyConstraintSolver(); MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration); World.SetInternalTickCallback(TickCallback, null, true); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; var linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var baseInertiaDiag = Vector3.Zero; const float baseMass = 0; MultiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); //MultiBody.UseRK4Integration = true; //MultiBody.BaseWorldTransform = Matrix.Identity; //init the links var hingeJointAxis = new Vector3(1, 0, 0); //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; for (int i = 0; i < numLinks; i++) { const float linkMass = 10; Vector3 linkInertiaDiag = Vector3.Zero; using (var shape = new SphereShape(radius)) { shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); } MultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } MultiBody.FinalizeMultiDof(); MultiBodyWorld.AddMultiBody(MultiBody); MultiBody.CanSleep = canSleep; MultiBody.HasSelfCollision = selfCollide; MultiBody.UseGyroTerm = gyro; #if PENDULUM_DAMPING MultiBody.LinearDamping = 0.1f; MultiBody.AngularDamping = 0.9f; #else MultiBody.LinearDamping = 0; MultiBody.AngularDamping = 0; #endif for (int i = 0; i < numLinks; i++) { var shape = new SphereShape(radius); var col = new MultiBodyLinkCollider(MultiBody, i); col.CollisionShape = shape; const bool isDynamic = true; CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); MultiBody.GetLink(i).Collider = col; } }