void CreateOrConfigureMultiBody(ref MultiBody mb, float baseMass, BCollisionShape[] shapes, BMultiBodyConstraint[] constraints)
{
BulletSharp.Math.Vector3 inertia = BulletSharp.Math.Vector3.Zero;
if (baseMass != 0)
{
CollisionShape cs = m_baseCollisionShape.GetCollisionShape();
cs.CalculateLocalInertia(baseMass, out inertia);
}
mb = new MultiBody(m_links.Count, baseMass, inertia, fixedBase, canSleep);
mb.BasePosition = transform.position.ToBullet();
UnityEngine.Quaternion r = UnityEngine.Quaternion.Inverse(transform.rotation);
mb.WorldToBaseRot = r.ToBullet();
for (int i = 0; i < m_links.Count; i++)
{
//Debug.Log("Found link " + i + " parent " + m_links[i].parentIndex + " index " + m_links[i].index);
BMultiBodyLink link = m_links[i];
CollisionShape cs = shapes[i].GetCollisionShape();
if (cs != null)
{
cs.CalculateLocalInertia(link.mass, out inertia);
}
else
{
inertia = BulletSharp.Math.Vector3.Zero;
}
FeatherstoneJointType jt = link.jointType;
int parentIdx = link.parentIndex;
// Vector from parent pivot (COM) to the joint pivot point in parent's frame
UnityEngine.Vector3 parentCOM2ThisPivotOffset;
link.FreezeJointAxis();
if (link.parentIndex >= 0)
{
parentCOM2ThisPivotOffset = link.parentCOM2JointPivotOffset;
}
else
{
parentCOM2ThisPivotOffset = transform.InverseTransformPoint(link.transform.TransformPoint(link.localPivotPosition));
}
// Vector from the joint pivot point to link's pivot point (COM) in link's frame.
UnityEngine.Vector3 thisPivotToThisCOMOffset = link.thisPivotToJointCOMOffset;
// Should rotate vectors in parent frame to vectors in local frame
UnityEngine.Quaternion parentToThisRotation = link.parentToJointRotation;
switch (jt)
{
case FeatherstoneJointType.Fixed:
mb.SetupFixed(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Planar:
mb.SetupPlanar(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Prismatic:
mb.SetupPrismatic(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Revolute:
mb.SetupRevolute(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), link.rotationAxis.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
case FeatherstoneJointType.Spherical:
mb.SetupSpherical(i, link.mass, inertia, link.parentIndex, parentToThisRotation.ToBullet(), parentCOM2ThisPivotOffset.ToBullet(), thisPivotToThisCOMOffset.ToBullet(), false);
break;
default:
Debug.LogError("Invalid joint type for link " + link.name);
break;
}
}
mb.CanSleep = true;
mb.HasSelfCollision = false;
mb.UseGyroTerm = true;
bool damping = true;
if (damping)
{
mb.LinearDamping = 0.1f;
mb.AngularDamping = 0.9f;
}
else
{
mb.LinearDamping = 0;
mb.AngularDamping = 0;
}
mb.FinalizeMultiDof();
m_multibody = mb;
}