MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, int numLinks, Vector3 basePosition, Vector3 baseHalfExtents, Vector3 linkHalfExtents, bool spherical, bool floating) { float mass = 1; Vector3 inertia = Vector3.Zero; if (mass != 0) { using (var box = new BoxShape(baseHalfExtents)) { box.CalculateLocalInertia(mass, out inertia); } } var mb = new MultiBody(numLinks, mass, inertia, !floating, false); //body.HasSelfCollision = false; //body.BaseVelocity = Vector3.Zero; mb.BasePosition = basePosition; //body.WorldToBaseRot = new Quaternion(0, 0, 1, -0.125f * (float)Math.PI); mb.WorldToBaseRot = Quaternion.Identity; float linkMass = 1; Vector3 linkInertia = Vector3.Zero; if (linkMass != 0) { using (var box = new BoxShape(linkHalfExtents)) { box.CalculateLocalInertia(linkMass, out linkInertia); } } //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset for (int i = 0; i < numLinks; i++) { if (spherical) { mb.SetupSpherical(i, linkMass, linkInertia, i - 1, Quaternion.Identity, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { Vector3 hingeJointAxis = new Vector3(1, 0, 0); mb.SetupRevolute(i, linkMass, linkInertia, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } } mb.FinalizeMultiDof(); (World as MultiBodyDynamicsWorld).AddMultiBody(mb); return mb; }
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; }
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 = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }