public MultiBodySliderConstraint(MultiBody bodyA, int linkA, MultiBody bodyB,
int linkB, Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB,
Vector3 jointAxis)
: base(btMultiBodySliderConstraint_new2(bodyA._native, linkA, bodyB._native,
linkB, ref pivotInA, ref pivotInB, ref frameInA, ref frameInB, ref jointAxis))
{
}
public MultiBodyJointLimitConstraint(MultiBody body, int link, float lower,
float upper)
: base(btMultiBodyJointLimitConstraint_new(body._native, link, lower,
upper))
{
_multiBodyA = body;
_multiBodyB = body;
}
public MultiBodyJointMotor(MultiBody body, int link, int linkDoF, float desiredVelocity,
float maxMotorImpulse)
: base(btMultiBodyJointMotor_new2(body._native, link, linkDoF, desiredVelocity,
maxMotorImpulse))
{
_multiBodyA = body;
_multiBodyB = body;
}
public MultiBodyPoint2Point(MultiBody bodyA, int linkA, MultiBody bodyB,
int linkB, Vector3 pivotInA, Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new2(bodyA._native, linkA, bodyB._native,
linkB, ref pivotInA, ref pivotInB))
{
_multiBodyA = bodyA;
_multiBodyB = bodyB;
}
public MultiBodyJointMotor(MultiBody body, int link, int linkDoF, float desiredVelocity, float maxMotorImpulse)
: base(btMultiBodyJointMotor_new2(body._native, link, linkDoF, desiredVelocity, maxMotorImpulse))
{
_multiBodyA = body;
_multiBodyB = body;
}
public MultiBodySliderConstraint(MultiBody body, int link, RigidBody bodyB,
Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB, Vector3 jointAxis)
: base(btMultiBodySliderConstraint_new(body.Native, link, bodyB.Native,
ref pivotInA, ref pivotInB, ref frameInA, ref frameInB, ref jointAxis), body, null)
{
}
internal MultiBodyConstraint(IntPtr native, MultiBody bodyA, MultiBody bodyB)
{
Native = native;
MultiBodyA = bodyA;
MultiBodyB = bodyB;
}
public void AddMultiBody(MultiBody body)
{
btMultiBodyDynamicsWorld_addMultiBody(_native, body._native);
_bodies.Add(body);
}
public MultiBodyPoint2Point(MultiBody body, int link, RigidBody bodyB, Vector3 pivotInA, Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new(body._native, link, bodyB._native, ref pivotInA, ref pivotInB))
{
_multiBodyA = body;
}
public MultiBodyLinkCollider(MultiBody multiBody, int link)
: base(btMultiBodyLinkCollider_new(multiBody._native, link))
{
_multiBody = multiBody;
}
public MultiBodyFixedConstraint(MultiBody bodyA, int linkA, MultiBody bodyB,
int linkB, Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB)
: base(btMultiBodyFixedConstraint_new2(bodyA._native, linkA, bodyB._native,
linkB, ref pivotInA, ref pivotInB, ref frameInA, ref frameInB))
{
}
internal bool _BuildMultiBody()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_multibody != null && isInWorld && world != null)
{
isInWorld = false;
world.RemoveMultiBody(this);
}
if (transform.localScale != UnityEngine.Vector3.one)
{
Debug.LogErrorFormat("The local scale on {0} rigid body is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.", name);
}
if (!fixedBase && baseMass <= 0f)
{
Debug.LogErrorFormat("If not fixed base then baseMass must be greater than zero.");
return(false);
}
m_baseCollisionShape = GetComponent <BCollisionShape>();
if (m_baseCollisionShape == null)
{
Debug.LogErrorFormat("There was no collision shape component attached to this BMultiBody. {0}", name);
return(false);
}
if (GetComponent <BMultiBodyLink>() != null)
{
Debug.LogErrorFormat("There must not be a BMultiBodyLink component attached to the gameObject with a BMultiBody component. {0}", name);
return(false);
}
m_links = new List <BMultiBodyLink>();
if (!GetLinksInChildrenAndNumber(transform, m_links, -1))
{
Debug.LogError("Error building multibody");
return(false);
}
if (m_links.Count == 0)
{
Debug.LogError("Could not find any links");
return(false);
}
if (!ValidateMultiBodyHierarchy(m_links))
{
return(false);
}
BCollisionShape[] shapes = new BCollisionShape[m_links.Count];
BMultiBodyConstraint[] constraints = new BMultiBodyConstraint[m_links.Count];
for (int i = 0; i < m_links.Count; i++)
{
shapes[i] = m_links[i].GetComponent <BCollisionShape>();
if (shapes[i] == null && shapes[i].GetComponent <RigidBody>() != null)
{
Debug.LogErrorFormat("BMultiBodyLink must not have a RigidBody component. {0}", m_links[i]);
return(false);
}
constraints[i] = m_links[i].GetComponent <BMultiBodyConstraint>();
}
BulletSharp.MultiBody mb = m_multibody;
CreateOrConfigureMultiBody(ref mb, baseMass, shapes, constraints);
m_multibody = mb;
//TODO is this allowed
//m_multibody.UserObject = this;
return(true);
}
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, int numLinks,
Vector3 basePosition, Vector3 baseHalfExtents, Vector3 linkHalfExtents, bool spherical, bool floating)
{
float mass = 1;
Vector3 inertia = Vector3.Zero;
if (mass != 0)
{
using (var box = new BoxShape(baseHalfExtents))
{
box.CalculateLocalInertia(mass, out inertia);
}
}
var mb = new MultiBody(numLinks, mass, inertia, !floating, false);
//body.HasSelfCollision = false;
//body.BaseVelocity = Vector3.Zero;
mb.BasePosition = basePosition;
//body.WorldToBaseRot = new Quaternion(0, 0, 1, -0.125f * (float)Math.PI);
mb.WorldToBaseRot = Quaternion.Identity;
float linkMass = 1;
Vector3 linkInertia = Vector3.Zero;
if (linkMass != 0)
{
using (var box = new BoxShape(linkHalfExtents))
{
box.CalculateLocalInertia(linkMass, out linkInertia);
}
}
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
for (int i = 0; i < numLinks; i++)
{
if (spherical)
{
mb.SetupSpherical(i, linkMass, linkInertia, i - 1,
Quaternion.Identity, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
else
{
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
mb.SetupRevolute(i, linkMass, linkInertia, i - 1,
Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
mb.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(mb);
return mb;
}
public MultiBodyPoint2Point(MultiBody body, int link, RigidBody bodyB, Vector3 pivotInA,
Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new(body._native, link, bodyB._native,
ref pivotInA, ref pivotInB))
{
}
public MultiBodyPoint2Point(MultiBody body, int link, RigidBody bodyB, Vector3 pivotInA,
Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new(body.Native, link, bodyB != null ? bodyB.Native : IntPtr.Zero,
ref pivotInA, ref pivotInB), body, null)
{
}
public MultiBodyPoint2Point(MultiBody bodyA, int linkA, MultiBody bodyB,
int linkB, Vector3 pivotInA, Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new2(bodyA.Native, linkA, bodyB.Native,
linkB, ref pivotInA, ref pivotInB), bodyA, bodyB)
{
}
public MultiBodyFixedConstraint(MultiBody bodyA, int linkA, MultiBody bodyB,
int linkB, Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB)
: base(btMultiBodyFixedConstraint_new2(bodyA._native, linkA, bodyB._native,
linkB, ref pivotInA, ref pivotInB, ref frameInA, ref frameInB))
{
}
public MultiBodyFixedConstraint(MultiBody body, int link, RigidBody bodyB,
Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB)
: base(btMultiBodyFixedConstraint_new(body._native, link, bodyB._native,
ref pivotInA, ref pivotInB, ref frameInA, ref frameInB))
{
}
public MultiBodyPoint2Point(MultiBody bodyA, int linkA, MultiBody bodyB, int linkB, Vector3 pivotInA, Vector3 pivotInB)
: base(btMultiBodyPoint2Point_new2(bodyA._native, linkA, bodyB._native, linkB, ref pivotInA, ref pivotInB))
{
_multiBodyA = bodyA;
_multiBodyB = bodyB;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -9.81f, 0);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//multiBody.UseRK4Integration = true;
//multiBody.BaseWorldTransform = Matrix.Identity;
//init the links
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for(int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
multiBody.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
multiBody.CanSleep = canSleep;
multiBody.HasSelfCollision = selfCollide;
multiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
#else
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
CollisionShapes.Add(shape);
var col = new MultiBodyLinkCollider(multiBody, i);
col.CollisionShape = shape;
const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
public void AddMultiBody(MultiBody body)
{
btMultiBodyDynamicsWorld_addMultiBody(_native, body._native);
_bodies.Add(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;
}
}
public void AddMultiBody(MultiBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, (short)group,
(short)mask);
_bodies.Add(body);
}
public MultiBodyFixedConstraint(MultiBody body, int link, RigidBody bodyB,
Vector3 pivotInA, Vector3 pivotInB, Matrix frameInA, Matrix frameInB)
: base(btMultiBodyFixedConstraint_new(body._native, link, bodyB._native,
ref pivotInA, ref pivotInB, ref frameInA, ref frameInB))
{
}
public void AddMultiBody(MultiBody body, short group, short mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, group,
mask);
_bodies.Add(body);
}
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;
}
public void RemoveMultiBody(MultiBody body)
{
btMultiBodyDynamicsWorld_removeMultiBody(_native, body._native);
_bodies.Remove(body);
}
public void AddMultiBody(MultiBody body, short group, short mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, group,
mask);
_bodies.Add(body);
}
public MultiBodyLinkCollider(MultiBody multiBody, int link)
: base(btMultiBodyLinkCollider_new(multiBody._native, link))
{
_multiBody = multiBody;
}
public void RemoveMultiBody(MultiBody body)
{
btMultiBodyDynamicsWorld_removeMultiBody(_native, body._native);
_bodies.Remove(body);
}
public MultiBodyJointMotor(MultiBody body, int link, double desiredVelocity,
double maxMotorImpulse)
: base(btMultiBodyJointMotor_new(body.Native, link, desiredVelocity,
maxMotorImpulse), body, body)
{
}
public void AddMultiBody(MultiBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
btMultiBodyDynamicsWorld_addMultiBody3(_native, body._native, (short)group, (short)mask);
_bodies.Add(body);
}
public MultiBodyJointMotor(MultiBody body, int link, int linkDoF, Scalar desiredVelocity,
Scalar maxMotorImpulse)
: base(btMultiBodyJointMotor_new2(body.Native, link, linkDoF, desiredVelocity,
maxMotorImpulse), body, body)
{
}