void AddColliders(MultiBody multiBody, Vector3 baseHalfExtents, Vector3 linkHalfExtents)
        {
            // Add a collider for the base
            Quaternion[] worldToLocal = new Quaternion[multiBody.NumLinks + 1];
            Vector3[]    localOrigin  = new Vector3[multiBody.NumLinks + 1];

            worldToLocal[0] = multiBody.WorldToBaseRot;
            localOrigin[0]  = multiBody.BasePosition;

            //if (true)
            {
                var collider = new MultiBodyLinkCollider(multiBody, -1);
                collider.CollisionShape = new BoxShape(baseHalfExtents);

                Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse);
                tr.Origin = localOrigin[0];
                collider.WorldTransform = tr;

                World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
                                         CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
                BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
                collider.Friction      = Friction;
                multiBody.BaseCollider = collider;
            }

            for (int i = 0; i < multiBody.NumLinks; i++)
            {
                int parent = multiBody.GetParent(i);
                worldToLocal[i + 1] = multiBody.GetParentToLocalRot(i) * worldToLocal[parent + 1];
                localOrigin[i + 1]  = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse.Rotate(multiBody.GetRVector(i)));
            }

            for (int i = 0; i < multiBody.NumLinks; i++)
            {
                var collider = new MultiBodyLinkCollider(multiBody, i);
                collider.CollisionShape = new BoxShape(linkHalfExtents);
                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;
                BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider);
                multiBody.GetLink(i).Collider = collider;
            }
        }
Beispiel #2
0
        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);
        }
Beispiel #3
0
        //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);
        }