public Physics()
{
// collision configuration contains default setup for memory, collision setup
collisionConf = new DefaultCollisionConfiguration();
dispatcher = new CollisionDispatcher(collisionConf);
broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(dispatcher, broadphase, null, collisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(50, 50, 50);
collisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix4.CreateTranslation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
const float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
collisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
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++)
{
Matrix4 startTransform = Matrix4.CreateTranslation(
new Vector3(
2*i + start_x,
2*k + start_y,
2*j + start_z
)
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
RigidBody body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
public void AlignmentTest()
{
const float mass = 1.0f;
for (int i = 0; i < 100; i++)
{
// RigidBodyConstructionInfo without the optional localInertia parameter will
// cause the default value to be passed, which is not aligned to 16 bytes in C++/CLI.
// If BulletSharp doesn't explicitly pass an aligned value and SSE is used,
// an AccessViolationException occurs.
var info = new RigidBodyConstructionInfo(mass, new DefaultMotionState(), boxShape); // , Vector3.Zero
info.Dispose();
}
}
BulletSharp.RigidBody createRigidBody (BulletSharp.CollisionShape shape, UnityEngine.Transform transform, float mass)
{
Matrix4x4 unityMatrix = Matrix4x4.TRS (transform.position, transform.rotation, UnityEngine.Vector3.one);
BulletSharp.Matrix bulletMatrix = new BulletSharp.Matrix (
unityMatrix.m00, unityMatrix.m10, unityMatrix.m20, unityMatrix.m30,
unityMatrix.m01, unityMatrix.m11, unityMatrix.m21, unityMatrix.m31,
unityMatrix.m02, unityMatrix.m12, unityMatrix.m22, unityMatrix.m32,
unityMatrix.m03, unityMatrix.m13, unityMatrix.m23, unityMatrix.m33);
BulletSharp.MotionState motionState = new BulletSharp.DefaultMotionState (bulletMatrix);
BulletSharp.Vector3 inertia = new BulletSharp.Vector3 (0f, 0f, 0f);
if (!Mathf.Approximately (mass, 0f)) {
shape.CalculateLocalInertia (mass, out inertia);
}
BulletSharp.RigidBodyConstructionInfo myRigidBodyCI = new BulletSharp.RigidBodyConstructionInfo (mass, motionState, shape, inertia);
BulletSharp.RigidBody myRigidBody = new BulletSharp.RigidBody (myRigidBodyCI);
myRigidBodyCI.Dispose ();
return myRigidBody;
}
public override RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.0f);
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
shape.CalculateLocalInertia(mass, out localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, null, shape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
body.ContactProcessingThreshold = defaultContactProcessingThreshold;
body.WorldTransform = startTransform;
World.AddRigidBody(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();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
GImpactCollisionAlgorithm.RegisterAlgorithm(Dispatcher);
string bulletFile;
string[] args = Environment.GetCommandLineArgs();
if (args.Length == 1)
{
bulletFile = "testFile.bullet";
}
else
{
bulletFile = args[1];
}
fileLoader = new CustomBulletWorldImporter(World);
if (!fileLoader.LoadFile(bulletFile))
{
CollisionShape groundShape = new BoxShape(50);
CollisionShapes.Add(groundShape);
RigidBody ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
Vector3[] positions = new Vector3[2] { new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.4f, 0.5f, 0.6f) };
float[] radi = new float[2] { 0.3f, 0.4f };
CollisionShape colShape = new MultiSphereShape(positions, radi);
//CollisionShape colShape = new CapsuleShapeZ(1, 1);
//CollisionShape colShape = new CylinderShapeZ(1, 1, 1);
//CollisionShape colShape = new BoxShape(1);
//CollisionShape colShape = new SphereShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
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(
2 * i + start_x,
2 * k + start_y,
2 * 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();
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
DefaultSerializer serializer = new DefaultSerializer();
serializer.RegisterNameForObject(ground, "GroundName");
for (i = 0; i < CollisionShapes.Count; i++)
serializer.RegisterNameForObject(CollisionShapes[i], "name" + i.ToString());
Point2PointConstraint p2p = new Point2PointConstraint((RigidBody)World.CollisionObjectArray[2], new Vector3(0, 1, 0));
World.AddConstraint(p2p);
serializer.RegisterNameForObject(p2p, "constraintje");
World.Serialize(serializer);
BulletSharp.DataStream data = serializer.LockBuffer();
byte[] dataBytes = new byte[data.Length];
data.Read(dataBytes, 0, dataBytes.Length);
FileStream file = new FileStream("testFile.bullet", FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Close();
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(20, 50, 10);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0,
Matrix.RotationAxis(new Vector3(0, 0, 1), (float)Math.PI * 0.03f) * Matrix.Translation(0, -50, 0),
groundShape);
ground.Friction = 1;
ground.RollingFriction = 1;
ground.UserObject = "Ground";
groundShape = new BoxShape(100, 50, 100);
CollisionShapes.Add(groundShape);
ground = LocalCreateRigidBody(0, Matrix.Translation(0, -54, 0), groundShape);
ground.Friction = 1;
ground.RollingFriction = 1;
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
CollisionShape[] colShapes = {
new SphereShape(1),
new CapsuleShape(0.5f,1),
new CapsuleShapeX(0.5f,1),
new CapsuleShapeZ(0.5f,1),
new ConeShape(0.5f,1),
new ConeShapeX(0.5f,1),
new ConeShapeZ(0.5f,1),
new CylinderShape(new Vector3(0.5f,1,0.5f)),
new CylinderShapeX(new Vector3(1,0.5f,0.5f)),
new CylinderShapeZ(new Vector3(0.5f,0.5f,1)),
};
foreach (var collisionShape in colShapes)
{
CollisionShapes.Add(collisionShape);
}
const float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, null, localInertia);
const float startX = StartPosX - ArraySizeX / 2;
const float startY = StartPosY;
const float startZ = StartPosZ - ArraySizeZ / 2;
int shapeIndex = 0;
for (int k = 0; k < ArraySizeY; k++)
{
for (int i = 0; i < ArraySizeX; i++)
{
for (int j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
2 * i + startX,
2 * k + startY + 20,
2 * j + startZ
);
shapeIndex++;
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
rbInfo.CollisionShape = colShapes[shapeIndex % colShapes.Length];
RigidBody body = new RigidBody(rbInfo);
body.Friction = 1;
body.RollingFriction = 0.3f;
body.SetAnisotropicFriction(colShape.AnisotropicRollingFrictionDirection, AnisotropicFrictionFlags.RollingFriction);
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
{
bool isDynamic = (mass != 0.0f);
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
shape.CalculateLocalInertia(mass, out localInertia);
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
ownerWorld.AddRigidBody(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();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
BoxShape groundShape = new BoxShape(50, 1, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, 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 startX = StartPosX - ArraySizeX / 2;
const float startY = StartPosY;
const float startZ = StartPosZ - ArraySizeZ / 2;
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
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(
2 * i + startX,
2 * k + startY,
2 * j + startZ
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
rbInfo.MotionState = new DefaultMotionState(startTransform);
RigidBody body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
public bool Build(
FeatureUpdateContext updateContext,
[ParentModel] Point[] Points,
double Size)
{
//-- Upkeep
//--
if( physics == null )
{
physics = new Sutd.Physics( );
}
else
{
Regenerate( );
physics.Reset( );
}
//-- Set Gravity
//--
physics.world.Gravity = new Vec3D( 0, 0, -10 );
//-- Create Ground
//--
var body = new Sutd.Physics.Body( );
{
//-- Define Infinite Plane
//--
var shape = new StaticPlaneShape( new Vec3D( 0, 0, 1 ), 0 );
physics.shapes.Add( shape );
//-- Set Physics State / Bullet
//-- Fixed bodies have zero mass and inertia
//--
var param = new RigidBodyConstructionInfo(
mass: 0.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
collisionShape: shape, localInertia: Vec3D.Zero );
body.rigid = new RigidBody( param );
param.Dispose( );
physics.world.AddRigidBody( body.rigid );
body.matrix = body.rigid.WorldTransform;
//-- Set Visual State / Rhino
//-- Create a very thin but wide finite box
//--
var transform = DTransform3d.Identity;
AddBox( transform, new DVector3d( 50, 50, 0.01 ) );
body.solid = geometry[geometry.Count - 1];
}
physics.bodies.Add( body );
//-- Create 3D Grid of Boxes
//--
float half = (float)( Size * 0.5 );
foreach( var point in Points )
{
body = new Sutd.Physics.Body( );
{
//-- Collision Shape
//--
var shape = new BoxShape( half, half, half );
physics.shapes.Add( shape );
//-- Mass Properties
//--
var inertia = Vec3D.Zero;
shape.CalculateLocalInertia( mass: 1.0f, inertia: out inertia );
//-- Physics State
//--
var param = new RigidBodyConstructionInfo(
mass: 1.0f, motionState: new DefaultMotionState( Mat4D.Identity ),
collisionShape: shape, localInertia: inertia );
body.rigid = new RigidBody( param );
param.Dispose( );
physics.world.AddRigidBody( body.rigid );
body.rigid.Translate( new Vec3D( (float)point.X, (float)point.Y, (float)point.Z ) );
body.matrix = body.rigid.WorldTransform;
//-- Visual State
//--
var transform = DTransform3d.Identity;
transform.Translation = point.DPoint3d;
AddBox( transform, new DVector3d( Size, Size, Size ) );
body.solid = geometry[geometry.Count - 1];
}
physics.bodies.Add( body );
}
return true;
}
public virtual RigidBody CreateRigidBody(bool isDynamic, float mass, ref Matrix startTransform, CollisionShape shape, string bodyName)
{
Vector3 localInertia;
if (mass != 0.0f)
{
shape.CalculateLocalInertia(mass, out localInertia);
}
else
{
localInertia = Vector3.Zero;
}
RigidBodyConstructionInfo info = new RigidBodyConstructionInfo(mass, null, shape, localInertia);
RigidBody body = new RigidBody(info);
info.Dispose();
body.WorldTransform = startTransform;
if (_dynamicsWorld != null)
{
_dynamicsWorld.AddRigidBody(body);
}
if (bodyName != null)
{
_objectNameMap.Add(body, bodyName);
_nameBodyMap.Add(bodyName, body);
}
_allocatedRigidBodies.Add(body);
return body;
}
void Start()
{
//Create a World
Debug.Log("Initialize physics");
List<CollisionShape> CollisionShapes = new List<CollisionShape>();
DefaultCollisionConfiguration CollisionConf = new DefaultCollisionConfiguration();
CollisionDispatcher Dispatcher = new CollisionDispatcher(CollisionConf);
DbvtBroadphase Broadphase = new DbvtBroadphase();
DiscreteDynamicsWorld World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
World.Gravity = new BulletSharp.Math.Vector3(0, -10, 0);
// create a few dynamic rigidbodies
const float mass = 1.0f;
//Add a single cube
RigidBody fallRigidBody;
BoxShape shape = new BoxShape(1f, 1f, 1f);
BulletSharp.Math.Vector3 localInertia = BulletSharp.Math.Vector3.Zero;
shape.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, null, shape, localInertia);
fallRigidBody = new RigidBody(rbInfo);
rbInfo.Dispose();
Matrix st = Matrix.Translation(new BulletSharp.Math.Vector3(0f, 10f, 0f));
fallRigidBody.WorldTransform = st;
World.AddRigidBody(fallRigidBody);
//Step the simulation 300 steps
for (int i = 0; i < 300; i++)
{
World.StepSimulation(1f / 60f, 10);
Matrix trans;
fallRigidBody.GetWorldTransform(out trans);
Debug.Log("box height: " + trans.Origin);
}
//Clean up.
World.RemoveRigidBody(fallRigidBody);
fallRigidBody.Dispose();
UnityEngine.Debug.Log("ExitPhysics");
if (World != null)
{
//remove/dispose constraints
int i;
for (i = World.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = World.GetConstraint(i);
World.RemoveConstraint(constraint);
constraint.Dispose();
}
//remove the rigidbodies from the dynamics world and delete them
for (i = World.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = World.CollisionObjectArray[i];
RigidBody body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
World.RemoveCollisionObject(obj);
obj.Dispose();
}
//delete collision shapes
foreach (CollisionShape ss in CollisionShapes)
ss.Dispose();
CollisionShapes.Clear();
World.Dispose();
Broadphase.Dispose();
Dispatcher.Dispose();
CollisionConf.Dispose();
}
if (Broadphase != null)
{
Broadphase.Dispose();
}
if (Dispatcher != null)
{
Dispatcher.Dispose();
}
if (CollisionConf != null)
{
CollisionConf.Dispose();
}
}
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);
}
/**
Creates or configures a RigidBody based on the current settings. Does not alter the internal state of this component in any way.
Can be used to create copies of this BRigidBody for use in other physics simulations.
*/
public bool CreateOrConfigureRigidBody(ref RigidBody rb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs, MotionState motionState)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
localInertia = BulletSharp.Math.Vector3.Zero;
if (isDynamic())
{
cs.CalculateLocalInertia(_mass, out localInertia);
}
if (rb == null)
{
float bulletMass = _mass;
if (!isDynamic())
{
bulletMass = 0f;
}
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(bulletMass, motionState, cs, localInertia);
rbInfo.Friction = _friction;
rbInfo.RollingFriction = _rollingFriction;
rbInfo.LinearDamping = _linearDamping;
rbInfo.AngularDamping = _angularDamping;
rbInfo.Restitution = _restitution;
rbInfo.LinearSleepingThreshold = _linearSleepingThreshold;
rbInfo.AngularSleepingThreshold = _angularSleepingThreshold;
rbInfo.AdditionalDamping = _additionalDamping;
rbInfo.AdditionalAngularDampingFactor = _additionalAngularDampingFactor;
rbInfo.AdditionalAngularDampingThresholdSqr = _additionalAngularDampingThresholdSqr;
rbInfo.AdditionalDampingFactor = _additionalDampingFactor;
rbInfo.AdditionalLinearDampingThresholdSqr = _additionalLinearDampingThresholdSqr;
rb = new RigidBody(rbInfo);
rbInfo.Dispose();
}
else {
float usedMass = 0f;
if (isDynamic())
{
usedMass = _mass;
}
rb.SetMassProps(usedMass, localInertia);
rb.Friction = _friction;
rb.RollingFriction = _rollingFriction;
rb.SetDamping(_linearDamping, _angularDamping);
rb.Restitution = _restitution;
rb.SetSleepingThresholds(_linearSleepingThreshold, _angularSleepingThreshold);
rb.CollisionShape = cs;
}
rb.AngularVelocity = angularVelocity.ToBullet();
rb.LinearVelocity = velocity.ToBullet();
rb.CollisionFlags = m_collisionFlags;
rb.LinearFactor = _linearFactor.ToBullet();
rb.AngularFactor = _angularFactor.ToBullet();
if (m_rigidBody != null)
{
rb.DeactivationTime = m_rigidBody.DeactivationTime;
rb.InterpolationLinearVelocity = m_rigidBody.InterpolationLinearVelocity;
rb.InterpolationAngularVelocity = m_rigidBody.InterpolationAngularVelocity;
rb.InterpolationWorldTransform = m_rigidBody.InterpolationWorldTransform;
}
//if kinematic then disable deactivation
if ((m_collisionFlags & BulletSharp.CollisionFlags.KinematicObject) != 0)
{
rb.ActivationState = ActivationState.DisableDeactivation;
}
return true;
}
protected override void OnInitializePhysics()
{
// 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.
Dispatcher = new CollisionDispatcher(CollisionConf);
VoronoiSimplexSolver simplex = new VoronoiSimplexSolver();
MinkowskiPenetrationDepthSolver pdSolver = new MinkowskiPenetrationDepthSolver();
Convex2DConvex2DAlgorithm.CreateFunc convexAlgo2d = new Convex2DConvex2DAlgorithm.CreateFunc(simplex, pdSolver);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2d);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Convex2DShape, convexAlgo2d);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Convex2DShape, BroadphaseNativeType.Box2DShape, convexAlgo2d);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.Box2DShape, BroadphaseNativeType.Box2DShape, new Box2DBox2DCollisionAlgorithm.CreateFunc());
Broadphase = new DbvtBroadphase();
// the default constraint solver.
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
CollisionShape groundShape = new BoxShape(150, 7, 150);
CollisionShapes.Add(groundShape);
RigidBody ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
float u = 0.96f;
Vector3[] points = { new Vector3(0, u, 0), new Vector3(-u, -u, 0), new Vector3(u, -u, 0) };
ConvexShape childShape0 = new BoxShape(1, 1, Depth);
ConvexShape colShape = new Convex2DShape(childShape0);
ConvexShape childShape1 = new ConvexHullShape(points);
ConvexShape colShape2 = new Convex2DShape(childShape1);
ConvexShape childShape2 = new CylinderShapeZ(1, 1, Depth);
ConvexShape colShape3 = new Convex2DShape(childShape2);
CollisionShapes.Add(colShape);
CollisionShapes.Add(colShape2);
CollisionShapes.Add(colShape3);
CollisionShapes.Add(childShape0);
CollisionShapes.Add(childShape1);
CollisionShapes.Add(childShape2);
colShape.Margin = 0.03f;
float mass = 1.0f;
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
Matrix startTransform;
Vector3 x = new Vector3(-ArraySizeX, 8, -20);
Vector3 y = Vector3.Zero;
Vector3 deltaX = new Vector3(1, 2, 0);
Vector3 deltaY = new Vector3(2, 0, 0);
int i, j;
for (i = 0; i < ArraySizeY; i++)
{
y = x;
for (j = 0; j < ArraySizeX; j++)
{
startTransform = Matrix.Translation(y - new Vector3(-10, 0, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo;
switch (j % 3)
{
case 0:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
break;
case 1:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape3, localInertia);
break;
default:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape2, localInertia);
break;
}
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
//body.ActivationState = ActivationState.IslandSleeping;
body.LinearFactor = new Vector3(1, 1, 0);
body.AngularFactor = new Vector3(0, 0, 1);
World.AddRigidBody(body);
y += deltaY;
}
x += deltaX;
}
}
public Physics(SceneManager sceneMgr)
{
// collision configuration contains default setup for memory, collision setup
collisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(collisionConf);
Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, collisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create the ground
CollisionShape groundShape = new BoxShape(50, 1, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix4.IDENTITY, groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
float mass = 1.0f;
CollisionShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
var rbInfo = new RigidBodyConstructionInfo(mass, null, colShape, localInertia);
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
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++)
{
Matrix4 startTransform = new Matrix4();
startTransform.MakeTrans(
new Vector3(
2*i + start_x,
2*k + start_y,
2*j + start_z
)
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
int index = (k * ArraySizeX + i) * ArraySizeZ + j;
Entity box = sceneMgr.CreateEntity("Box" + index.ToString(), "box.mesh");
box.SetMaterialName("BoxMaterial/Active");
SceneNode boxNode = sceneMgr.RootSceneNode.CreateChildSceneNode("BoxNode" + index.ToString());
boxNode.AttachObject(box);
boxNode.Scale(new Vector3(2, 2, 2));
var mogreMotionState = new MogreMotionState(box, boxNode, startTransform);
rbInfo.MotionState = mogreMotionState;
RigidBody body = new RigidBody(rbInfo);
mogreMotionState.Body = body;
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
rbInfo.Dispose();
}
public virtual RigidBody LocalCreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.0f);
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
shape.CalculateLocalInertia(mass, out localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
_world.AddRigidBody(body);
return body;
}
/*private void MyTickCallBack(ManifoldPoint cp, CollisionObjectWrapper colobj0wrap, int partid0, int index0, CollisionObjectWrapper colobj1wrap, int partid1, int index1)
{
Debug.WriteLine("MyTickCallBack");
int numManifolds = BtWorld.Dispatcher.NumManifolds;
RigidBodyImp myRb;
//Debug.WriteLine("numManifolds: " + numManifolds);
for (int i = 0; i < numManifolds; i++)
{
PersistentManifold contactManifold = BtWorld.Dispatcher.GetManifoldByIndexInternal(i);
int numContacts = contactManifold.NumContacts;
if (numContacts > 0)
{
CollisionObject obA = (CollisionObject) contactManifold.Body0;
CollisionObject obB = (CollisionObject) contactManifold.Body1;
// Debug.WriteLine(numContacts);
var pnA = obA.UserObject;
for (int j = 0; j < numContacts; j++)
{
ManifoldPoint pt = contactManifold.GetContactPoint(j);
}
}
}
}*/
public IRigidBodyImp AddRigidBody(float mass, float3 worldTransform, float3 orientation, ICollisionShapeImp colShape/*, float3 intertia*/)
{
// Use bullet to do what needs to be done:
var btMatrix = Matrix.RotationX(orientation.x)
* Matrix.RotationY(orientation.y)
* Matrix.RotationZ(orientation.z)
* Matrix.Translation(worldTransform.x, worldTransform.y, worldTransform.z);
var btMotionState = new DefaultMotionState(btMatrix);
var shapeType = colShape.GetType().ToString();
CollisionShape btColShape;
var isStatic = false;
switch (shapeType)
{
//Primitives
case "Fusee.Engine.BoxShapeImp":
var box = (BoxShapeImp) colShape;
var btBoxHalfExtents = Translater.Float3ToBtVector3(box.HalfExtents);
btColShape = new BoxShape(btBoxHalfExtents);
break;
case "Fusee.Engine.CapsuleShapeImp":
var capsule = (CapsuleShapeImp) colShape;
btColShape = new CapsuleShape(capsule.Radius, capsule.HalfHeight);
break;
case "Fusee.Engine.ConeShapeImp":
var cone = (ConeShapeImp) colShape;
btColShape = new ConeShape(cone.Radius, cone.Height);
break;
case "Fusee.Engine.CylinderShapeImp":
var cylinider = (CylinderShapeImp) colShape;
var btCylinderHalfExtents = Translater.Float3ToBtVector3(cylinider.HalfExtents);
btColShape = new CylinderShape(btCylinderHalfExtents);
break;
case "Fusee.Engine.MultiSphereShapeImp":
var multiSphere = (MultiSphereShapeImp) colShape;
var btPositions = new Vector3[multiSphere.SphereCount];
var btRadi = new float[multiSphere.SphereCount];
for (int i = 0; i < multiSphere.SphereCount; i++)
{
var pos = Translater.Float3ToBtVector3(multiSphere.GetSpherePosition(i));
btPositions[i] = pos;
btRadi[i] = multiSphere.GetSphereRadius(i);
}
btColShape = new MultiSphereShape(btPositions, btRadi);
break;
case "Fusee.Engine.SphereShapeImp":
var sphere = (SphereShapeImp) colShape;
var btRadius = sphere.Radius;
btColShape = new SphereShape(btRadius);
break;
//Misc
case "Fusee.Engine.CompoundShapeImp":
var compShape = (CompoundShapeImp) colShape;
btColShape = new CompoundShape(true);
btColShape = compShape.BtCompoundShape;
break;
case "Fusee.Engine.EmptyShapeImp":
btColShape = new EmptyShape();
break;
//Meshes
case "Fusee.Engine.ConvexHullShapeImp":
var convHull = (ConvexHullShapeImp) colShape;
var btPoints= new Vector3[convHull.GetNumPoints()];
for (int i = 0; i < convHull.GetNumPoints(); i++)
{
var point = convHull.GetScaledPoint(i);
btPoints[i] = Translater.Float3ToBtVector3(point);
}
btColShape = new ConvexHullShape(btPoints);
//btColShape.LocalScaling = new Vector3(3,3,3);
break;
case "Fusee.Engine.StaticPlaneShapeImp":
var staticPlane = (StaticPlaneShapeImp) colShape;
Debug.WriteLine("staticplane: " + staticPlane.Margin);
var btNormal = Translater.Float3ToBtVector3(staticPlane.PlaneNormal);
btColShape = new StaticPlaneShape(btNormal, staticPlane.PlaneConstant);
isStatic = true;
//btColShape.Margin = 0.04f;
//Debug.WriteLine("btColshape" + btColShape.Margin);
break;
case "Fusee.Engine.GImpactMeshShapeImp":
var gImpMesh = (GImpactMeshShapeImp)colShape;
gImpMesh.BtGImpactMeshShape.UpdateBound();
var btGimp = new GImpactMeshShape(gImpMesh.BtGImpactMeshShape.MeshInterface);
btGimp.UpdateBound();
btColShape = btGimp;
break;
//Default
default:
Debug.WriteLine("defaultImp");
btColShape = new EmptyShape();
break;
}
var btLocalInertia = btColShape.CalculateLocalInertia(mass);
// btLocalInertia *= (10.0f*10);
RigidBodyConstructionInfo btRbcInfo = new RigidBodyConstructionInfo(mass, btMotionState, btColShape,
btLocalInertia);
var btRigidBody = new RigidBody(btRbcInfo);
btRigidBody.Restitution = 0.2f;
btRigidBody.Friction = 0.2f;
btRigidBody.CollisionFlags = CollisionFlags.CustomMaterialCallback;
BtWorld.AddRigidBody(btRigidBody);
btRbcInfo.Dispose();
var retval = new RigidBodyImp();
retval._rbi = btRigidBody;
btRigidBody.UserObject = retval;
return retval;
}
static CustomVehicle()
{
var ci = new RigidBodyConstructionInfo(0, null, null);
fixedBody = new RigidBody(ci);
fixedBody.SetMassProps(0, Vector3.Zero);
ci.Dispose();
}
//called by Physics World just before rigid body is added to world.
//the current rigid body properties are used to rebuild the rigid body.
internal override bool _BuildCollisionObject()
{
BPhysicsWorld world = BPhysicsWorld.Get();
if (m_rigidBody != null) {
if (isInWorld && world != null) {
isInWorld = false;
world.RemoveRigidBody(m_rigidBody);
}
}
if (transform.localScale != UnityEngine.Vector3.one) {
BDebug.LogError(debugType, "The local scale on this rigid body is not one. Bullet physics does not support scaling on a rigid body world transform. Instead alter the dimensions of the CollisionShape.");
}
m_collisionShape = GetComponent<BCollisionShape>();
if (m_collisionShape == null) {
BDebug.LogError(debugType, "There was no collision shape component attached to this BRigidBody. {0}", name);
return false;
}
CollisionShape cs = m_collisionShape.GetCollisionShape();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
_localInertia = BulletSharp.Math.Vector3.Zero;
if (isDynamic()) {
cs.CalculateLocalInertia(_mass, out _localInertia);
}
if (m_rigidBody == null) {
m_motionState = new BGameObjectMotionState(transform);
float bulletMass = _mass;
if (!isDynamic())
{
bulletMass = 0f;
}
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(bulletMass, m_motionState, cs, _localInertia);
rbInfo.Friction = _friction;
rbInfo.RollingFriction = _rollingFriction;
rbInfo.LinearDamping = _linearDamping;
rbInfo.AngularDamping = _angularDamping;
rbInfo.Restitution = _restitution;
rbInfo.LinearSleepingThreshold = _linearSleepingThreshold;
rbInfo.AngularSleepingThreshold = _angularSleepingThreshold;
rbInfo.AdditionalDamping = _additionalDamping;
rbInfo.AdditionalAngularDampingFactor = _additionalAngularDampingFactor;
rbInfo.AdditionalAngularDampingThresholdSqr = _additionalAngularDampingThresholdSqr;
rbInfo.AdditionalDampingFactor = _additionalDampingFactor;
rbInfo.AdditionalLinearDampingThresholdSqr = _additionalLinearDampingThresholdSqr;
m_rigidBody = new RigidBody(rbInfo);
m_rigidBody.UserObject = this;
m_rigidBody.AngularVelocity = _angularVelocity.ToBullet();
m_rigidBody.LinearVelocity = _linearVelocity.ToBullet();
rbInfo.Dispose();
} else {
float usedMass = 0f;
if (isDynamic())
{
usedMass = _mass;
}
m_rigidBody.SetMassProps(usedMass, _localInertia);
m_rigidBody.Friction = _friction;
m_rigidBody.RollingFriction = _rollingFriction;
m_rigidBody.SetDamping(_linearDamping,_angularDamping);
m_rigidBody.Restitution = _restitution;
m_rigidBody.SetSleepingThresholds(_linearSleepingThreshold, _angularSleepingThreshold);
m_rigidBody.AngularVelocity = _angularVelocity.ToBullet();
m_rigidBody.LinearVelocity = _linearVelocity.ToBullet();
m_rigidBody.CollisionShape = cs;
}
m_rigidBody.CollisionFlags = m_collisionFlags;
m_rigidBody.LinearFactor = _linearFactor.ToBullet();
m_rigidBody.AngularFactor = _angularFactor.ToBullet();
//if kinematic then disable deactivation
if ((m_collisionFlags & BulletSharp.CollisionFlags.KinematicObject) != 0) {
m_rigidBody.ActivationState = ActivationState.DisableDeactivation;
}
return true;
}
