public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache,
MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration)
{
IntPtr native = btMultiBodyDynamicsWorld_new(dispatcher.Native, pairCache.Native,
constraintSolver.Native, collisionConfiguration.Native);
InitializeUserOwned(native);
InitializeMembers(dispatcher, pairCache, constraintSolver);
}
public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache,
MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration)
: base(btMultiBodyDynamicsWorld_new(dispatcher._native, pairCache._native,
constraintSolver._native, collisionConfiguration._native), dispatcher, pairCache)
{
_constraintSolver = constraintSolver;
_bodies = new List <MultiBody>();
_constraints = new List <MultiBodyConstraint>();
}
public MultiBodyDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, MultiBodyConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration)
: base(btMultiBodyDynamicsWorld_new(dispatcher._native, pairCache._native, constraintSolver._native, collisionConfiguration._native))
{
_constraintSolver = constraintSolver;
_dispatcher = dispatcher;
_broadphase = pairCache;
_bodies = new List<MultiBody>();
_constraints = new List<MultiBodyConstraint>();
}
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);
using (var rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia))
{
var body = new RigidBody(rbInfo);
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);
}
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 = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
