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));
}
/// <summary>
/// Default Constructor
/// </summary>
public Physics()
{
DantzigSolver mlcp = new DantzigSolver();
collisionConf = new SoftBodyRigidBodyCollisionConfiguration();
dispatcher = new CollisionDispatcher(new DefaultCollisionConfiguration());
solver = new DefaultSoftBodySolver();
ConstraintSolver cSolver = new MultiBodyConstraintSolver();
broadphase = new DbvtBroadphase();
World = new SoftRigidDynamicsWorld(dispatcher, broadphase, cSolver, collisionConf, solver);
//Actual scaling is unknown, this gravity is probably not right
World.Gravity = new Vector3(0, -98.1f, 0);
World.SetInternalTickCallback(new DynamicsWorld.InternalTickCallback((w, f) => DriveJoints.UpdateAllMotors(Skeleton, cachedArgs)));
//Roobit
RigidNode_Base.NODE_FACTORY = (Guid guid) => new BulletRigidNode(guid);
string RobotPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Robots\";
string dir = RobotPath;
GetFromDirectory(RobotPath, s => { Skeleton = (BulletRigidNode)BXDJSkeleton.ReadSkeleton(s + "skeleton.bxdj"); dir = s; });
List <RigidNode_Base> nodes = Skeleton.ListAllNodes();
for (int i = 0; i < nodes.Count; i++)
{
BulletRigidNode bNode = (BulletRigidNode)nodes[i];
bNode.CreateRigidBody(dir + bNode.ModelFileName);
bNode.CreateJoint();
if (bNode.joint != null)
{
World.AddConstraint(bNode.joint, true);
}
World.AddCollisionObject(bNode.BulletObject);
collisionShapes.Add(bNode.BulletObject.CollisionShape);
}
//Field
string fieldPath = @"C:\Program Files (x86)\Autodesk\Synthesis\Synthesis\Fields\";
GetFromDirectory(fieldPath, s => f = BulletFieldDefinition.FromFile(s));
foreach (RigidBody b in f.Bodies)
{
World.AddRigidBody(b);
collisionShapes.Add(b.CollisionShape);
}
}
//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;
}
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));
}
/*
* 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);
}
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);
}
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 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");
}
//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();
}
}
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;
}
}
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;
}
}
