// Object A has the constraint compontent and is constrainted to object B // The constraint frame is defined relative to A by three vectors. // This method calculates the frames A and B that need to be passed to bullet public bool CreateFramesA_B(Vector3 forwardInA, Vector3 upInA, Vector3 constraintPivotInA, out BM.Matrix frameInA, out BM.Matrix frameInB, ref string errorMsg) { frameInA = BM.Matrix.Identity; if (!CreateFrame(forwardInA, upInA, constraintPivotInA, ref frameInA, ref errorMsg)) { frameInB = BM.Matrix.Identity; return(false); } BM.Vector4 x = frameInA.Row1; BM.Vector4 y = frameInA.Row2; BM.Vector4 z = frameInA.Row3; Vector3 xx = new Vector3(x.X, x.Y, x.Z); Vector3 yy = new Vector3(y.X, y.Y, y.Z); Vector3 zz = new Vector3(z.X, z.Y, z.Z); Quaternion q = transform.localRotation * Quaternion.Inverse(m_otherRigidBody.transform.localRotation); xx = q * xx; yy = q * yy; zz = q * zz; frameInB = BM.Matrix.Identity; frameInB.Row1 = new BM.Vector4(xx.ToBullet(), 0f); frameInB.Row2 = new BM.Vector4(yy.ToBullet(), 0f); frameInB.Row3 = new BM.Vector4(zz.ToBullet(), 0f); frameInB.Origin = m_otherRigidBody.transform.InverseTransformPoint(transform.TransformPoint(constraintPivotInA)).ToBullet(); return(true); }
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. public static 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; int numLinks = 2; 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(); } 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); 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); 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); }