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 FeatherStoneDemoSimulation()
{
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
Broadphase = new DbvtBroadphase();
_solver = new MultiBodyConstraintSolver();
MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration);
CreateGround();
int numLinks = 5;
bool spherical = true;
bool floatingBase = false;
var basePosition = new Vector3(-0.4f, 3.0f, 0.0f);
var baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
var linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
var multiBody = CreateFeatherstoneMultiBody(MultiBodyWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase);
bool damping = true;
if (damping)
{
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
}
else
{
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
}
if (numLinks > 0)
{
float q0 = 45.0f * (float)Math.PI / 180.0f;
if (spherical)
{
Quaternion quat0 = Quaternion.RotationAxis(Vector3.Normalize(new Vector3(1, 1, 0)), q0);
quat0.Normalize();
multiBody.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W });
}
else
{
multiBody.SetJointPosMultiDof(0, new float[] { q0 });
}
}
CreateColliders(multiBody, baseHalfExtents, linkHalfExtents);
CreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f));
}
//GUIHelperInterface m_guiHelper;
public CommonMultiBodyBase()
{
//m_filterCallback = null;
m_pairCache = null;
m_broadphase = null;
m_dispatcher = null;
m_solver = null;
m_collisionConfiguration = null;
m_dynamicsWorld = null;
//m_pickedBody = null;
//m_pickedConstraint = null;
//m_pickingMultiBodyPoint2Point = null;
m_prevCanSleep = false;
//m_guiHelper = null;
}
private static void CleanupMultiBodyWorld(MultiBodyDynamicsWorld world)
{
for (int i = world.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint multiBodyConstraint = world.GetMultiBodyConstraint(i);
world.RemoveMultiBodyConstraint(multiBodyConstraint);
multiBodyConstraint.Dispose();
}
for (int i = world.NumMultibodies - 1; i >= 0; i--)
{
MultiBody multiBody = world.GetMultiBody(i);
world.RemoveMultiBody(multiBody);
multiBody.Dispose();
}
}
public Physics()
{
ActiveBodies = new List <PhysicalBody>();
AddBodyQueue = new List <PhysicalBody>();
RemoveBodyQueue = new List <PhysicalBody>();
collisionConf = new DefaultCollisionConfiguration();
dispatcher = new CollisionDispatcher(collisionConf);
broadphase = new DbvtBroadphase();
var w = new MultiBodyDynamicsWorld(dispatcher, broadphase, new MultiBodyConstraintSolver(), collisionConf);
w.SolverInfo.SolverMode = SolverModes.CacheFriendly;
w.SolverInfo.Restitution = 0;
w.Gravity = new Vector3(0, -9.81f, 0);
World = w;
}
public virtual void createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
Dispatcher = new CollisionDispatcher(CollisionConf);
m_pairCache = new HashedOverlappingPairCache();
Broadphase = new DbvtBroadphase(m_pairCache);
m_solver = new MultiBodyConstraintSolver();
World = m_dynamicsWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, m_solver, CollisionConf);
m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0));
}
public virtual void createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
//m_collisionConfiguration.setConvexConvexMultipointIterations();
//m_filterCallback = new MyOverlapFilterCallback2();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_pairCache = new HashedOverlappingPairCache();
//m_pairCache.OverlapFilterCallback = (m_filterCallback);
m_broadphase = new DbvtBroadphase(m_pairCache); //btSimpleBroadphase();
m_solver = new MultiBodyConstraintSolver();
m_dynamicsWorld = new MultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld.Gravity = (new BulletSharp.Math.Vector3(0, -10, 0));
}
public override void ExitPhysics()
{
if (m_inverseModel != null)
{
Debug.Log("Dispose inverse model " + m_inverseModel.NumBodies);
m_inverseModel.Dispose();
}
Debug.Log("InverseDynamicsExitPhysics");
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld == null)
{
int i;
for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint tc = m_dynamicsWorld.GetConstraint(i);
m_dynamicsWorld.RemoveConstraint(tc);
tc.Dispose();
}
for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i);
m_dynamicsWorld.RemoveMultiBodyConstraint(mbc);
mbc.Dispose();
}
for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--)
{
MultiBody mb = m_dynamicsWorld.GetMultiBody(i);
m_dynamicsWorld.RemoveMultiBody(mb);
mb.Dispose();
}
for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i];
RigidBody body = RigidBody.Upcast(obj);
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
m_dynamicsWorld.RemoveCollisionObject(obj);
obj.Dispose();
}
}
if (m_multiBody != null)
{
m_multiBody.Dispose();
}
//delete collision shapes
for (int j = 0; j < CollisionShapes.Count; j++)
{
CollisionShape shape = CollisionShapes[j];
shape.Dispose();
}
CollisionShapes.Clear();
m_dynamicsWorld.Dispose();
m_dynamicsWorld = null;
m_solver.Dispose();
m_solver = null;
Broadphase.Dispose();
Broadphase = null;
Dispatcher.Dispose();
Dispatcher = null;
m_pairCache.Dispose();
m_pairCache = null;
CollisionConf.Dispose();
CollisionConf = null;
Debug.Log("After dispose B");
}
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, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -51.55f, 0), groundShape);
ground.UserObject = "Ground";
int numLinks = 5;
bool spherical = true;
bool floatingBase = false;
Vector3 basePosition = new Vector3(-0.4f, 3.0f, 0.0f);
Vector3 baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
Vector3 linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
var mb = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase);
floatingBase = !floatingBase;
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;
}
if (numLinks > 0)
{
float q0 = 45.0f * (float)Math.PI / 180.0f;
if (spherical)
{
Quaternion quat0 = Quaternion.RotationAxis(new Vector3(1, 1, 0).Normalized, q0);
quat0.Normalize();
mb.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W });
}
else
{
mb.SetJointPosMultiDof(0, new float[] { q0 });
}
}
AddColliders(mb, baseHalfExtents, linkHalfExtents);
LocalCreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f));
}
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 = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
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, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
const float mass = 1.0f;
BoxShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
const float start_x = StartPosX - ArraySizeX / 2;
const float start_y = StartPosY;
const float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
3 * i + start_x,
3 * k + start_y,
3 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
World.AddRigidBody(body);
}
}
}
var settings = new MultiBodySettings()
{
BasePosition = new Vector3(60, 29.5f, -2) * Scaling,
CanSleep = true,
CreateConstraints = true,
DisableParentCollision = true, // the self-collision has conflicting/non-resolvable contact normals
IsFixedBase = false,
NumLinks = 2,
UsePrismatic = true
};
var multiBodyA = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.NumLinks = 10;
settings.BasePosition = new Vector3(0, 29.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = false;
var multiBodyB = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20 * Scaling, 29.5f * Scaling, -settings.NumLinks * 4 * Scaling);
settings.IsFixedBase = false;
var multiBodyC = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20, 9.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = true;
settings.DisableParentCollision = true;
var multiBodyPrim = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
}
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 virtual void exitPhysics()
{
removePickingConstraint();
//cleanup in the reverse order of creation/initialization
//remove the rigidbodies from the dynamics world and delete them
if (m_dynamicsWorld == null)
{
int i;
for (i = m_dynamicsWorld.NumConstraints - 1; i >= 0; i--)
{
m_dynamicsWorld.RemoveConstraint(m_dynamicsWorld.GetConstraint(i));
}
for (i = m_dynamicsWorld.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint mbc = m_dynamicsWorld.GetMultiBodyConstraint(i);
m_dynamicsWorld.RemoveMultiBodyConstraint(mbc);
mbc.Dispose();
}
for (i = m_dynamicsWorld.NumMultibodies - 1; i >= 0; i--)
{
MultiBody mb = m_dynamicsWorld.GetMultiBody(i);
m_dynamicsWorld.RemoveMultiBody(mb);
mb.Dispose();
}
for (i = m_dynamicsWorld.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_dynamicsWorld.CollisionObjectArray[i];
RigidBody body = RigidBody.Upcast(obj);
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
m_dynamicsWorld.RemoveCollisionObject(obj);
obj.Dispose();
}
}
//delete collision shapes
for (int j = 0; j < m_collisionShapes.Count; j++)
{
CollisionObject shape = m_collisionShapes[j];
shape.Dispose();
}
m_collisionShapes.Clear();
m_dynamicsWorld.Dispose();
m_dynamicsWorld = null;
m_solver.Dispose();
m_solver = null;
m_broadphase.Dispose();
m_broadphase = null;
m_dispatcher.Dispose();
m_dispatcher = null;
m_pairCache.Dispose();
m_pairCache = null;
//m_filterCallback.Dispose();
//m_filterCallback = null;
m_collisionConfiguration.Dispose();
m_collisionConfiguration = null;
}
public void ConvertURDF2BulletInternal(
URDFImporterInterface u2b,
MultiBodyCreationInterface creation,
URDF2BulletCachedData cache, int urdfLinkIndex,
Matrix parentTransformInWorldSpace,
GameObject parentGameObject,
MultiBodyDynamicsWorld world1,
bool createMultiBody, string pathPrefix,
bool enableConstraints,
ConvertURDFFlags flags = 0)
{
Matrix linkTransformInWorldSpace = Matrix.Identity;
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
RigidBody parentRigidBody = null;
//b3Printf();
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("mb link index = {0}\n", mbLinkIndex);
}
Matrix parentLocalInertialFrame = Matrix.Identity;
float parentMass = (1);
BulletSharp.Math.Vector3 parentLocalInertiaDiagonal = new BulletSharp.Math.Vector3(1, 1, 1);
if (urdfParentIndex == -2)
{
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("root link has no parent\n");
}
}
else
{
if (debugLevel >= BDebug.DebugType.Debug)
{
Debug.LogFormat("urdf parent index = {0}", urdfParentIndex);
Debug.LogFormat("mb parent index = {0}", mbParentIndex);
}
parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia(urdfParentIndex, out parentMass, out parentLocalInertiaDiagonal, out parentLocalInertialFrame);
}
float mass = 0;
Matrix localInertialFrame = Matrix.Identity;
BulletSharp.Math.Vector3 localInertiaDiagonal = new BulletSharp.Math.Vector3(0, 0, 0);
u2b.getMassAndInertia(urdfLinkIndex, out mass, out localInertiaDiagonal, out localInertialFrame);
Matrix parent2joint = Matrix.Identity;
UrdfJointTypes jointType;
BulletSharp.Math.Vector3 jointAxisInJointSpace;
float jointLowerLimit;
float jointUpperLimit;
float jointDamping;
float jointFriction;
float jointMaxForce;
float jointMaxVelocity;
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
string linkName = u2b.getLinkName(urdfLinkIndex);
if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
{
Matrix tmp = new Matrix();
Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
}
else
{
if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
{
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
}
else
{
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
}
}
GameObject gameObject = new GameObject(linkName);
if (parentGameObject != null)
{
gameObject.transform.parent = parentGameObject.transform;
}
//--------------------
/*
* bool hasParentJoint = loader.getJointInfo2(linkIndex, out parent2joint, out linkTransformInWorldSpace, out jointAxisInJointSpace, out jointType, out jointLowerLimit, out jointUpperLimit, out jointDamping, out jointFriction, out jointMaxForce, out jointMaxVelocity);
* string linkName = loader.getLinkName(linkIndex);
*
* if ((flags & ConvertURDFFlags.CUF_USE_SDF) != 0)
* {
* Matrix tmp = parentTransformInWorldSpace.Inverse();
* Matrix.Multiply(ref tmp, ref linkTransformInWorldSpace, out parent2joint);
* }
* else
* {
* if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
* {
* linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
* }
* else
* {
* linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
* }
* }
*
* lgo.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
*/
//--------------------
gameObject.transform.position = linkTransformInWorldSpace.Origin.ToUnity();
gameObject.transform.rotation = linkTransformInWorldSpace.Rotation.ToUnity();
BCollisionShape compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame, gameObject);
int graphicsIndex;
{
graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex, pathPrefix, ref localInertialFrame);
}
if (compoundShape != null)
{
UrdfMaterialColor matColor;
Color color2 = Color.red;
Color specular = new Color(0.5f, 0.5f, 0.5f);
if (u2b.getLinkColor2(urdfLinkIndex, out matColor))
{
color2 = matColor.m_rgbaColor;
specular = matColor.m_specularColor;
}
if (mass != 0)
{
if ((flags & ConvertURDFFlags.CUF_USE_URDF_INERTIA) == 0)
{
compoundShape.GetCollisionShape().CalculateLocalInertia(mass, out localInertiaDiagonal);
Debug.Assert(localInertiaDiagonal[0] < 1e10);
Debug.Assert(localInertiaDiagonal[1] < 1e10);
Debug.Assert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
//temporary inertia scaling until we load inertia from URDF
if ((contactInfo.m_flags & URDF_LinkContactFlags.URDF_CONTACT_HAS_INERTIA_SCALING) != 0)
{
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
}
}
RigidBody linkRigidBody = null;
Matrix inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
if (!createMultiBody)
{
RigidBody body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape.GetCollisionShape());
linkRigidBody = body;
world1.AddRigidBody(body);
compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
ProcessContactParameters(contactInfo, body);
creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2, specular, graphicsIndex);
cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
}
else
{
if (cache.m_bulletMultiBody == null)
{
// creating base
bool canSleep = false;
bool isFixedBase = (mass == 0);//todo: figure out when base is fixed
int totalNumJoints = cache.m_totalNumJoints1;
//cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
BMultiBody bmm = cache.m_bulletMultiBody = gameObject.AddComponent <BMultiBody>();
bmm.fixedBase = isFixedBase;
bmm.canSleep = canSleep;
bmm.baseMass = mass;
//new btMultiBody(totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
//if ((flags & ConvertURDFFlags.CUF_USE_MJCF) != 0)
//{
// cache.m_bulletMultiBody.BaseWorldTransform = (linkTransformInWorldSpace);
//}
//cache.registerMultiBody(urdfLinkIndex, cache.m_bulletMultiBody, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape.GetCollisionShape(), ref localInertialFrame);
}
}
//create a joint if necessary
BMultiBodyLink bmbl = null;
if (hasParentJoint)
{
//====================
//btTransform offsetInA, offsetInB;
//offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
//offsetInB = localInertialFrame.inverse();
//btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
//=====================
Matrix offsetInA = new Matrix(), offsetInB = new Matrix();
Matrix.Invert(ref parentLocalInertialFrame, out offsetInA);
offsetInA = offsetInA * parent2joint;
Matrix.Invert(ref localInertialFrame, out offsetInB);
Matrix offsetInAInv = new Matrix();
Matrix.Invert(ref offsetInA, out offsetInAInv);
BulletSharp.Math.Quaternion parentRotToThis = offsetInB.GetRotation() * offsetInAInv.GetRotation();
Matrix tmp = new Matrix();
Matrix.Invert(ref parentTransformInWorldSpace, out tmp);
Matrix tmp2 = new Matrix();
Matrix.Invert(ref parent2joint, out tmp2);
Matrix tmp3 = new Matrix();
Matrix tmp4 = linkTransformInWorldSpace;
Matrix link2joint = new Matrix();
Matrix.Multiply(ref tmp, ref tmp2, out tmp3);
Matrix.Multiply(ref tmp3, ref tmp4, out link2joint);
if (debugLevel >= BDebug.DebugType.Debug)
{
Matrix linkTransformInWorldSpaceInv = new Matrix(), parentTransformInWorldSpaceInv = new Matrix();
Matrix.Invert(ref linkTransformInWorldSpace, out linkTransformInWorldSpaceInv);
Matrix.Invert(ref parentTransformInWorldSpace, out parentTransformInWorldSpaceInv);
Debug.Log("Creating link " + linkName + " offsetInA=" + offsetInA.Origin + " offsetInB=" + offsetInB.Origin + " parentLocalInertialFrame=" + parentLocalInertialFrame.Origin + " localInertialFrame=" + localInertialFrame.Origin + " localTrnaform=" +
" linkTransInWorldSpace=" + linkTransformInWorldSpace.Origin +
" linkTransInWorldSpaceInv=" + linkTransformInWorldSpaceInv.Origin +
" parentTransInWorldSpace=" + parentTransformInWorldSpace.Origin +
" parentTransInWorldSpaceInv=" + parentTransformInWorldSpaceInv.Origin +
" link2joint=" + link2joint.Origin +
" jointType=" + jointType);
}
bool disableParentCollision = true;
switch (jointType)
{
case UrdfJointTypes.URDFFloatingJoint:
case UrdfJointTypes.URDFPlanarJoint:
case UrdfJointTypes.URDFFixedJoint:
{
if ((jointType == UrdfJointTypes.URDFFloatingJoint) || (jointType == UrdfJointTypes.URDFPlanarJoint))
{
Debug.Log("Warning: joint unsupported, creating a fixed joint instead.");
}
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//todo: adjust the center of mass transform and pivot axis properly
/*
* cache.m_bulletMultiBody.SetupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
* parentRotToThis, offsetInA.Origin, -offsetInB.Origin);
*/
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Fixed;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
}
else
{
//b3Printf("Fixed joint\n");
Debug.LogError("TODO Setup 6dof ");
/*
* Generic6DofSpring2Constraint dof6 = null;
*
* //backward compatibility
* if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
* {
* dof6 = creation.createFixedJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB);
* }
* else
* {
* dof6 = creation.createFixedJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA);
* }
* if (enableConstraints)
* world1.AddConstraint(dof6, true);
*/
}
break;
}
case UrdfJointTypes.URDFContinuousJoint:
case UrdfJointTypes.URDFRevoluteJoint:
{
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//cache.m_bulletMultiBody.SetupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
// parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
// -offsetInB.Origin,
// disableParentCollision);
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Revolute;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();
if (jointType == UrdfJointTypes.URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
{
//string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
mbc.m_jointLowerLimit = jointLowerLimit;
mbc.m_jointUpperLimit = jointUpperLimit;
}
/*
* Debug.Log("=========== Creating joint for: " + gameObject.name);
* Debug.Log("parentRotateToThis: " + parentRotToThis.ToUnity().eulerAngles);
* Debug.Log("rotationAxis: " + bmbl.rotationAxis);
* Debug.Log("offsetInA: " + offsetInA.Origin);
* Debug.Log("negOffsetInB: " + -offsetInB.Origin);
*/
}
else
{
Generic6DofSpring2Constraint dof6 = null;
//backwards compatibility
if ((flags & ConvertURDFFlags.CUF_RESERVED) != 0)
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
else
{
dof6 = creation.createRevoluteJoint(urdfLinkIndex, linkRigidBody, parentRigidBody, offsetInB, offsetInA, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
}
if (enableConstraints)
{
world1.AddConstraint(dof6, true);
}
//b3Printf("Revolute/Continuous joint\n");
}
break;
}
case UrdfJointTypes.URDFPrismaticJoint:
{
//creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
if (createMultiBody)
{
//cache.m_bulletMultiBody.SetupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
// parentRotToThis, (offsetInB.GetRotation().Rotate(jointAxisInJointSpace)), offsetInA.Origin,//parent2joint.getOrigin(),
// -offsetInB.Origin,
// disableParentCollision);
bmbl = gameObject.AddComponent <BMultiBodyLink>();
bmbl.jointType = FeatherstoneJointType.Prismatic;
bmbl.mass = mass;
bmbl.localPivotPosition = offsetInB.Origin.ToUnity();
bmbl.rotationAxis = offsetInB.Rotation.Rotate(jointAxisInJointSpace).ToUnity();
if (jointLowerLimit <= jointUpperLimit)
{
//string name = u2b.getLinkName(urdfLinkIndex);
//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
BMultiBodyJointLimitConstraint mbc = gameObject.AddComponent <BMultiBodyJointLimitConstraint>();
mbc.m_jointLowerLimit = jointLowerLimit;
mbc.m_jointUpperLimit = jointUpperLimit;
}
//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
}
else
{
Generic6DofSpring2Constraint dof6 = creation.createPrismaticJoint(urdfLinkIndex, parentRigidBody, linkRigidBody, offsetInA, offsetInB, jointAxisInJointSpace, jointLowerLimit, jointUpperLimit);
if (enableConstraints)
{
world1.AddConstraint(dof6, true);
}
//b3Printf("Prismatic\n");
}
break;
}
default:
{
//b3Printf("Error: unsupported joint type in URDF (%d)\n", jointType);
Debug.Assert(false);
break;
}
}
}
if (createMultiBody)
{
if (bmbl != null)
{
bmbl.jointDamping = jointDamping;
bmbl.jointFriction = jointFriction;
//bmbl.jointLowerLimit = jointLowerLimit;
//bmbl.jointUpperLimit = jointUpperLimit;
//bmbl.jointMaxForce = jointMaxForce;
//bmbl.jointMaxVelocity = jointMaxVelocity;
}
{
/*
* MultiBodyLinkCollider col = creation.allocateMultiBodyLinkCollider(urdfLinkIndex, mbLinkIndex, cache.m_bulletMultiBody);
*
* compoundShape.GetCollisionShape().UserIndex = (graphicsIndex);
*
* col.CollisionShape = (compoundShape.GetCollisionShape());
*
* Matrix tr = Matrix.Identity;
*
* tr = linkTransformInWorldSpace;
* //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
*
* col.WorldTransform = (tr);
*
* //base and fixed? . static, otherwise flag as dynamic
* bool isDynamic = (mbLinkIndex < 0 && cache.m_bulletMultiBody.HasFixedBase) ? false : true;
* CollisionFilterGroups collisionFilterGroup = isDynamic ? (CollisionFilterGroups.DefaultFilter) : (CollisionFilterGroups.StaticFilter);
* CollisionFilterGroups collisionFilterMask = isDynamic ? (CollisionFilterGroups.AllFilter) : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
*
* CollisionFilterGroups colGroup = 0, colMask = 0;
* UrdfCollisionFlags collisionFlags = u2b.getCollisionGroupAndMask(urdfLinkIndex, out colGroup, out colMask);
* if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_GROUP) != 0)
* {
* collisionFilterGroup = colGroup;
* }
* if ((collisionFlags & UrdfCollisionFlags.URDF_HAS_COLLISION_MASK) != 0)
* {
* collisionFilterMask = colMask;
* }
* world1.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
*
* color2 = Color.red;//(0.0,0.0,0.5);
* Color specularColor = new Color(1, 1, 1);
* UrdfMaterialColor matCol;
* if (u2b.getLinkColor2(urdfLinkIndex, out matCol))
* {
* color2 = matCol.m_rgbaColor;
* specularColor = matCol.m_specularColor;
* }
* {
*
*
* creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex, col, color2, specularColor);
* }
* {
*
*
* u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, ref localInertialFrame, col, u2b.getBodyUniqueId());
* }
* URDFLinkContactInfo contactInfo;
* u2b.getLinkContactInfo(urdfLinkIndex, out contactInfo);
*
*
* ProcessContactParameters(contactInfo, col);
*
* if (mbLinkIndex >= 0) //???? double-check +/- 1
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Collider = col;
* if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_PARENT) != 0)
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
* }
* if ((flags & ConvertURDFFlags.CUF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) != 0)
* {
* cache.m_bulletMultiBody.GetLink(mbLinkIndex).Flags |= (int)btMultiBodyLinkFlags.BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION;
* }
* }
* else
* {
* cache.m_bulletMultiBody.BaseCollider = (col);
* }
*/
}
}
else
{
//u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
}
}
List <int> urdfChildIndices = new List <int>();
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.Count;
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2BulletInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, gameObject, world1, createMultiBody, pathPrefix, enableConstraints, flags);
}
}
/*
* Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation.
*/
public bool CreatePhysicsWorld(out CollisionWorld world,
out CollisionConfiguration collisionConfig,
out CollisionDispatcher dispatcher,
out BroadphaseInterface broadphase,
out ConstraintSolver solver,
out SoftBodyWorldInfo softBodyWorldInfo)
{
bool success = true;
if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType=SoftBodyRigidBodyCollisionConf. Switching");
m_collisionType = CollisionConfType.SoftBodyRigidBodyCollisionConf;
success = false;
}
collisionConfig = null;
if (m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
collisionConfig = new DefaultCollisionConfiguration();
}
else if (m_collisionType == CollisionConfType.SoftBodyRigidBodyCollisionConf)
{
collisionConfig = new SoftBodyRigidBodyCollisionConfiguration();
}
dispatcher = new CollisionDispatcher(collisionConfig);
if (m_broadphaseType == BroadphaseType.DynamicAABBBroadphase)
{
broadphase = new DbvtBroadphase();
}
else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase)
{
broadphase = new AxisSweep3(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
}
else if (m_broadphaseType == BroadphaseType.Axis3SweepBroadphase_32bit)
{
broadphase = new AxisSweep3_32Bit(m_axis3SweepBroadphaseMin.ToBullet(), m_axis3SweepBroadphaseMax.ToBullet(), axis3SweepMaxProxies);
}
else
{
broadphase = null;
}
world = null;
softBodyWorldInfo = null;
solver = null;
if (m_worldType == WorldType.CollisionOnly)
{
world = new CollisionWorld(dispatcher, broadphase, collisionConfig);
}
else if (m_worldType == WorldType.RigidBodyDynamics)
{
switch (solverType)
{
case SolverType.MLCP:
DantzigSolver dtsolver = new DantzigSolver();
solver = new MlcpSolver(dtsolver);
break;
default:
break;
}
world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfig);
}
else if (m_worldType == WorldType.MultiBodyWorld)
{
MultiBodyConstraintSolver mbConstraintSolver = null;
switch (solverType)
{
/* case SolverType.MLCP:
* DantzigSolver dtsolver = new DantzigSolver();
* mbConstraintSolver = new MultiBodyMLCPConstraintSolver(dtsolver);
* break;*/
default:
mbConstraintSolver = new MultiBodyConstraintSolver();
break;
}
world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig);
}
else if (m_worldType == WorldType.SoftBodyAndRigidBody)
{
SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver();
constraintSolver = siConstraintSolver;
siConstraintSolver.RandSeed = sequentialImpulseConstraintSolverRandomSeed;
m_world = new SoftRigidDynamicsWorld(Dispatcher, Broadphase, siConstraintSolver, CollisionConf);
_ddWorld = (DiscreteDynamicsWorld)m_world;;
SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)m_world;
m_world.DispatchInfo.EnableSpu = true;
_sworld.WorldInfo.SparseSdf.Initialize();
_sworld.WorldInfo.SparseSdf.Reset();
_sworld.WorldInfo.AirDensity = 1.2f;
_sworld.WorldInfo.WaterDensity = 0;
_sworld.WorldInfo.WaterOffset = 0;
_sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
_sworld.WorldInfo.Gravity = m_gravity.ToBullet();
}
if (world is DiscreteDynamicsWorld)
{
((DiscreteDynamicsWorld)world).Gravity = m_gravity.ToBullet();
}
if (_doDebugDraw)
{
DebugDrawUnity db = new DebugDrawUnity();
db.DebugMode = _debugDrawMode;
world.DebugDrawer = db;
}
return(success);
}
//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);
}
private void CreateWorld()
{
if (physicWorldParameters.worldType == WorldType.SoftBodyAndRigidBody && physicWorldParameters.collisionType != CollisionConfType.SoftBodyRigidBodyCollisionConf)
{
Debug.LogError("For World Type = SoftBodyAndRigidBody collisionType must be collisionType = SoftBodyRigidBodyCollisionConf.");
return;
}
isDisposed = false;
switch (physicWorldParameters.collisionType)
{
case CollisionConfType.DefaultDynamicsWorldCollisionConf:
collisionConf = new DefaultCollisionConfiguration();
break;
case CollisionConfType.SoftBodyRigidBodyCollisionConf:
collisionConf = new SoftBodyRigidBodyCollisionConfiguration();
break;
}
dispatcher = new CollisionDispatcher(collisionConf);
switch (physicWorldParameters.broadphaseType)
{
default:
case BroadphaseType.DynamicAABBBroadphase:
broadphase = new DbvtBroadphase();
break;
case BroadphaseType.Axis3SweepBroadphase:
broadphase = new AxisSweep3(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies);
break;
case BroadphaseType.Axis3SweepBroadphase_32bit:
broadphase = new AxisSweep3_32Bit(physicWorldParameters.axis3SweepBroadphaseMin, physicWorldParameters.axis3SweepBroadphaseMax, physicWorldParameters.axis3SweepMaxProxies);
break;
}
switch (physicWorldParameters.worldType)
{
case WorldType.CollisionOnly:
World = new CollisionWorld(dispatcher, broadphase, collisionConf);
break;
case WorldType.RigidBodyDynamics:
World = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConf);
break;
case WorldType.MultiBodyWorld:
MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver();
constraintSolver = mbConstraintSolver;
World = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConf);
break;
case WorldType.SoftBodyAndRigidBody:
SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver();
constraintSolver = siConstraintSolver;
siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed;
World = new SoftRigidDynamicsWorld(dispatcher, broadphase, siConstraintSolver, collisionConf);
_world.DispatchInfo.EnableSpu = true;
SoftWorld.WorldInfo.SparseSdf.Initialize();
SoftWorld.WorldInfo.SparseSdf.Reset();
SoftWorld.WorldInfo.AirDensity = 1.2f;
SoftWorld.WorldInfo.WaterDensity = 0;
SoftWorld.WorldInfo.WaterOffset = 0;
SoftWorld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
SoftWorld.WorldInfo.Gravity = physicWorldParameters.gravity;
break;
}
if (_world is DiscreteDynamicsWorld)
{
((DiscreteDynamicsWorld)_world).Gravity = physicWorldParameters.gravity;
}
Debug.LogFormat("Physic World of type {0} Instanced", physicWorldParameters.worldType);
if (physicWorldParameters.debug)
{
Debug.Log("Physic World Debug is active");
DebugDrawUnity db = new DebugDrawUnity();
db.DebugMode = physicWorldParameters.debugDrawMode;
_world.DebugDrawer = db;
}
}
//Does not set any local variables. Is safe to use to create duplicate physics worlds for independant simulation.
public void CreatePhysicsWorld
(
out CollisionWorld world,
out CollisionConfiguration collisionConfig,
out CollisionDispatcher dispatcher,
out BroadphaseInterface broadphase,
out ConstraintSolver solver,
out SoftBodyWorldInfo softBodyWorldInfo
)
{
collisionConfig = null;
switch (physicWorldParameters.collisionType)
{
default:
case CollisionConfType.DefaultDynamicsWorldCollisionConf:
collisionConfig = new DefaultCollisionConfiguration();
break;
case CollisionConfType.SoftBodyRigidBodyCollisionConf:
collisionConfig = new SoftBodyRigidBodyCollisionConfiguration();
break;
}
dispatcher = new CollisionDispatcher(collisionConfig);
switch (physicWorldParameters.broadphaseType)
{
default:
case BroadphaseType.DynamicAABBBroadphase:
broadphase = new DbvtBroadphase();
break;
case BroadphaseType.Axis3SweepBroadphase:
broadphase = new AxisSweep3(
physicWorldParameters.axis3SweepBroadphaseMin,
physicWorldParameters.axis3SweepBroadphaseMax,
physicWorldParameters.axis3SweepMaxProxies
);
break;
case BroadphaseType.Axis3SweepBroadphase_32bit:
broadphase = new AxisSweep3_32Bit(
physicWorldParameters.axis3SweepBroadphaseMin,
physicWorldParameters.axis3SweepBroadphaseMax,
physicWorldParameters.axis3SweepMaxProxies
);
break;
}
world = null;
softBodyWorldInfo = null;
solver = null;
switch (physicWorldParameters.worldType)
{
case WorldType.CollisionOnly:
world = new CollisionWorld(dispatcher, broadphase, collisionConfig);
break;
default:
case WorldType.RigidBodyDynamics:
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
break;
case WorldType.MultiBodyWorld:
MultiBodyConstraintSolver mbConstraintSolver = new MultiBodyConstraintSolver();
constraintSolver = mbConstraintSolver;
world = new MultiBodyDynamicsWorld(dispatcher, broadphase, mbConstraintSolver, collisionConfig);
break;
case WorldType.SoftBodyAndRigidBody:
SequentialImpulseConstraintSolver siConstraintSolver = new SequentialImpulseConstraintSolver();
constraintSolver = siConstraintSolver;
siConstraintSolver.RandSeed = physicWorldParameters.sequentialImpulseConstraintSolverRandomSeed;
world = new SoftRigidDynamicsWorld(this.dispatcher, this.broadphase, siConstraintSolver, collisionConf);
world.DispatchInfo.EnableSpu = true;
SoftRigidDynamicsWorld _sworld = (SoftRigidDynamicsWorld)world;
_sworld.WorldInfo.SparseSdf.Initialize();
_sworld.WorldInfo.SparseSdf.Reset();
_sworld.WorldInfo.AirDensity = 1.2f;
_sworld.WorldInfo.WaterDensity = 0;
_sworld.WorldInfo.WaterOffset = 0;
_sworld.WorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
_sworld.WorldInfo.Gravity = Gravity.ToBullet();
break;
}
if (world is DiscreteDynamicsWorld)
{
((DiscreteDynamicsWorld)world).Gravity = Gravity.ToBullet();
}
}
protected void Dispose(bool disposing)
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.Log("BDynamicsWorld Disposing physics.");
}
if (PhysicsWorldHelper != null)
{
PhysicsWorldHelper.m_ddWorld = null;
PhysicsWorldHelper.m_world = null;
}
if (m_world != null)
{
//remove/dispose constraints
int i;
if (_ddWorld != null)
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing Constraints {0}", _ddWorld.NumConstraints);
}
for (i = _ddWorld.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = _ddWorld.GetConstraint(i);
_ddWorld.RemoveConstraint(constraint);
if (constraint.Userobject is BTypedConstraint)
{
((BTypedConstraint)constraint.Userobject).m_isInWorld = false;
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);
}
constraint.Dispose();
}
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing Collision Objects {0}", m_world.NumCollisionObjects);
}
//remove the rigidbodies from the dynamics world and delete them
for (i = m_world.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = m_world.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
Debug.Assert(body.NumConstraintRefs == 0, "Rigid body still had constraints");
body.MotionState.Dispose();
}
m_world.RemoveCollisionObject(obj);
if (obj.UserObject is BCollisionObject)
{
((BCollisionObject)obj.UserObject).isInWorld = false;
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removed CollisionObject {0}", obj.UserObject);
}
obj.Dispose();
}
MultiBodyDynamicsWorld _mbdWorld = m_world as MultiBodyDynamicsWorld;
if (_mbdWorld != null)
{
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing MultiBody Constraints {0}", _mbdWorld.NumMultiBodyConstraints);
}
for (i = _mbdWorld.NumMultiBodyConstraints - 1; i >= 0; i--)
{
MultiBodyConstraint constraint = _mbdWorld.GetMultiBodyConstraint(i);
_mbdWorld.RemoveMultiBodyConstraint(constraint);
/* if (constraint.Userobject is BTypedConstraint) ((BTypedConstraint)constraint.Userobject).m_isInWorld = false;
* if (debugType >= BDebug.DebugType.Debug) Debug.LogFormat("Removed Constaint {0}", constraint.Userobject);*/
constraint.Dispose();
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removing Multibodies {0}", _mbdWorld.NumMultibodies);
}
//remove the rigidbodies from the dynamics world and delete them
for (i = _mbdWorld.NumMultibodies - 1; i >= 0; i--)
{
MultiBody mb = _mbdWorld.GetMultiBody(i);
_mbdWorld.RemoveMultiBody(mb);
if (mb.UserObject is BMultiBody bmb)
{
bmb.isInWorld = false;
}
if (debugType >= BDebug.DebugType.Debug)
{
Debug.LogFormat("Removed CollisionObject {0}", mb.UserObject);
}
mb.Dispose();
}
}
if (m_world.DebugDrawer != null)
{
if (m_world.DebugDrawer is IDisposable)
{
IDisposable dis = (IDisposable)m_world.DebugDrawer;
dis.Dispose();
}
}
m_world.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
_ddWorld = null;
m_world = null;
}
if (Broadphase != null)
{
Broadphase.Dispose();
Broadphase = null;
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
Dispatcher = null;
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
CollisionConf = null;
}
if (constraintSolver != null)
{
constraintSolver.Dispose();
constraintSolver = null;
}
if (softBodyWorldInfo != null)
{
softBodyWorldInfo.Dispose();
softBodyWorldInfo = null;
}
_isDisposed = true;
singleton = null;
}
public PendulumDemoSimulation()
{
CollisionConfiguration = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConfiguration);
Broadphase = new DbvtBroadphase();
_solver = new MultiBodyConstraintSolver();
MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration);
World.SetInternalTickCallback(TickCallback, null, true);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
var linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
var baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
var 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
var 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();
MultiBodyWorld.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);
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;
}
}