void PickingPreTickCallback(DynamicsWorld world, float timeStep)
{
if (!drag) return;
Vector3 rayFrom = Freelook.Eye;
Vector3 rayTo = GetRayTo(lastMousePos, Freelook.Eye, Freelook.Target, FieldOfView);
Vector3 rayDir = rayTo - rayFrom;
rayDir.Normalize();
Vector3 N = Freelook.Target - rayFrom;
N.Normalize();
float O = Vector3.Dot(impact, N);
float den = Vector3.Dot(N, rayDir);
if ((den * den) > 0)
{
float num = O - Vector3.Dot(N, rayFrom);
float hit = num / den;
if (hit > 0 && hit < 1500)
{
goal = rayFrom + rayDir * hit;
}
}
Vector3 delta = goal - node.X;
float maxDrag = 10;
if (delta.LengthSquared() > (maxDrag * maxDrag))
{
delta.Normalize();
delta *= maxDrag;
}
node.Velocity += delta / timeStep;
}
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);
CollisionShapes = new List<CollisionShape>();
// create the ground
CollisionShape groundShape = new BoxShape(50, 1, 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.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);
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.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
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (lines.Count == 0)
return;
if (effect == null)
{
effect = new BasicEffect(graphics.Device);
effect.World = Microsoft.Xna.Framework.Matrix.Identity;
effect.VertexColorEnabled = true;
pass = effect.CurrentTechnique.Passes[0];
}
effect.Projection = graphics.GetEffect().Projection;
effect.View = graphics.GetEffect().View;
pass.Apply();
int pointCount = lines.Count;
int linesCount = pointCount / 2;
VertexPositionColor[] linesArray = new VertexPositionColor[pointCount];
for (int i = 0; i < pointCount; i++)
{
int color = lines[i].Color;
linesArray[i].Color = new Color(color & 0xff, (color & 0xff00) >> 8, (color & 0xff0000) >> 16, 1);
linesArray[i].Position = MathHelper.Convert(lines[i].Position);
}
graphics.Device.DrawUserPrimitives(PrimitiveType.LineList, linesArray, 0, linesCount);
lines.Clear();
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (LineIndex == 0)
return;
Vector3[] positionArray = new Vector3[LineIndex];
int[] colorArray = new int[LineIndex];
int i;
for (i = 0; i < LineIndex; i++)
{
positionArray[i] = Lines[i].Position;
colorArray[i] = Lines[i].Color;
}
LineIndex = 0;
GL.EnableClientState(ArrayCap.VertexArray);
GL.EnableClientState(ArrayCap.ColorArray);
GL.VertexPointer(3, VertexPointerType.Float, 0, positionArray);
GL.ColorPointer(3, ColorPointerType.UnsignedByte, sizeof(int), colorArray);
GL.DrawArrays(PrimitiveType.Lines, 0, positionArray.Length);
GL.DisableClientState(ArrayCap.ColorArray);
GL.DisableClientState(ArrayCap.VertexArray);
}
public BulletPhysicWorld(Vector3 gravity,Dispatcher dispatcher, BroadphaseInterface pairCache, ConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration)
{
objs = new List<IPhysicObject>();
ctns = new List<IPhysicConstraint>();
world = new DiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration);
world.Gravity = gravity;
}
public override void LoadContent()
{
base.LoadContent();
_collisionConfiguration = new DefaultCollisionConfiguration();
_collisionDispatcher = new CollisionDispatcher(_collisionConfiguration);
_broadphaseInterface = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(_collisionDispatcher, _broadphaseInterface, null, _collisionConfiguration);
World.Gravity = new Vector3(0, 9.8f, 0);
_worldSyncEvent = new AutoResetEvent(false);
_worldUpdateTask = new Task(UpdateWorld);
}
public ActionInterfaceWrapper(IAction actionInterface, DynamicsWorld world)
{
_actionInterface = actionInterface;
_world = world;
_debugDraw = new DebugDrawUnmanagedDelegate(DebugDrawUnmanaged);
_updateAction = new UpdateActionUnmanagedDelegate(UpdateActionUnmanaged);
_native = btActionInterfaceWrapper_new(
Marshal.GetFunctionPointerForDelegate(_debugDraw),
Marshal.GetFunctionPointerForDelegate(_updateAction));
}
/// <summary>
/// Attach self to a bullet3d physics world.
/// </summary>
/// <param name="world"></param>
internal override void AddSelfToBulletWorld(BulletSharp.DynamicsWorld world)
{
// update flags
UpdateCollisionFlags();
// add to world
world.AddRigidBody(BulletRigidBody, CollisionGroup, CollisionMask);
// if this rigid body has custom gravity, override it by self value after adding to world.
// this is because when you add a body to Bullet3d world its gravity gets overriden.
if (HasCustomGravity)
{
Gravity = Gravity;
}
}
public ActionInterfaceWrapper(IAction actionInterface, DynamicsWorld world)
{
_actionInterface = actionInterface;
_world = world;
_debugDraw = new DebugDrawUnmanagedDelegate(DebugDrawUnmanaged);
_updateAction = new UpdateActionUnmanagedDelegate(UpdateActionUnmanaged);
GCHandle handle = GCHandle.Alloc(this, GCHandleType.Normal);
//UnityEngine.Debug.Log("Created " + GCHandle.ToIntPtr(handle).ToInt64());
_native = btActionInterfaceWrapper_new(
Marshal.GetFunctionPointerForDelegate(_debugDraw),
Marshal.GetFunctionPointerForDelegate(_updateAction),
GCHandle.ToIntPtr(handle));
//UnityEngine.Debug.Log("Intptr" + bgActionInterface_getManagedWrapperPntr(_native));
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (lines.Count == 0)
return;
int lighting = device.GetRenderState(RenderState.Lighting);
device.SetRenderState(RenderState.Lighting, false);
device.SetTransform(TransformState.World, global::SlimDX.Matrix.Identity);
device.VertexFormat = VertexFormat.Position | VertexFormat.Diffuse;
device.DrawUserPrimitives(PrimitiveType.LineList, lines.Count / 2, lines.ToArray());
lines.Clear();
device.SetRenderState(RenderState.Lighting, lighting);
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (lines.Count == 0)
return;
inputAssembler.InputLayout = inputLayout;
if (lineArray.Length != lines.Count)
{
lineArray = new PositionColored[lines.Count];
lines.CopyTo(lineArray);
if (vertexBuffer != null)
{
vertexBuffer.Dispose();
}
vertexBufferDesc.SizeInBytes = PositionColored.Stride * lines.Count;
using (var data = new DataStream(vertexBufferDesc.SizeInBytes, false, true))
{
data.WriteRange(lineArray);
data.Position = 0;
vertexBuffer = new Buffer(device, data, vertexBufferDesc);
}
vertexBufferBinding.Buffer = vertexBuffer;
}
else
{
lines.CopyTo(lineArray);
DataStream ds;
var map = device.ImmediateContext.MapSubresource(vertexBuffer, MapMode.WriteDiscard, SharpDX.Direct3D11.MapFlags.None, out ds);
ds.WriteRange(lineArray);
device.ImmediateContext.UnmapSubresource(vertexBuffer, 0);
}
inputAssembler.SetVertexBuffers(0, vertexBufferBinding);
inputAssembler.PrimitiveTopology = global::SharpDX.Direct3D.PrimitiveTopology.LineList;
device.ImmediateContext.VertexShader.Set(vertexShader);
device.ImmediateContext.PixelShader.Set(pixelShader);
device.ImmediateContext.Draw(lines.Count, 0);
lines.Clear();
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (lines.Count == 0)
return;
inputAssembler.InputLayout = inputLayout;
if (lineArray.Length != lines.Count)
{
lineArray = new PositionColored[lines.Count];
lines.CopyTo(lineArray);
if (vertexBuffer != null)
{
vertexBuffer.Dispose();
}
vertexBufferDesc.SizeInBytes = PositionColored.Stride * lines.Count;
using (var data = new DataStream(vertexBufferDesc.SizeInBytes, false, true))
{
data.WriteRange(lineArray);
data.Position = 0;
vertexBuffer = new Buffer(device, data, vertexBufferDesc);
}
vertexBufferBinding.Buffer = vertexBuffer;
}
else
{
lines.CopyTo(lineArray);
using (var map = vertexBuffer.Map(MapMode.WriteDiscard))
{
map.WriteRange(lineArray);
}
vertexBuffer.Unmap();
}
inputAssembler.SetVertexBuffers(0, vertexBufferBinding);
inputAssembler.PrimitiveTopology = global::SharpDX.Direct3D.PrimitiveTopology.LineList;
device.Draw(lines.Count, 0);
lines.Clear();
}
/*
protected override void Dispose(bool disposing)
{
if (disposing)
{
if (vertexBuffer != null)
{
vertexBuffer.Dispose();
vertexBuffer = null;
}
}
base.Dispose(disposing);
}
*/
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
if (LineIndex == 0)
return;
_inputAssembler.InputLayout = _inputLayout;
if (_vertexCount != LineIndex)
{
if (_vertexBuffer != null)
{
_vertexBuffer.Dispose();
}
_vertexCount = LineIndex;
_vertexBufferDesc.SizeInBytes = PositionColored.Stride * _vertexCount;
using (var data = new DataStream(_vertexBufferDesc.SizeInBytes, false, true))
{
data.WriteRange(Lines, 0, _vertexCount);
data.Position = 0;
_vertexBuffer = new Buffer(_device, data, _vertexBufferDesc);
}
_vertexBufferBinding.Buffer = _vertexBuffer;
}
else
{
using (var map = _vertexBuffer.Map(MapMode.WriteDiscard))
{
map.WriteRange(Lines, 0, _vertexCount);
}
_vertexBuffer.Unmap();
}
_inputAssembler.SetVertexBuffers(0, _vertexBufferBinding);
_inputAssembler.PrimitiveTopology = global::SharpDX.Direct3D.PrimitiveTopology.LineList;
_device.Draw(_vertexCount, 0);
LineIndex = 0;
}
/// <summary>
/// Remove self from a bullet3d physics world.
/// </summary>
/// <param name="world">World to remove from.</param>
internal override void RemoveSelfFromBulletWorld(BulletSharp.DynamicsWorld world)
{
world.RemoveCollisionObject(BulletCollisionObject);
}
// since the only times this method is called is inside #if NLERPERS, we can put this entire method inside it too
#if NLERPERS
/// <summary>
/// Aligns the kart with the callback's normal
/// </summary>
/// <param name="useNlerp">Should we use a nlerper? If false, this just updates the orientation instantly.</param>
private void AlignKartWithNormal(Kart kart, DynamicsWorld.ClosestRayResultCallback callback, bool useNlerp, float duration = 1) {
// don't add a nlerper if we're drifting
if (kart.IsDriftingAtAll)
return;
// don't bother if they're already within 1 degree of each other
if (kart.ActualOrientation.YAxis.DirectionEquals(callback.HitNormalWorld, 0.01745f))
return;
// get the rotation we need to do to rotate this kart to the ground's normal
Quaternion rotTo = kart.ActualOrientation.YAxis.GetRotationTo(callback.HitNormalWorld);
// rotTo * old orientation is the same as rotate(rotTo) on SceneNodes, but since this isn't a scene node we have to do it this way
Quaternion newOrientation = rotTo * kart.ActualOrientation;
if (useNlerp) {
// if we already have a nlerper, get rid of it
helperMgr.RemoveNlerper(kart);
helperMgr.CreateNlerper(kart, duration, newOrientation);
}
else {
// update our body
kart.Body.SetOrientation(newOrientation);
}
}
void PickingPreTickCallback(DynamicsWorld world, float timeStep)
{
if (drag)
{
Vector3 rayFrom = Freelook.Eye;
Vector3 rayTo = GetRayTo(lastMousePos, Freelook.Eye, Freelook.Target, Graphics.FieldOfView);
Vector3 rayDir = (rayTo - rayFrom);
rayDir.Normalize();
Vector3 N = (Freelook.Target - Freelook.Eye);
N.Normalize();
float O = Vector3.Dot(impact, N);
float den = Vector3.Dot(N, rayDir);
if ((den * den) > 0)
{
float num = O - Vector3.Dot(N, rayFrom);
float hit = num / den;
if ((hit > 0) && (hit < 1500))
{
goal = rayFrom + rayDir * hit;
}
}
Vector3 delta = goal - node.Position;
const float maxdrag = 10;
if (delta.LengthSquared > (maxdrag * maxdrag))
{
delta.Normalize();
delta *= maxdrag;
}
node.Velocity += delta / timeStep;
}
}
void WorldPreTickCallback(DynamicsWorld world2, double timeStep)
static void WorldPreTickCallback(DynamicsWorld world2, float timeStep)
{
TestManifoldPoints();
//Console.WriteLine("WorldPreTickCallback");
if (!object.ReferenceEquals(world, world2))
{
Console.WriteLine("WorldPreTickCallback: World reference lost!");
}
}
/// <summary>
/// Remove self from a bullet3d physics world.
/// </summary>
/// <param name="world">World to remove from.</param>
internal virtual void RemoveSelfFromBulletWorld(BulletSharp.DynamicsWorld world)
{
}
public Ragdoll(DynamicsWorld ownerWorld, Vector3 positionOffset)
{
this.ownerWorld = ownerWorld;
// Setup the geometry
shapes[(int)BodyPart.Pelvis] = new CapsuleShape(0.15f, 0.20f);
shapes[(int)BodyPart.Spine] = new CapsuleShape(0.15f, 0.28f);
shapes[(int)BodyPart.Head] = new CapsuleShape(0.10f, 0.05f);
shapes[(int)BodyPart.LeftUpperLeg] = new CapsuleShape(0.05f, 0.45f);
shapes[(int)BodyPart.LeftLowerLeg] = new CapsuleShape(0.04f, 0.37f);
shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.05f, 0.45f);
shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.04f, 0.37f);
shapes[(int)BodyPart.LeftUpperArm] = new CapsuleShape(0.04f, 0.33f);
shapes[(int)BodyPart.LeftLowerArm] = new CapsuleShape(0.03f, 0.25f);
shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.04f, 0.33f);
shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.03f, 0.25f);
Matrix offset = Matrix.Translation(positionOffset);
Matrix transform;
transform = offset * Matrix.Translation(0, 1, 0);
bodies[(int)BodyPart.Pelvis] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Pelvis]);
transform = offset * Matrix.Translation(0, 1.2f, 0);
bodies[(int)BodyPart.Spine] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Spine]);
transform = offset * Matrix.Translation(0, 1.6f, 0);
bodies[(int)BodyPart.Head] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Head]);
transform = offset * Matrix.Translation(-0.18f, 0.6f, 0);
bodies[(int)BodyPart.LeftUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperLeg]);
transform = offset * Matrix.Translation(-0.18f, 0.2f, 0);
bodies[(int)BodyPart.LeftLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerLeg]);
transform = offset * Matrix.Translation(0.18f, 0.65f, 0);
bodies[(int)BodyPart.RightUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperLeg]);
transform = offset * Matrix.Translation(0.18f, 0.2f, 0);
bodies[(int)BodyPart.RightLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerLeg]);
transform = Matrix.RotationZ(PI_2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);
transform = Matrix.RotationZ(PI_2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);
transform = Matrix.RotationZ(-PI_2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);
transform = Matrix.RotationZ(-PI_2) * offset * Matrix.Translation(0.7f, 1.45f, 0);
bodies[(int)BodyPart.RightLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerArm]);
// Setup some damping on the m_bodies
foreach (RigidBody body in bodies)
{
body.SetDamping(0.05f, 0.85f);
body.DeactivationTime = 0.8f;
body.SetSleepingThresholds(1.6f, 2.5f);
}
// Now setup the constraints
HingeConstraint hingeC;
ConeTwistConstraint coneC;
Matrix localA, localB;
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.15f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
hingeC.SetLimit(-PI_4, PI_2);
joints[(int)Joint.PelvisSpine] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.PelvisSpine], true);
localA = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, 0.30f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.14f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.Head], localA, localB);
coneC.SetLimit(PI_4, PI_4, PI_2);
joints[(int)Joint.SpineHead] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.SpineHead], true);
localA = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(-0.18f, -0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
coneC.SetLimit(PI_4, PI_4, 0);
joints[(int)Joint.LeftHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftHip], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperLeg], bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.LeftKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0.18f, -0.10f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.RightUpperLeg], localA, localB);
coneC.SetLimit(PI_4, PI_4, 0);
joints[(int)Joint.RightHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightHip], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.RightKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI) * Matrix.Translation(-0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
coneC.SetLimit(PI_2, PI_2, 0);
joints[(int)Joint.LeftShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftShoulder], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.LeftElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftElbow], true);
localA = Matrix.RotationYawPitchRoll(0, 0, 0) * Matrix.Translation(0.2f, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
coneC.SetLimit(PI_2, PI_2, 0);
joints[(int)Joint.RightShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightShoulder], true);
localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperArm], bodies[(int)BodyPart.RightLowerArm], localA, localB);
//hingeC.SetLimit(-PI_2, 0);
hingeC.SetLimit(0, PI_2);
joints[(int)Joint.RightElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
}
/// <summary>
/// Run when our kart is still on the ground
/// </summary>
private void Ground(Kart kart, DynamicsWorld.ClosestRayResultCallback callback) {
// first get the object we're currently driving on
CollisionObject current;
if (CurrentlyDrivingOn.TryGetValue(kart, out current)) {
// if it's different than the old one, then we need to run our event and update the dictionary
if (current != callback.CollisionObject) {
// run the event
if (OnGroundChanged != null)
OnGroundChanged(kart, callback.CollisionObject, CurrentlyDrivingOn[kart]);
// and then update the dictionary
CurrentlyDrivingOn[kart] = callback.CollisionObject;
}
}
// run this too
if (OnGround != null)
OnGround(kart, callback);
}
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 WorldImporter(DynamicsWorld world)
{
_dynamicsWorld = world;
}
public static void SetGravity(this DynamicsWorld obj, OpenTK.Vector3 value)
{
SetGravity(obj, ref value);
}
public static OpenTK.Vector3 GetGravity(this DynamicsWorld obj)
{
OpenTK.Vector3 value;
GetGravity(obj, out value);
return(value);
}
public DrawingResult(DynamicsWorld world)
{
this.world = world;
}
void MotorPreTickCallback(DynamicsWorld world, float timeStep)
{
SetMotorTargets(timeStep);
}
/// <summary>
/// Run when our short ray penetrates the ground but we have not yet "landed"
/// </summary>
private void TouchDown(Kart kart, DynamicsWorld.ClosestRayResultCallback callback) {
// now we have actually landed
kart.IsInAir = false;
#if NLERPERS
// if we have a nlerper, get rid of it
helperMgr.RemoveNlerper(kart);
#endif
#if SKIDDERS
// add a skidder!
helperMgr.CreateSkidder(kart, skidderDuration);
#endif
#if NLERPERS
// align the kart just to make sure
AlignKartWithNormal(kart, callback, true, 0.1f);
#endif
CurrentlyDrivingOn[kart] = callback.CollisionObject;
if (OnTouchdown != null)
OnTouchdown(kart, callback);
}
public TestRig(DynamicsWorld ownerWorld, Vector3 positionOffset, bool bFixed)
{
this.ownerWorld = ownerWorld;
Vector3 vUp = new Vector3(0, 1, 0);
//
// Setup geometry
//
const float fBodySize = 0.25f;
const float fLegLength = 0.45f;
const float fForeLegLength = 0.75f;
const float PI_2 = (float)(0.5f * Math.PI);
const float PI_4 = (float)(0.25f * Math.PI);
const float PI_8 = (float)(0.125f * Math.PI);
shapes[0] = new CapsuleShape(fBodySize, 0.10f);
int i;
for (i = 0; i < NumLegs; i++)
{
shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
}
//
// Setup rigid bodies
//
const float fHeight = 0.5f;
Matrix offset = Matrix.Translation(positionOffset);
// root
Vector3 vRoot = new Vector3(0, fHeight, 0);
Matrix transform = Matrix.Translation(vRoot);
if (bFixed)
{
bodies[0] = LocalCreateRigidBody(0, transform * offset, shapes[0]);
}
else
{
bodies[0] = LocalCreateRigidBody(1, transform * offset, shapes[0]);
}
// legs
for (i = 0; i < NumLegs; i++)
{
float fAngle = (float)(2 * Math.PI * i / NumLegs);
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
Vector3 vBoneOrigin = new Vector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));
// thigh
Vector3 vToBone = (vBoneOrigin - vRoot);
vToBone.Normalize();
Vector3 vAxis = Vector3.Cross(vToBone, vUp);
transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(vBoneOrigin);
bodies[1 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[1 + 2 * i]);
// shin
transform = Matrix.Translation(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
bodies[2 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[2 + 2 * i]);
}
// Setup some damping on the bodies
for (i = 0; i < BodyPartCount; ++i)
{
bodies[i].SetDamping(0.05f, 0.85f);
bodies[i].DeactivationTime = 0.8f;
//bodies[i].SetSleepingThresholds(1.6f, 2.5f);
bodies[i].SetSleepingThresholds(0.5f, 0.5f);
}
//
// Setup the constraints
//
HingeConstraint hingeC;
//ConeTwistConstraint coneC;
Matrix localA, localB, localC;
for (i = 0; i < NumLegs; i++)
{
float fAngle = (float)(2 * Math.PI * i / NumLegs);
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
// hip joints
localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK
localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
hingeC = new HingeConstraint(bodies[0], bodies[1 + 2 * i], localA, localB);
hingeC.SetLimit(-0.75f * PI_4, PI_8);
//hingeC.SetLimit(-0.1f, 0.1f);
joints[2 * i] = hingeC;
ownerWorld.AddConstraint(joints[2 * i], true);
// knee joints
localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength));
localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
localC = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[2 + 2 * i].WorldTransform);
hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC);
//hingeC.SetLimit(-0.01f, 0.01f);
hingeC.SetLimit(-PI_8, 0.2f);
joints[1 + 2 * i] = hingeC;
ownerWorld.AddConstraint(joints[1 + 2 * i], true);
}
}
/// <summary>
/// Remove self from a bullet3d physics world.
/// </summary>
/// <param name="world">World to remove from.</param>
internal override void RemoveSelfFromBulletWorld(BulletSharp.DynamicsWorld world)
{
world.RemoveRigidBody(BulletRigidBody);
}
/// <summary>
/// Attach self to a bullet3d physics world.
/// </summary>
/// <param name="world">World to add to.</param>
internal virtual void AddSelfToBulletWorld(BulletSharp.DynamicsWorld world)
{
}
/// <summary>
/// Run when our long ray penetrates the ground
/// </summary>
private void GettingCloseToTouchingDown(Kart kart, DynamicsWorld.ClosestRayResultCallback callback, SelfRighter srh) {
#if SELFRIGHTERS
// getting rid of our SRH means that we're close to landing but we haven't landed yet
srh.Detach();
SelfRighters.TryRemove(kart, out srh);
#endif
#if NLERPERS
AlignKartWithNormal(kart, callback, true, 0.2f);
#endif
if (OnCloseToTouchdown != null)
OnCloseToTouchdown(kart, callback);
}
/// <summary>
/// Run when our short ray detects that we're not on the ground any more
/// </summary>
private void Liftoff(Kart kart, DynamicsWorld.ClosestRayResultCallback callback) {
#if SELFRIGHTERS
// make a new SRH
SelfRighters.GetOrAdd(kart, new SelfRighter(kart));
#endif
// we are in the air
kart.IsInAir = true;
CurrentlyDrivingOn[kart] = null;
if (OnLiftoff != null)
OnLiftoff(kart, callback);
}
/// <summary>
/// Run when our kart is currently in the air
/// </summary>
private void InAir(Kart kart, DynamicsWorld.ClosestRayResultCallback callback) {
if (OnInAir != null)
OnInAir(kart, callback);
}
public void DrawDebugWorld(DynamicsWorld world)
{
world.DebugDrawWorld();
}
public DefaultVehicleRaycaster(DynamicsWorld world)
{
_dynamicsWorld = world;
}
public BulletWorldImporter(DynamicsWorld world)
: base(world)
{
}
/// <summary>
/// Attach self to a bullet3d physics world.
/// </summary>
/// <param name="world"></param>
internal override void AddSelfToBulletWorld(BulletSharp.DynamicsWorld world)
{
UpdateCollisionFlags();
world.AddCollisionObject(BulletCollisionObject, CollisionGroup, CollisionMask);
}
public BulletXmlWorldImporter(DynamicsWorld world)
: base(world)
{
}
public DefaultVehicleRaycaster(DynamicsWorld world)
{
m_dynamicsWorld = world;
}