public Physics()
{
ActiveBodies = 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;
}
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);
}
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;
}
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));
}
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;
}
/*
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 SequentialImpulseConstraintSolver solver,
out SoftBodyWorldInfo softBodyWorldInfo)
{
bool success = true;
if (m_worldType == WorldType.SoftBodyAndRigidBody && m_collisionType == CollisionConfType.DefaultDynamicsWorldCollisionConf)
{
BDebug.LogError(debugType, "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)
{
world = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
}
else if (m_worldType == WorldType.MultiBodyWorld)
{
world = new MultiBodyDynamicsWorld(dispatcher, broadphase, null, collisionConfig);
}
else if (m_worldType == WorldType.SoftBodyAndRigidBody)
{
solver = new SequentialImpulseConstraintSolver();
solver.RandSeed = sequentialImpulseConstraintSolverRandomSeed;
softBodyWorldInfo = new SoftBodyWorldInfo
{
AirDensity = 1.2f,
WaterDensity = 0,
WaterOffset = 0,
WaterNormal = BulletSharp.Math.Vector3.Zero,
Gravity = UnityEngine.Physics.gravity.ToBullet(),
Dispatcher = dispatcher,
Broadphase = broadphase
};
softBodyWorldInfo.SparseSdf.Initialize();
world = new SoftRigidDynamicsWorld(dispatcher, broadphase, solver, collisionConfig);
world.DispatchInfo.EnableSpu = true;
softBodyWorldInfo.SparseSdf.Reset();
softBodyWorldInfo.AirDensity = 1.2f;
softBodyWorldInfo.WaterDensity = 0;
softBodyWorldInfo.WaterOffset = 0;
softBodyWorldInfo.WaterNormal = BulletSharp.Math.Vector3.Zero;
softBodyWorldInfo.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;
}
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;
}
}
