// not using GetComponentsInChildren because the order in which links are discovered is important.
private bool GetLinksInChildrenAndNumber(Transform t, List <BMultiBodyLink> links, int parentIndex)
{
BMultiBodyLink mbl = t.GetComponent <BMultiBodyLink>();
if (mbl != null)
{
links.Add(mbl);
mbl.index = links.Count - 1;
mbl.parentIndex = parentIndex;
}
for (int i = 0; i < t.childCount; i++)
{
int newParent;
if (mbl == null)
{
newParent = parentIndex;
}
else
{
newParent = mbl.index;
}
if (!GetLinksInChildrenAndNumber(t.GetChild(i), links, newParent))
{
return(false);
}
}
return(true);
}
private bool ValidateMultiBodyHierarchy(List <BMultiBodyLink> links)
{
for (int i = 0; i < links.Count; i++)
{
BMultiBodyLink link = links[i];
if (link.transform.parent.GetComponent <BMultiBodyLink>() == null &&
link.transform.parent.GetComponent <BMultiBody>() == null)
{
Debug.LogErrorFormat("Bad multibody hierarchy. The parent of link {0} does not have a BMultiBodyLink or BMultiBody component", link.name);
return(false);
}
}
return(true);
}
protected void OnDrawGizmosSelected()
{
if (!m_doDrawGizmos)
{
return;
}
BMultiBodyLink link = GetComponent <BMultiBodyLink>();
if (link != null)
{
if (link.rotationAxis != Vector3.zero)
{
float arcLen = m_jointUpperLimit - m_jointLowerLimit;
UnityEditor.Handles.color = new Color(.7f, .5f, .5f, .6f);
Vector3 p = transform.TransformPoint(link.localPivotPosition);
Vector3 from;
Vector3 axisOfRotation;
if (link.axesAreFrozen)
{
axisOfRotation = transform.parent.TransformDirection(link.rotationAxisInParentFrame);
from = transform.parent.TransformDirection(link.jointToThisCOMInParentFrame);
}
else
{
axisOfRotation = transform.TransformDirection(link.rotationAxis);
from = transform.TransformDirection(-link.localPivotPosition);
if (from.magnitude < 10E-7f)
{
from = p - transform.parent.position;
}
from = Vector3.ProjectOnPlane(from, axisOfRotation);
}
if (from.magnitude > 10E-7f)
{
from.Normalize();
Quaternion q = Quaternion.AngleAxis(m_jointLowerLimit, axisOfRotation);
from = q * from;
UnityEditor.Handles.DrawSolidArc(p, axisOfRotation, from, arcLen, 1f * link.gizmoScale);
}
}
}
}
public MultiBodyConstraint CreateMultiBodyConstraint(MultiBody mb)
{
if (m_multiBodyConstraintPtr == null)
{
BMultiBodyLink mbl = GetComponent <BMultiBodyLink>();
if (mbl == null)
{
Debug.LogError("BMultiBodyConstraint must have a BMultiBodyLink");
return(null);
}
if (mbl.index == -1)
{
Debug.LogError("Bad BMultiBodyLink index");
return(null);
}
m_multiBodyConstraintPtr = _CreateConstraint(mb, mbl.index);
}
return(m_multiBodyConstraintPtr);
}
public bool AddMultiBody(BMultiBody mb)
{
if (!_isDisposed)
{
if (m_worldType < WorldType.MultiBodyWorld)
{
Debug.LogError("World type must be be multibody");
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Adding multibody {0} to world", mb);
}
// This is complicated because the native parts for multiple components are created by bullet when the MultiBody is created. We
// need to let Bullet create these then get the references to the native parts and assign them to the components.
if (mb._BuildMultiBody())
{
((MultiBodyDynamicsWorld)m_world).AddMultiBody(mb.GetMultiBody(), (int)mb.groupsIBelongTo, (int)mb.collisionMask);
mb.CreateColliders();
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Adding MultiBodyBaseCollider {0} to world", mb);
}
m_world.AddCollisionObject(mb.GetBaseCollider(), mb.groupsIBelongTo, mb.collisionMask);
List <BMultiBodyLink> links = mb.GetLinks();
for (int i = 0; i < links.Count; i++)
{
BMultiBodyLink link = links[i];
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Adding MultiBodyLinkCollider {0} to world", link);
}
m_world.AddCollisionObject(link.GetLinkCollider(), link.groupsIBelongTo, link.collisionMask);
link.isInWorld = true;
BMultiBodyConstraint bmbc = link.GetComponent <BMultiBodyConstraint>();
if (bmbc != null)
{
MultiBodyConstraint mbc = bmbc.CreateMultiBodyConstraint(mb.GetMultiBody());
mbc.FinalizeMultiDof();
if (mbc != null)
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Adding MultiBodyLinkConstraint {0} to world", mbc);
}
((MultiBodyDynamicsWorld)m_world).AddMultiBodyConstraint(mbc);
bmbc.isInWorld = true;
}
}
}
MultiBody mbb = mb.GetMultiBody();
for (int i = 0; i < links.Count; i++)
{
MultiBodyLink mbLink = mbb.GetLink(i);
links[i].AssignMultiBodyLinkOnCreation(mb, mbLink);
}
mb.isInWorld = true;
}
else
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogWarningFormat("Failed To Add MultiBody {0} to world ", mb);
}
}
return(true);
}
return(false);
}
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;
}