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);
}
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);
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;
multiBody.GetLink(i).Collider = collider;
}
}
//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);
}