protected override MultiBodyConstraint _CreateConstraint(MultiBody mb, int linkIndex) { return(new MultiBodyJointMotor(mb, linkIndex, _targetVelocity, _maxMotorImpulse)); }
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, false); //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), 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, 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 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; } }
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.Everything; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }
// Update is called once per frame void FixedUpdate() { MultiBody mb = m_cachedMultibody.GetMultiBody(); if (m_inverseModel == null) { // construct inverse model MultiBodyTreeCreator id_creator = new MultiBodyTreeCreator(); if (-1 == id_creator.CreateFromMultiBody(mb)) { Debug.LogError("error creating tree\n"); } else { m_inverseModel = id_creator.CreateMultiBodyTree(); } Debug.Log("Created inverse model"); } ReadCurrentPose(); GetDesiredPose(); CalculatedDesiredAccelerations(); int baseDofs = mb.HasFixedBase ? 0 : 6; int numDofs = mb.NumDofs; float[] qq, qqdot, ddesiredAcceleration; if (mb.HasFixedBase) { qq = q; qqdot = qdot; ddesiredAcceleration = desiredAcceleration; } else { // inverseModel stores dofs for the base if floating base. Need to prepad to handle this. qq = new float[baseDofs + numDofs]; qqdot = new float[baseDofs + numDofs]; ddesiredAcceleration = new float[baseDofs + numDofs]; for (int i = 0; i < numDofs; i++) { qq[i + baseDofs] = q[i]; qqdot[i + baseDofs] = qdot[i]; ddesiredAcceleration[i + baseDofs] = desiredAcceleration[i]; } } float[] joint_force = new float[numDofs + baseDofs]; if (-1 != m_inverseModel.CalculateInverseDynamics(mb.HasFixedBase, qq, qqdot, ddesiredAcceleration, joint_force)) { // use inverse model: apply joint force corresponding to // desired acceleration nu. for floating base will have been padded by baseDofs for (int dof = 0; dof < numDofs; dof++) { mb.AddJointTorque(dof, joint_force[dof + baseDofs]); } } else { Debug.LogError("Bad return from CalculateInverseDynamics"); } }
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. public MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase) { BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[] { new BulletSharp.Math.Vector4(1, 0, 0, 1), new BulletSharp.Math.Vector4(0, 1, 0, 1), new BulletSharp.Math.Vector4(0, 1, 1, 1), new BulletSharp.Math.Vector4(1, 1, 0, 1), }; int curColor = 0; bool damping = false; bool gyro = false; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f); BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f); //mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); float baseMass = fixedBase ? 0.0f : 10.0f; if (baseMass != 0) { //CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); shape.CalculateLocalInertia(baseMass, out baseInertiaDiag); shape.Dispose(); } Debug.Log("NumLinks " + numLinks); MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); pMultiBody.BaseWorldTransform = baseWorldTrans; BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0); // pMultiBody.setBaseVel(vel); //init the links BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0); //y-axis assumed up BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset ////// float q0 = 1.0f * Mathf.PI / 180.0f; BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0); quat0.Normalize(); ///// for (int i = 0; i < numLinks; ++i) { float linkMass = 1.0f; //if (i==3 || i==2) // linkMass= 1000; BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// } else { shape = new SphereShape(radius); } shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); shape.Dispose(); if (!spherical) { //pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); if (i == 0) { pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom); } } else { //pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false); pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } } pMultiBody.FinalizeMultiDof(); world.AddMultiBody(pMultiBody); MultiBody mbC = pMultiBody; mbC.CanSleep = (canSleep); mbC.HasSelfCollision = (selfCollide); mbC.UseGyroTerm = (gyro); // if (!damping) { mbC.LinearDamping = (0.0f); mbC.AngularDamping = (0.0f); } else { mbC.LinearDamping = (0.1f); mbC.AngularDamping = (0.9f); } if (numLinks > 0) { q0 = 180.0f * Mathf.PI / 180.0f; if (!spherical) { mbC.SetJointPosMultiDof(0, new float[] { q0 }); } else { BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0); vv.Normalize(); quat0 = new BulletSharp.Math.Quaternion(vv, q0); quat0.Normalize(); float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }; mbC.SetJointPosMultiDof(0, quat0fs); } } /// BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion> world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1]; BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3> local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1]; world_to_local[0] = pMultiBody.WorldToBaseRot; local_origin[0] = pMultiBody.BasePosition; // double friction = 1; { if (true) { CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]); // guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1); links.Add(col); col.CollisionShape = shape; Matrix tr = new Matrix(); tr.ScaleVector = BulletSharp.Math.Vector3.One; //if we don't set the initial pose of the btCollisionObject, the simulator will do this //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.Origin = local_origin[0]; BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f); tr.Rotation = (orn); col.WorldTransform = (tr); bool isDynamic = (baseMass > 0 && !fixedBase); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2); BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f); //guiHelper.createCollisionObjectGraphicsObject(col, color); // col.setFriction(friction); pMultiBody.BaseCollider = (col); } } for (int i = 0; i < pMultiBody.NumLinks; ++i) { int parent = pMultiBody.GetParent(i); world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1]; BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i)); local_origin[i + 1] = local_origin[parent + 1] + vv; } for (int i = 0; i < pMultiBody.NumLinks; ++i) { BulletSharp.Math.Vector3 posr = local_origin[i + 1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W }; CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); } else { shape = new SphereShape(radius); } //guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i); links.Add(col); col.CollisionShape = (shape); Matrix tr = new Matrix(); tr.ScaleVector = new BulletSharp.Math.Vector3(); tr.Origin = (posr); tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3])); col.WorldTransform = (tr); // col.setFriction(friction); bool isDynamic = true;//(linkMass > 0); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; //if (i==0||i>numLinks-2) { world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2); BulletSharp.Math.Vector4 color = colors[curColor]; curColor++; curColor &= 3; //guiHelper.createCollisionObjectGraphicsObject(col, color); pMultiBody.GetLink(i).Collider = col; } } return(pMultiBody); }
public 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; CollisionShapes.Clear(); links.Clear(); Debug.Log("After dispose B"); }