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();
}
public TestRig(DynamicsWorld ownerWorld, int numLegs, Vector3 position, bool isFixed)
{
_world = ownerWorld;
Legs = new Leg[numLegs];
for (int i = 0; i < numLegs; i++)
{
Legs[i] = new Leg();
}
_bodyShape = new CapsuleShape(BodyRadius, 0.10f);
_thighShape = new CapsuleShape(0.1f, ThighLength);
_shinShape = new CapsuleShape(0.08f, ShinLength);
SetupRigidBodies(position, isFixed);
SetupConstraints();
}
/// <summary>
/// Draw the physical world.
/// </summary>
/// <param name="world">World to draw.</param>
public void DrawDebugWorld(DynamicsWorld world)
{
// no camera? skip
if (Graphics.GraphicsManager.ActiveCamera == null)
{
return;
}
// create effect if needed
if (_effect == null)
{
_effect = new BasicEffect(_device);
_effect.VertexColorEnabled = true;
}
// set effect properties
_effect.View = Graphics.GraphicsManager.ActiveCamera.View;
_effect.Projection = Graphics.GraphicsManager.ActiveCamera.Projection;
// set self as the debug drawer
world.DebugDrawer = this;
// reset depth stencil and rasterizer states
RasterizerState RasterizerState = new RasterizerState();
DepthStencilState DepthStencilState = new DepthStencilState();
RasterizerState.CullMode = CullMode.None;
RasterizerState.DepthClipEnable = true;
RasterizerState.FillMode = FillMode.Solid;
DepthStencilState.DepthBufferEnable = true;
DepthStencilState.DepthBufferWriteEnable = true;
Graphics.GraphicsManager.GraphicsDevice.RasterizerState = RasterizerState;
Graphics.GraphicsManager.GraphicsDevice.DepthStencilState = DepthStencilState;
// apply effect
foreach (EffectPass pass in _effect.CurrentTechnique.Passes)
{
// draw current pass
pass.Apply();
// draw world
world.DebugDrawWorld();
}
}
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);
}
/*
* 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>
/// Instantiates a new scene.
/// </summary>
/// <param name="flags">Specifies the behaviour of the world.</param>
public Scene(SceneCreationFlags flags)
{
this.root = SceneNode.CreateRoot(this);
this.defaultRenderer = new SimpleRenderer();
if (flags.HasFlag(SceneCreationFlags.EnablePhysics))
{
this.CollisionConfiguration = new DefaultCollisionConfiguration();
this.Dispatcher = new CollisionDispatcher(this.CollisionConfiguration);
this.Broadphase = new DbvtBroadphase();
this.Broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback());
this.world = new DiscreteDynamicsWorld(this.Dispatcher, this.Broadphase, null, this.CollisionConfiguration);
// Register physics handler
Game.Current.UpdateNonScene += this.UpdatePhysics;
}
Game.Current.Disposing += this.Dispose;
}
public PrimitiveMeshProcessor(DynamicsWorld world)
{
World = world;
ShaderManager.GetInstance.LoadShader("phys");
shaderProgram = ShaderManager.GetInstance.GetShader("phys");
//setting shader uniform references
foreach (var uni in shaderProgram.GetUniforms)
{
if (uni.Name == "world")
{
worldS = uni;
}
else if (uni.Name == "color")
{
color = uni;
}
}
CreateTri();
}
private static void CleanupBodiesAndShapes(DynamicsWorld world)
{
var shapes = new HashSet <CollisionShape>();
for (int i = world.NumCollisionObjects - 1; i >= 0; i--)
{
CollisionObject obj = world.CollisionObjectArray[i];
var body = obj as RigidBody;
if (body != null && body.MotionState != null)
{
body.MotionState.Dispose();
}
world.RemoveCollisionObject(obj);
GetShapeWithChildShapes(obj.CollisionShape, shapes);
obj.Dispose();
}
foreach (var shape in shapes)
{
shape.Dispose();
}
}
public void InitPhysics()
{
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback());
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SolverMode |= SolverModes.Use2FrictionDirections | SolverModes.RandomizeOrder;
World.SolverInfo.NumIterations = 20;
World.DispatchInfo.AllowedCcdPenetration = 0.0001f;
World.DispatchInfo.UseContinuous = true;
World.Gravity = new Vector3(0, -600, 0);
ClosestConvexResult = new ClosestConvexResultCallback();
EntPhysics MapPhysics = new EntPhysics(libTechCollisionShape.FromVerticesConcave(GetModels().First().GetMeshes().SelectMany(M => M.GetVertices().Select(V => V.Position))), 0);
SpawnEntity(MapPhysics);
}
void PickingPreTickCallback(DynamicsWorld world, float timeStep)
{
if (!drag)
{
return;
}
Vector3 rayFrom = Freelook.Eye;
Vector3 rayTo = GetRayTo(lastMousePos, Freelook.Eye, Freelook.Target, Graphics.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;
}
private static void CleanupConstraints(DynamicsWorld world)
{
var nonWorldObjects = new HashSet <CollisionObject>();
for (int i = world.NumConstraints - 1; i >= 0; i--)
{
TypedConstraint constraint = world.GetConstraint(i);
world.RemoveConstraint(constraint);
if (constraint.RigidBodyA.BroadphaseHandle == null)
{
nonWorldObjects.Add(constraint.RigidBodyA);
}
if (constraint.RigidBodyB.BroadphaseHandle == null)
{
nonWorldObjects.Add(constraint.RigidBodyB);
}
constraint.Dispose();
}
foreach (var obj in nonWorldObjects)
{
obj.Dispose();
}
}
/// <summary>
/// Disposes all native components.
/// </summary>
public void Dispose()
{
Game.Current.Disposing -= this.Dispose;
if (this.PhysicsEnabled)
{
// Unregister physics handler
Game.Current.UpdateNonScene -= this.UpdatePhysics;
}
this.root.Release();
if (this.World != null)
{
this.World.Dispose();
}
if (this.Broadphase != null)
{
this.Broadphase.Dispose();
}
if (this.Dispatcher != null)
{
this.Dispatcher.Dispose();
}
if (this.CollisionConfiguration != null)
{
this.CollisionConfiguration.Dispose();
}
this.world = null;
this.Broadphase = null;
this.Dispatcher = null;
this.CollisionConfiguration = null;
GC.SuppressFinalize(this);
}
//Bullet callback method (equivalent to Unity's FixedUpdate but for Bullet)
private void BulletUpdate(DynamicsWorld world, float bulletTimeStep)
{
//Log debug info if enabled
if (_debugging)
{
DebugReadout = "[BulletDiscreteDynamicsWorld]\n" +
$"RenderDelta: {1000f * Time.deltaTime:n3}(ms)\n" +
$"PhysicsDelta: {1000f * bulletTimeStep:n3}(ms)\n" +
$"PhysicsBodies: {_bulletRigidBodies.Count}\n" +
$"SimCallback: {_bulletBehaviors.Count} Listeners\n";
}
//Return if no behaviors to update
if (_bulletBehaviors.Count == 0)
{
return;
}
//Update all bullet behaviors
foreach (var behavior in _bulletBehaviors)
{
behavior.BulletUpdate(world, bulletTimeStep);
}
}
public override void Run()
{
#region Create renderers
// Note: the renderers take care of creating their own
// device resources and listen for DeviceManager.OnInitialize
// Create a axis-grid renderer
var axisGrid = ToDispose(new AxisGridRenderer());
axisGrid.Initialize(this);
// Create and initialize the mesh renderer
var loadedMesh = Common.Mesh.LoadFromFile("PhysicsScene1.cmo");
List <MeshRenderer> meshes = new List <MeshRenderer>();
meshes.AddRange(from mesh in loadedMesh
select ToDispose(new MeshRenderer(mesh)));
foreach (var m in meshes)
{
m.Initialize(this);
m.World = Matrix.Identity;
}
// Set the first animation as the current animation and start clock
foreach (var m in meshes)
{
if (m.Mesh.Animations != null && m.Mesh.Animations.Any())
{
m.CurrentAnimation = m.Mesh.Animations.First().Value;
}
m.Clock.Start();
}
loadedMesh = Common.Mesh.LoadFromFile("SubdividedPlane.cmo");
var waterMesh = ToDispose(new MeshRenderer(loadedMesh.First()));
waterMesh.Initialize(this);
loadedMesh = Common.Mesh.LoadFromFile("Bataux.cmo");
List <MeshRenderer> shipMeshes = new List <MeshRenderer>();
shipMeshes.AddRange((from mesh in loadedMesh
select ToDispose(new MeshRenderer(mesh))));
foreach (var m in shipMeshes)
{
m.Initialize(this);
m.World = Matrix.Scaling(3) * Matrix.RotationAxis(Vector3.UnitY, -1.57079f);
}
//var anchor = new SphereRenderer(0.05f);
//anchor.Initialize(this);
//var anchorWorld = Matrix.Identity;
//var sphere = new SphereRenderer();
//sphere.Initialize(this);
//var sphereWorld = Matrix.Identity;
// Create and initialize a Direct2D FPS text renderer
var fps = ToDispose(new Common.FpsRenderer("Calibri", Color.CornflowerBlue, new Point(8, 8), 16));
fps.Initialize(this);
// Create and initialize a general purpose Direct2D text renderer
// This will display some instructions and the current view and rotation offsets
var textRenderer = ToDispose(new Common.TextRenderer("Calibri", Color.CornflowerBlue, new Point(8, 40), 12));
textRenderer.Initialize(this);
#endregion
#region Initialize physics engine
CollisionConfiguration defaultConfig = new DefaultCollisionConfiguration();
ConstraintSolver solver = new SequentialImpulseConstraintSolver();
BulletSharp.Dispatcher dispatcher = new CollisionDispatcher(defaultConfig);
BroadphaseInterface broadphase = new DbvtBroadphase();
DynamicsWorld world = null;
Action initializePhysics = () =>
{
RemoveAndDispose(ref world);
world = ToDispose(new BulletSharp.DiscreteDynamicsWorld(dispatcher, broadphase, solver, defaultConfig));
world.Gravity = new Vector3(0, -10, 0);
// For each mesh, create a RigidBody and add to "world" for simulation
meshes.ForEach(m =>
{
// We use the name of the mesh to determine the correct body
if (String.IsNullOrEmpty(m.Mesh.Name))
{
return;
}
var name = m.Mesh.Name.ToLower();
var extent = m.Mesh.Extent;
BulletSharp.CollisionShape shape;
#region Create collision shape
if (name.Contains("box") || name.Contains("cube"))
{
// Assumes the box/cube has an axis-aligned neutral orientation
shape = new BulletSharp.BoxShape(
Math.Abs(extent.Max.Z - extent.Min.Z) / 2.0f,
Math.Abs(extent.Max.Y - extent.Min.Y) / 2.0f,
Math.Abs(extent.Max.X - extent.Min.X) / 2.0f);
}
else if (name.Contains("sphere"))
{
shape = new BulletSharp.SphereShape(extent.Radius);
}
else // use mesh vertices directly
{
// for each SubMesh, retrieve the vertex and index buffers
// to create a TriangleMeshShape for collision detection.
List <Vector3> vertices = new List <Vector3>();
List <int> indices = new List <int>();
int vertexOffset = 0;
foreach (var sm in m.Mesh.SubMeshes)
{
vertexOffset += vertices.Count;
indices.AddRange(
(from indx in m.Mesh.IndexBuffers[(int)sm.IndexBufferIndex]
select vertexOffset + (int)indx));
vertices.AddRange(
(from v in m.Mesh
.VertexBuffers[(int)sm.VertexBufferIndex]
select v.Position - extent.Center));
}
// Create the collision shape
var iva = new BulletSharp.TriangleIndexVertexArray(indices.ToArray(), vertices.ToArray());
shape = new BulletSharp.BvhTriangleMeshShape(iva, true);
}
#endregion
m.World = Matrix.Identity; // Reset mesh location
float mass; Vector3 vec;
shape.GetBoundingSphere(out vec, out mass);
var body = new BulletSharp.RigidBody(
new BulletSharp.RigidBodyConstructionInfo(name.Contains("static") ? 0 : mass,
new MeshMotionState(m),
shape, shape.CalculateLocalInertia(mass)));
if (body.IsStaticObject)
{
body.Restitution = 1f;
body.Friction = 0.4f;
}
// Add to the simulation
world.AddRigidBody(body);
});
#if DEBUG
world.DebugDrawer = ToDispose(new PhysicsDebugDraw(this.DeviceManager));
world.DebugDrawer.DebugMode = DebugDrawModes.DrawAabb | DebugDrawModes.DrawWireframe;
#endif
};
initializePhysics();
// Newton's Cradle
//var box = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.BoxShape(7, 1, 2));
//box.Position = new Jitter.LinearMath.JVector(0, 8, 0);
//world.AddBody(box);
//box.IsStatic = true;
//var anchorBody = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.SphereShape(0.05f));
//anchorBody.Position = new Jitter.LinearMath.JVector(0, 4, 0);
//world.AddBody(anchorBody);
//anchorBody.IsStatic = true;
//for (var bodyCount = -3; bodyCount < 4; bodyCount++)
//{
// var testBody = new Jitter.Dynamics.RigidBody(new Jitter.Collision.Shapes.SphereShape(0.501f));
// testBody.Position = new Jitter.LinearMath.JVector(bodyCount, 0, 0);
// world.AddBody(testBody);
// world.AddConstraint(new Jitter.Dynamics.Constraints.PointPointDistance(box, testBody,
// testBody.Position + Jitter.LinearMath.JVector.Up * 8f + Jitter.LinearMath.JVector.Forward * 3f + Jitter.LinearMath.JVector.Down * 0.5f,
// testBody.Position) { Softness = 1.0f, BiasFactor = 0.8f });
// world.AddConstraint(new Jitter.Dynamics.Constraints.PointPointDistance(box, testBody,
// testBody.Position + Jitter.LinearMath.JVector.Up * 8f + Jitter.LinearMath.JVector.Backward * 3f + Jitter.LinearMath.JVector.Down * 0.5f,
// testBody.Position) { Softness = 1.0f, BiasFactor = 0.8f });
// testBody.Material.Restitution = 1.0f;
// testBody.Material.StaticFriction = 1.0f;
//}
#endregion
// Initialize the world matrix
var worldMatrix = Matrix.Identity;
// Set the camera position slightly behind (z)
var cameraPosition = new Vector3(0, 1, 10);
var cameraTarget = Vector3.Zero; // Looking at the origin 0,0,0
var cameraUp = Vector3.UnitY; // Y+ is Up
// Prepare matrices
// Create the view matrix from our camera position, look target and up direction
var viewMatrix = Matrix.LookAtRH(cameraPosition, cameraTarget, cameraUp);
viewMatrix.TranslationVector += new Vector3(0, -0.98f, 0);
// Create the projection matrix
/* FoV 60degrees = Pi/3 radians */
// Aspect ratio (based on window size), Near clip, Far clip
var projectionMatrix = Matrix.PerspectiveFovRH((float)Math.PI / 3f, Width / (float)Height, 0.1f, 100f);
// Maintain the correct aspect ratio on resize
Window.Resize += (s, e) =>
{
projectionMatrix = Matrix.PerspectiveFovRH((float)Math.PI / 3f, Width / (float)Height, 0.1f, 100f);
};
bool debugDraw = false;
bool paused = false;
var simTime = new System.Diagnostics.Stopwatch();
simTime.Start();
float time = 0.0f;
float timeStep = 0.0f;
#region Rotation and window event handlers
// Create a rotation vector to keep track of the rotation
// around each of the axes
var rotation = new Vector3(0.0f, 0.0f, 0.0f);
// We will call this action to update text
// for the text renderer
Action updateText = () =>
{
textRenderer.Text =
String.Format("Rotation ({0}) (Up/Down Left/Right Wheel+-)\nView ({1}) (A/D, W/S, Shift+Wheel+-)"
//+ "\nPress 1,2,3,4,5,6,7,8 to switch shaders"
+ "\nTime: {2:0.00} (P to toggle, R to reset scene)"
+ "\nPhysics debug draw: {3} (E to toggle)"
+ "\nBackspace: toggle between Physics and Waves",
rotation,
viewMatrix.TranslationVector,
simTime.Elapsed.TotalSeconds,
debugDraw);
};
Dictionary <Keys, bool> keyToggles = new Dictionary <Keys, bool>();
keyToggles[Keys.Z] = false;
keyToggles[Keys.F] = false;
keyToggles[Keys.Back] = false;
// Support keyboard/mouse input to rotate or move camera view
var moveFactor = 0.02f; // how much to change on each keypress
var shiftKey = false;
var ctrlKey = false;
var background = Color.White;
var showNormals = false;
var enableNormalMap = true;
Window.KeyDown += (s, e) =>
{
var context = DeviceManager.Direct3DContext;
shiftKey = e.Shift;
ctrlKey = e.Control;
switch (e.KeyCode)
{
// WASD -> pans view
case Keys.A:
viewMatrix.TranslationVector += new Vector3(moveFactor * 2, 0f, 0f);
break;
case Keys.D:
viewMatrix.TranslationVector -= new Vector3(moveFactor * 2, 0f, 0f);
break;
case Keys.S:
if (shiftKey)
{
viewMatrix.TranslationVector += new Vector3(0f, moveFactor * 2, 0f);
}
else
{
viewMatrix.TranslationVector -= new Vector3(0f, 0f, 1) * moveFactor * 2;
}
break;
case Keys.W:
if (shiftKey)
{
viewMatrix.TranslationVector -= new Vector3(0f, moveFactor * 2, 0f);
}
else
{
viewMatrix.TranslationVector += new Vector3(0f, 0f, 1) * moveFactor * 2;
}
break;
// Up/Down and Left/Right - rotates around X / Y respectively
// (Mouse wheel rotates around Z)
case Keys.Down:
worldMatrix *= Matrix.RotationX(moveFactor);
rotation += new Vector3(moveFactor, 0f, 0f);
break;
case Keys.Up:
worldMatrix *= Matrix.RotationX(-moveFactor);
rotation -= new Vector3(moveFactor, 0f, 0f);
break;
case Keys.Left:
worldMatrix *= Matrix.RotationY(moveFactor);
rotation += new Vector3(0f, moveFactor, 0f);
break;
case Keys.Right:
worldMatrix *= Matrix.RotationY(-moveFactor);
rotation -= new Vector3(0f, moveFactor, 0f);
break;
case Keys.T:
fps.Show = !fps.Show;
textRenderer.Show = !textRenderer.Show;
break;
case Keys.B:
if (background == Color.White)
{
background = new Color(30, 30, 34);
}
else
{
background = Color.White;
}
break;
case Keys.G:
axisGrid.Show = !axisGrid.Show;
break;
case Keys.P:
paused = !paused;
if (paused)
{
simTime.Stop();
}
else
{
simTime.Start();
}
// Pause or resume mesh animation
meshes.ForEach(m => {
if (m.Clock.IsRunning)
{
m.Clock.Stop();
}
else
{
m.Clock.Start();
}
});
updateText();
break;
case Keys.X:
// To test for correct resource recreation
// Simulate device reset or lost.
System.Diagnostics.Debug.WriteLine(SharpDX.Diagnostics.ObjectTracker.ReportActiveObjects());
DeviceManager.Initialize(DeviceManager.Dpi);
System.Diagnostics.Debug.WriteLine(SharpDX.Diagnostics.ObjectTracker.ReportActiveObjects());
break;
case Keys.Z:
keyToggles[Keys.Z] = !keyToggles[Keys.Z];
if (keyToggles[Keys.Z])
{
context.PixelShader.Set(depthPixelShader);
}
else
{
context.PixelShader.Set(pixelShader);
}
break;
case Keys.F:
keyToggles[Keys.F] = !keyToggles[Keys.F];
RasterizerStateDescription rasterDesc;
if (context.Rasterizer.State != null)
{
rasterDesc = context.Rasterizer.State.Description;
}
else
{
rasterDesc = new RasterizerStateDescription()
{
CullMode = CullMode.None,
FillMode = FillMode.Solid
}
};
if (keyToggles[Keys.F])
{
rasterDesc.FillMode = FillMode.Wireframe;
context.Rasterizer.State = ToDispose(new RasterizerState(context.Device, rasterDesc));
}
else
{
rasterDesc.FillMode = FillMode.Solid;
context.Rasterizer.State = ToDispose(new RasterizerState(context.Device, rasterDesc));
}
break;
case Keys.N:
if (!shiftKey)
{
showNormals = !showNormals;
}
else
{
enableNormalMap = !enableNormalMap;
}
break;
case Keys.E:
debugDraw = !debugDraw;
break;
case Keys.R:
//world = new Jitter.World(new Jitter.Collision.CollisionSystemSAP());
initializePhysics();
if (simTime.IsRunning)
{
simTime.Restart();
}
else
{
simTime.Reset();
}
break;
case Keys.D1:
context.PixelShader.Set(pixelShader);
break;
case Keys.D2:
context.PixelShader.Set(lambertShader);
break;
case Keys.D3:
context.PixelShader.Set(phongShader);
break;
case Keys.D4:
context.PixelShader.Set(blinnPhongShader);
break;
case Keys.Back:
keyToggles[Keys.Back] = !keyToggles[Keys.Back];
break;
}
updateText();
};
Window.KeyUp += (s, e) =>
{
// Clear the shift/ctrl keys so they aren't sticky
if (e.KeyCode == Keys.ShiftKey)
{
shiftKey = false;
}
if (e.KeyCode == Keys.ControlKey)
{
ctrlKey = false;
}
};
Window.MouseWheel += (s, e) =>
{
if (shiftKey)
{
// Zoom in/out
viewMatrix.TranslationVector += new Vector3(0f, 0f, (e.Delta / 120f) * moveFactor * 2);
}
else
{
// rotate around Z-axis
viewMatrix *= Matrix.RotationZ((e.Delta / 120f) * moveFactor);
rotation += new Vector3(0f, 0f, (e.Delta / 120f) * moveFactor);
}
updateText();
};
var lastX = 0;
var lastY = 0;
Window.MouseDown += (s, e) =>
{
if (e.Button == MouseButtons.Left)
{
lastX = e.X;
lastY = e.Y;
}
};
Window.MouseMove += (s, e) =>
{
if (e.Button == MouseButtons.Left)
{
var yRotate = lastX - e.X;
var xRotate = lastY - e.Y;
lastY = e.Y;
lastX = e.X;
// Mouse move changes
viewMatrix *= Matrix.RotationX(-xRotate * moveFactor);
viewMatrix *= Matrix.RotationY(-yRotate * moveFactor);
updateText();
}
};
// Display instructions with initial values
updateText();
#endregion
var clock = new System.Diagnostics.Stopwatch();
clock.Start();
#region Render loop
// Create and run the render loop
RenderLoop.Run(Window, () =>
{
// Update simulation, at 60fps
if (!paused)
{
if ((float)simTime.Elapsed.TotalSeconds < time)
{
time = 0;
timeStep = 0;
}
timeStep = ((float)simTime.Elapsed.TotalSeconds - time);
time = (float)simTime.Elapsed.TotalSeconds;
world.StepSimulation(timeStep, 7);
// For how to choose the maxSubSteps see:
// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Stepping_The_World
}
updateText();
// Start of frame:
// Retrieve immediate context
var context = DeviceManager.Direct3DContext;
// Clear depth stencil view
context.ClearDepthStencilView(DepthStencilView, DepthStencilClearFlags.Depth | DepthStencilClearFlags.Stencil, 1.0f, 0);
// Clear render target view
context.ClearRenderTargetView(RenderTargetView, background);
// Create viewProjection matrix
var viewProjection = Matrix.Multiply(viewMatrix, projectionMatrix);
// Extract camera position from view
var camPosition = Matrix.Transpose(Matrix.Invert(viewMatrix)).Column4;
cameraPosition = new Vector3(camPosition.X, camPosition.Y, camPosition.Z);
var perFrame = new ConstantBuffers.PerFrame();
perFrame.Light.Color = new Color(0.8f, 0.8f, 0.8f, 1.0f);
var lightDir = Vector3.Transform(new Vector3(1f, -1f, -1f), worldMatrix);
perFrame.Light.Direction = new Vector3(lightDir.X, lightDir.Y, lightDir.Z);
perFrame.CameraPosition = cameraPosition;
perFrame.Time = (float)simTime.Elapsed.TotalSeconds; // Provide simulation time to shader
context.UpdateSubresource(ref perFrame, perFrameBuffer);
// Render each object
var perMaterial = new ConstantBuffers.PerMaterial();
perMaterial.Ambient = new Color4(0.2f);
perMaterial.Diffuse = Color.White;
perMaterial.Emissive = new Color4(0);
perMaterial.Specular = Color.White;
perMaterial.SpecularPower = 20f;
perMaterial.HasTexture = 0;
perMaterial.UVTransform = Matrix.Identity;
context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
var perObject = new ConstantBuffers.PerObject();
// MESH
if (!keyToggles[Keys.Back])
{
meshes.ForEach((m) =>
{
perObject.World = m.World * worldMatrix;
// Provide the material constant buffer to the mesh renderer
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.ViewProjection = viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
m.PerMaterialBuffer = perMaterialBuffer;
m.PerArmatureBuffer = perArmatureBuffer;
m.Render();
if (showNormals)
{
using (var prevPixelShader = context.PixelShader.Get())
{
perMaterial.HasTexture = 0;
perMaterial.UVTransform = Matrix.Identity;
context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
context.PixelShader.Set(pixelShader);
context.GeometryShader.Set(debugNormals);
m.Render();
context.PixelShader.Set(prevPixelShader);
context.GeometryShader.Set(null);
}
}
});
if (debugDraw)
{
perObject.World = Matrix.Identity;
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.ViewProjection = viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
(world.DebugDrawer as PhysicsDebugDraw).DrawDebugWorld(world);
context.VertexShader.Set(vertexShader);
context.PixelShader.Set(pixelShader);
context.InputAssembler.InputLayout = vertexLayout;
}
}
else
{
perObject.World = waterMesh.World * worldMatrix;
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.ViewProjection = viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
waterMesh.EnableNormalMap = enableNormalMap;
waterMesh.PerMaterialBuffer = perMaterialBuffer;
waterMesh.PerArmatureBuffer = perArmatureBuffer;
context.VertexShader.Set(waterVertexShader);
waterMesh.Render();
if (showNormals)
{
using (var prevPixelShader = context.PixelShader.Get())
{
perMaterial.HasTexture = 0;
perMaterial.UVTransform = Matrix.Identity;
context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
context.PixelShader.Set(pixelShader);
context.GeometryShader.Set(debugNormals);
waterMesh.Render();
context.PixelShader.Set(prevPixelShader);
context.GeometryShader.Set(null);
}
}
context.VertexShader.Set(vertexShader);
foreach (var m in shipMeshes)
{
perObject.World = m.World * worldMatrix;
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
// Provide the material constant buffer to the mesh renderer
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.ViewProjection = viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
m.PerMaterialBuffer = perMaterialBuffer;
m.PerArmatureBuffer = perArmatureBuffer;
m.Render();
if (showNormals)
{
using (var prevPixelShader = context.PixelShader.Get())
{
perMaterial.HasTexture = 0;
perMaterial.UVTransform = Matrix.Identity;
context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
context.PixelShader.Set(pixelShader);
context.GeometryShader.Set(debugNormals);
m.Render();
context.PixelShader.Set(prevPixelShader);
context.GeometryShader.Set(null);
}
}
}
}
perMaterial.Ambient = new Color4(0.2f);
perMaterial.Diffuse = Color.White;
perMaterial.Emissive = new Color4(0);
perMaterial.Specular = Color.White;
perMaterial.SpecularPower = 20f;
perMaterial.UVTransform = Matrix.Identity;
context.UpdateSubresource(ref perMaterial, perMaterialBuffer);
// AXIS GRID
context.HullShader.Set(null);
context.DomainShader.Set(null);
context.GeometryShader.Set(null);
using (var prevPixelShader = context.PixelShader.Get())
using (var prevVertexShader = context.VertexShader.Get())
{
context.VertexShader.Set(vertexShader);
context.PixelShader.Set(pixelShader);
perObject.World = worldMatrix;
perObject.WorldInverseTranspose = Matrix.Transpose(Matrix.Invert(perObject.World));
perObject.WorldViewProjection = perObject.World * viewProjection;
perObject.ViewProjection = viewProjection;
perObject.Transpose();
context.UpdateSubresource(ref perObject, perObjectBuffer);
axisGrid.Render();
context.PixelShader.Set(prevPixelShader);
context.VertexShader.Set(prevVertexShader);
}
// Render FPS
fps.Render();
// Render instructions + position changes
textRenderer.Render();
// Present the frame
Present();
});
#endregion
}
public static void DeleteAndDisposeConstraint(this DynamicsWorld world, TypedConstraint constraint)
{
world.RemoveConstraint(constraint);
constraint.UserObject = null;
constraint.Dispose();
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
private void Initialize()
{
Inventor.Application InvApp = AdnInventorUtilities.InvApplication;
if(!(InvApp.ActiveDocument is AssemblyDocument))
return;
AssemblyDocument doc = InvApp.ActiveDocument as AssemblyDocument;
_dynamicsWorld = new DynamicsWorld();
_mapOccurrencesToBodies.Clear();
//create dynamic bodies
double[] transfo = new double[16];
for(int idx=1; idx < doc.ComponentDefinition.Occurrences.Count; ++idx)
{
ComponentOccurrence occurrence = doc.ComponentDefinition.Occurrences[idx];
PartComponentDefinition compDef = occurrence.Definition as PartComponentDefinition;
if (occurrence.Grounded)
{
}
else
{
occurrence.Transformation.GetMatrixData(ref transfo);
int vertexCount;
int facetCount;
double[] vertexCoords = new double[]{};
double[] normals = new double[]{};
int[] indices = new int[]{};
compDef.SurfaceBodies[1].CalculateFacets(0.01,
out vertexCount,
out facetCount,
out vertexCoords,
out normals,
out indices);
RigidBody body = new RigidBody(facetCount, vertexCoords, normals, indices, occurrence.MassProperties.Mass, transfo);
ValueTypeEnum type;
int vx = 0, vy = 0, vz = 0;
object ovx = null, ovy = null, ovz = null;
if (AdnInventorUtilities.ReadAttribute((object)occurrence, "Simulation", "xVelInit", out ovx, out type))
vx = (int)ovx;
if(AdnInventorUtilities.ReadAttribute((object)occurrence, "Simulation", "yVelInit", out ovy, out type))
vy = (int)ovy;
if(AdnInventorUtilities.ReadAttribute((object)occurrence, "Simulation", "zVelInit", out ovz, out type))
vz = (int)ovz;
body.SetLinearVelocity(vx, vy, vz);
_mapOccurrencesToBodies.Add(occurrence, body);
_dynamicsWorld.AddRigidBody(body);
}
}
ComponentOccurrence groundOcc =
doc.ComponentDefinition.Occurrences[doc.ComponentDefinition.Occurrences.Count];
double[] pos = new double[]
{
groundOcc.Transformation.Translation.X,
groundOcc.Transformation.Translation.Y,
groundOcc.Transformation.Translation.Z
};
RigidBody groundBody = new RigidBody(pos);
_dynamicsWorld.AddRigidBody(groundBody);
}
public CustomBulletWorldImporter(DynamicsWorld world)
: base(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.07f, 0.45f);
shapes[(int)BodyPart.LeftLowerLeg] = new CapsuleShape(0.05f, 0.37f);
shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.07f, 0.45f);
shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.05f, 0.37f);
shapes[(int)BodyPart.LeftUpperArm] = new CapsuleShape(0.05f, 0.33f);
shapes[(int)BodyPart.LeftLowerArm] = new CapsuleShape(0.04f, 0.25f);
shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.05f, 0.33f);
shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.04f, 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.RotationX((float)Math.PI / 2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);
transform = Matrix.RotationY((float)Math.PI / 2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);
transform = Matrix.RotationY(-(float)Math.PI / 2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);
transform = Matrix.RotationY(-(float)Math.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(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.15f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.15f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
hingeC.SetLimit(-(float)Math.PI / 4, -(float)Math.PI / 2);
joints[(int)Joint.PelvisSpine] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.PelvisSpine], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, 0.30f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2) * Matrix.Translation(0, -0.14f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.Head], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 2);
joints[(int)Joint.SpineHead] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.SpineHead], true);
localA = Matrix.RotationYawPitchRoll(0, 0, -5 * (float)Math.PI / 4) * Matrix.Translation(-0.18f, -0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, -5 * (float)Math.PI / 4) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, 0);
joints[(int)Joint.LeftHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftHip], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperLeg], bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.LeftKnee] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftKnee], true);
localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 4) * Matrix.Translation(0.18f, -0.10f, 0);
localB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 4) * Matrix.Translation(0, 0.225f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.RightUpperLeg], localA, localB);
coneC.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, 0);
joints[(int)Joint.RightHip] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightHip], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.225f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.185f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
hingeC.SetLimit(0, (float)Math.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, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
joints[(int)Joint.LeftShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.LeftShoulder], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
hingeC.SetLimit(0, (float)Math.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, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
joints[(int)Joint.RightShoulder] = coneC;
coneC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightShoulder], true);
localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.18f, 0);
localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.14f, 0);
hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperArm], bodies[(int)BodyPart.RightLowerArm], localA, localB);
//hingeC.SetLimit(-(float)Math.PI / 2, 0);
hingeC.SetLimit(0, (float)Math.PI / 2);
joints[(int)Joint.RightElbow] = hingeC;
hingeC.DebugDrawSize = ConstraintDebugSize;
ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
}
public BspToBulletConverter(DynamicsWorld world)
{
_world = world;
}
public TestRig(DemoApplication demoApplication, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, bool bFixed)
{
m_dynamicsWorld = ownerWorld;
IndexedVector3 vUp = new IndexedVector3(0, 1, 0);
//
// Setup geometry
//
float fBodySize = 0.25f;
float fLegLength = 0.45f;
float fForeLegLength = 0.75f;
m_shapes[0] = new CapsuleShape(fBodySize, 0.10f);
for (int i = 0; i < NUM_LEGS; i++)
{
m_shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
m_shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
}
//
// Setup rigid bodies
//
float fHeight = 0.5f;
IndexedMatrix offset = IndexedMatrix.Identity;
offset._origin = positionOffset;
// root
IndexedVector3 vRoot = new IndexedVector3(0, fHeight, 0);
IndexedMatrix transform = IndexedMatrix.Identity;
transform._origin = vRoot;
if (bFixed)
{
m_bodies[0] = demoApplication.LocalCreateRigidBody(0.0f, offset * transform, m_shapes[0]);
}
else
{
m_bodies[0] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[0]);
}
// legs
for (int i = 0; i < NUM_LEGS; i++)
{
float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
transform = IndexedMatrix.Identity;
IndexedVector3 vBoneOrigin = new IndexedVector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));
transform._origin = vBoneOrigin;
// thigh
IndexedVector3 vToBone = (vBoneOrigin - vRoot);
vToBone.Normalize();
IndexedVector3 vAxis = IndexedVector3.Cross(vToBone, vUp);
transform._basis = new IndexedBasisMatrix(new IndexedQuaternion(vAxis, MathUtil.SIMD_HALF_PI));
transform._origin = vBoneOrigin;
m_bodies[1 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[1 + 2 * i]);
// shin
transform = IndexedMatrix.Identity;
transform._origin = new IndexedVector3(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
m_bodies[2 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[2 + 2 * i]);
}
// Setup some damping on the m_bodies
for (int i = 0; i < BODYPART_COUNT; ++i)
{
m_bodies[i].SetDamping(0.05f, 0.85f);
m_bodies[i].SetDeactivationTime(0.8f);
//m_bodies[i].setSleepingThresholds(1.6, 2.5);
m_bodies[i].SetSleepingThresholds(0.5f, 0.5f);
}
//
// Setup the constraints
//
HingeConstraint hingeC;
IndexedMatrix localA, localB, localC;
for (int i = 0; i < NUM_LEGS; i++)
{
float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
float fSin = (float)Math.Sin(fAngle);
float fCos = (float)Math.Cos(fAngle);
// hip joints
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
localA._origin = new IndexedVector3(fCos * fBodySize, 0.0f, fSin * fBodySize);
localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
if (BulletGlobals.g_streamWriter != null && false)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalA", localA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalB", localB);
}
hingeC = new HingeConstraint(m_bodies[0], m_bodies[1 + 2 * i], ref localA, ref localB);
hingeC.SetLimit(-0.75f * MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI / 2f);
m_joints[2 * i] = hingeC;
m_dynamicsWorld.AddConstraint(m_joints[2 * i], true);
// knee joints
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localC = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
localA._origin = new IndexedVector3(fCos * (fBodySize + fLegLength), 0.0f, fSin * (fBodySize + fLegLength));
localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
localC = m_bodies[2 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
if (BulletGlobals.g_streamWriter != null && false)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalA", localA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalB", localB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalC", localC);
}
hingeC = new HingeConstraint(m_bodies[1 + 2 * i], m_bodies[2 + 2 * i], ref localB, ref localC);
//hingeC.setLimit(float(-0.01), float(0.01));
hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI / 2f, 0.2f);
m_joints[1 + 2 * i] = hingeC;
m_dynamicsWorld.AddConstraint(m_joints[1 + 2 * i], true);
}
}
/// <summary> /// Called once per Bullet time step /// </summary> /// <param name="world">The BulletPhysicsWorld from which the call originated</param> /// <param name="bulletTimeStep">The time (s) that the simulation has stepped</param> public abstract void BulletUpdate(DynamicsWorld world, float bulletTimeStep);
public BoxShooter(DynamicsWorld world)
{
_world = world;
}
void WorldPreTickCallback(DynamicsWorld world2, double timeStep)
static void WorldPreTickCallback(DynamicsWorld world, float timeStep)
{
Console.WriteLine("WorldPreTickCallback");
}
/// <summary>
/// Initializes a new BVehicleRaycaster.
/// </summary>
/// <param name="world"></param>
public BRobotRaycaster(DynamicsWorld world)
{
dynamicsWorld = world;
}
void MotorPreTickCallback(DynamicsWorld world, float timeStep)
{
SetMotorTargets(timeStep);
}
public DrawingResult(DynamicsWorld world)
{
_world = world;
}
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);
}
}
public void InternalTickCallback(DynamicsWorld world, float timeStep)
{
MotorDemo motorDemo = (MotorDemo)world.GetWorldUserInfo();
motorDemo.SetMotorTargets(timeStep);
}
public MyContactResultCallback(CollisionDispatcher dispatch, DynamicsWorld world, RigidBody boxBody)
{
dispatcher = dispatch;
_world = world;
bodyCollision = boxBody;
}
/*public void SpawnCollisionObject()
* {
* Entity collisionObject = entityManager.CreateEntity(CollisionEntity);
* entityManager.SetSharedComponentData(collisionObject, SphereSmallRendererComponent.Value);
* entityManager.SetComponentData(collisionObject, new Unity.Transforms.Position { Value = startPos });
* entityManager.SetSharedComponentData(sphereRender, new FollowRigidBody { RigidBodyEntity = rigidBody });
* }*/
protected 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 BulletSharp.Math.Vector3(0, -10, 0);
BoxShape groundShape = new BoxShape(10, 0.5f, 10);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0.0f, Matrix.Identity, groundShape, true);
ground.UserObject = "Ground";
BoxShape WallShape1 = new BoxShape(0.5f, 25, 10);
CollisionShapes.Add(WallShape1);
CollisionObject wall1 = LocalCreateRigidBody(0.0f, Matrix.Translation(new BulletSharp.Math.Vector3(-10, 25, 0)), WallShape1, true);
wall1.UserObject = "Ground";
CollisionShapes.Add(WallShape1);
CollisionObject wall2 = LocalCreateRigidBody(0.0f, Matrix.Translation(new BulletSharp.Math.Vector3(10, 25, 0)), WallShape1, true);
wall2.UserObject = "Ground";
BoxShape WallShape2 = new BoxShape(10, 25, 0.5f);
CollisionShapes.Add(WallShape2);
CollisionObject wall3 = LocalCreateRigidBody(0.0f, Matrix.Translation(new BulletSharp.Math.Vector3(0, 25, -10)), WallShape2, true);
wall3.UserObject = "Ground";
CollisionShapes.Add(WallShape2);
CollisionObject wall4 = LocalCreateRigidBody(0.0f, Matrix.Translation(new BulletSharp.Math.Vector3(0, 25, 10)), WallShape2, true);
wall4.UserObject = "Ground";
float cubeSize = 0.5f;
float spacing = cubeSize;
float mass = 1.0f;
int size = 8;
BulletSharp.Math.Vector3 pos = new BulletSharp.Math.Vector3(0.0f, 20, 0.0f);
float offset = -size / 2 + 0.5f;
// 3000
BoxShape blockShape = new BoxShape(cubeSize);
mass = 2.0f;
for (int k = 0; k < MaxHeight; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + j * (cubeSize * 2.0f);
for (int i = 0; i < size; i++)
{
pos[0] = offset + i * (cubeSize * 2.0f);
/*RigidBody cmbody =*/
LocalCreateRigidBody(mass, Matrix.Translation(pos), blockShape);
}
}
pos[1] += (cubeSize + spacing) * 2.0f;
}
}
