/// <summary>
/// Initialization
/// </summary>
/// <param name="device"></param>
/// <param name="physicsEngine"></param>
public override void Initialize(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
{
try
{
// GPS sensor dimensions and relative position
_shape = new BoxShape(new BoxShapeProperties(
"GPS Sensor",
0.01f,
new Pose(new Vector3(0,0.8f,0)),
new Vector3(0.01f, 0.01f, 0.01f)));
State.PhysicsPrimitives.Add(_shape);
base.Initialize(device, physicsEngine);
if (Parent == null)
{
throw new Exception("GPS Sensor entity must be a child of another entity.");
}
CreateAndInsertPhysicsEntity(physicsEngine);
Flags |= VisualEntityProperties.DisableRendering;
AddShapeToPhysicsEntity(_shape, new VisualEntityMesh(device, 0.01f, 0.01f));
}
catch (Exception ex)
{
HasBeenInitialized = false;
InitError = ex.ToString();
}
}
public static string GetStringfromBoxShape(BoxShape bs)
{
//前面添加了BoxUnit_
switch (bs)
{
case BoxShape.OBINARY:
return "Obinary.png";
case BoxShape.BLANK:
return "Blank.png";
case BoxShape.I:
return "I.png";
case BoxShape.J:
return "J.png";
case BoxShape.L:
return "L.png";
case BoxShape.O:
return "O.png";
case BoxShape.S:
return "S.png";
case BoxShape.Z:
return "Z.png";
case BoxShape.T:
return "T.png";
case BoxShape.SHADOW:
return "Shadow.png";
case BoxShape.BAN:
return "Ban.png";
default:
return "Blank.png";
}
}
public void AddChildShape(float4x4 localTransform, IBoxShapeImp shape)
{
Debug.WriteLine("AddBox");
var btHalfExtents = Translater.Float3ToBtVector3(shape.HalfExtents);
var btChildShape = new BoxShape(btHalfExtents);
var btLocalTransform = Translater.Float4X4ToBtMatrix(localTransform);
BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
}
public void Test_CreateBulletObject()
{
var box = new BoxShape (1, 1, 1);
Assert.IsNotNull (box.CreateGhostObject ());
Assert.IsNotNull (box.CreateRigidBody (1));
Assert.IsNotNull (box.CreateBulletShape());
}
public void Test_CreateShape()
{
var box = new BoxShape (1, 2, 3);
var shp = box.CreateBulletShape () as BulletSharp.BoxShape;
var size = shp.HalfExtentsWithMargin.ToDD() * PhysicsSimulator.PPM;
Assert.AreEqual(new Vector3(1,2,3), size);
}
public void Extent()
{
BoxShape b = new BoxShape();
b.Extent = new Vector3F(1, 2, 3);
Assert.AreEqual(1, b.WidthX);
Assert.AreEqual(2, b.WidthY);
Assert.AreEqual(3, b.WidthZ);
}
//CollisionShapes
//Primitives
//BoxShape
public IBoxShapeImp AddBoxShape(float boxHalfExtents)
{
var btBoxShape = new BoxShape(boxHalfExtents);
BtCollisionShapes.Add(btBoxShape);
var retval = new BoxShapeImp();
retval.BtBoxShape = btBoxShape;
btBoxShape.UserObject = retval;
return retval;
}
public void Clone()
{
BoxShape box = new BoxShape(0.1234f, 2.345f, 5.43f);
BoxShape clone = box.Clone() as BoxShape;
Assert.IsNotNull(clone);
Assert.AreEqual(box.WidthX, clone.WidthX);
Assert.AreEqual(box.WidthY, clone.WidthY);
Assert.AreEqual(box.WidthZ, clone.WidthZ);
Assert.AreEqual(box.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(box.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void Test_New_2()
{
var box = new BoxShape (1);
Assert.AreEqual (1, box.HalfWidth);
Assert.AreEqual (1, box.HalfHeight);
Assert.AreEqual (1, box.HalfDepth);
Assert.AreEqual (2, box.Width);
Assert.AreEqual (2, box.Height);
Assert.AreEqual (2, box.Depth);
}
public void Draw(BoxShape bs)
{
try
{
label.Dispatcher.Invoke(
new Action(
delegate
{
label.Background = BoxFactory.GetBoxImageBrush(bs);
}));
}
catch
{ }
}
public static ImageBrush GetBoxImageBrush(BoxShape bs)
{
if (bs == BoxShape.BLANK || bs == null) return null;
if ((int)bs < 20)
{
if (brush[(int)bs] == null)
{
string path = System.Environment.CurrentDirectory + "/images/BoxUnit_";
path += Resources.GetStringfromBoxShape(bs);
if (File.Exists(path))
brush[(int)bs] = new ImageBrush(new BitmapImage(new Uri(path, UriKind.Absolute)));
}
return brush[(int)bs];
}
else
return null;
}
public override void NewBoxArrive(BoxShape active, BoxShape ready)
{
Console.WriteLine();
Console.WriteLine();
//Console.WriteLine("New Box Type is " + ready.ToString()+" Box Id:{0} ========================",aiFrame.boxNum);
perioty = 0;
operations = new List<Key>();
int i, j;
for (j = 0; j < aiFrame.activeBox.Shapes().Count; j++)
{
//Console.WriteLine(" Shape Change Time is {0}",j);
for (i = col / 2; i < col; i++)
{
int mark = MakeTry(i, j, active);
//Console.WriteLine(" Position {0} : Mark is {1}",i,mark);
//if (mark < -10) PaintMap();
if (mark == -1) break;
if (mark > perioty)
{
perioty = mark;
operations = tempOpr;
}
}
for (i = col / 2 - 1; i >= 0; i--)
{
int mark = MakeTry(i, j, active);
// Console.WriteLine(" Position {0} : Mark is {1}", i, mark);
if (mark == -1) break;
if (mark > perioty)
{
perioty = mark;
operations = tempOpr;
}
}
}
}
protected override void OnLoad()
{
// Add rigid bodies to simulation.
var simulation = _services.GetInstance <Simulation>();
// ----- Add a ground plane.
AddBody(simulation, "GroundPlane", Pose.Identity, new PlaneShape(Vector3.UnitY, 0), MotionType.Static);
// ----- Create a height field.
var numberOfSamplesX = 20;
var numberOfSamplesZ = 20;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
if (x == 0 || z == 0 || x == 19 || z == 19)
{
samples[z * numberOfSamplesX + x] = -1;
}
else
{
samples[z * numberOfSamplesX + x] = 1.0f + (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 1.0f);
}
}
}
HeightField heightField = new HeightField(0, 0, 120, 120, samples, numberOfSamplesX, numberOfSamplesZ);
//heightField.UseFastCollisionApproximation = true;
AddBody(simulation, "HeightField", new Pose(new Vector3(10, 0, 20)), heightField, MotionType.Static);
// ----- Create rubble on the floor (small random objects on the floor).
for (int i = 0; i < 60; i++)
{
Vector3 position = new Vector3(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20));
Quaternion orientation = RandomHelper.Random.NextQuaternion();
BoxShape shape = new BoxShape(RandomHelper.Random.NextVector3(0.05f, 0.5f));
AddBody(simulation, "Stone" + i, new Pose(position, orientation), shape, MotionType.Static);
}
// ----- Slopes with different tilt angles.
// Create a loop.
Vector3 slopePosition = new Vector3(-20, -0.25f, -5);
BoxShape slopeShape = new BoxShape(8, 0.5f, 2);
for (int i = 1; i < 33; i++)
{
Matrix oldRotation = Matrix.CreateRotationX((i - 1) * MathHelper.ToRadians(10));
Matrix rotation = Matrix.CreateRotationX(i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3(0, 0, -slopeShape.WidthZ)) / 2;
AddBody(simulation, "Loop" + i, new Pose(slopePosition, rotation), slopeShape, MotionType.Static);
}
// Create an arched bridge.
slopePosition = new Vector3(-10, -2, -15);
slopeShape = new BoxShape(8f, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix oldRotation = Matrix.CreateRotationX(MathHelper.ToRadians(40) - (i - 1) * MathHelper.ToRadians(10));
Matrix rotation = Matrix.CreateRotationX(MathHelper.ToRadians(40) - i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3(0, 0, -slopeShape.WidthZ)) / 2;
Vector3 position = slopePosition - rotation * new Vector3(0, slopeShape.WidthY / 2, 0);
AddBody(simulation, "Bridge" + i, new Pose(position, rotation), slopeShape, MotionType.Static);
}
// ----- Create a mesh object.
// We first build a composite shape out of several primitives and then convert the
// composite shape to a triangle mesh. (Just for testing.)
CompositeShape compositeShape = new CompositeShape();
compositeShape.Children.Add(new GeometricObject(heightField, Pose.Identity));
compositeShape.Children.Add(new GeometricObject(new CylinderShape(1, 2), new Pose(new Vector3(10, 1, 10))));
compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3(15, 0, 15))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Pose(new Vector3(15, 0, 5))));
ITriangleMesh mesh = compositeShape.GetMesh(0.01f, 3);
TriangleMeshShape meshShape = new TriangleMeshShape(mesh, true);
meshShape.Partition = new AabbTree <int>()
{
BottomUpBuildThreshold = 0
};
AddBody(simulation, "Mesh", new Pose(new Vector3(-120, 0, 20)), meshShape, MotionType.Static);
// ----- Create a seesaw.
var seesawBase = AddBody(simulation, "SeesawBase", new Pose(new Vector3(15, 0.5f, 0)), new BoxShape(0.2f, 1, 6), MotionType.Static);
var seesaw = AddBody(simulation, "Seesaw", new Pose(new Vector3(16, 1.05f, 0)), new BoxShape(15, 0.1f, 6), MotionType.Dynamic);
seesaw.MassFrame.Mass = 500;
seesaw.CanSleep = false;
// Connect seesaw using a hinge joint.
simulation.Constraints.Add(new HingeJoint
{
BodyA = seesaw,
BodyB = seesawBase,
AnchorPoseALocal = new Pose(new Vector3(1.0f, 0, 0),
new Matrix(0, 0, -1,
0, 1, 0,
1, 0, 0)),
AnchorPoseBLocal = new Pose(new Vector3(0, 0.5f, 0),
new Matrix(0, 0, -1,
0, 1, 0,
1, 0, 0)),
CollisionEnabled = false,
});
// ----- Distribute a few dynamic spheres and boxes across the landscape.
SphereShape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 40; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-60, 60);
position.Y = 10;
AddBody(simulation, "Sphere" + i, new Pose(position), sphereShape, MotionType.Dynamic);
}
BoxShape boxShape = new BoxShape(1.0f, 1.0f, 1.0f);
for (int i = 0; i < 40; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-60, 60);
position.Y = 1;
AddBody(simulation, "Box" + i, new Pose(position), boxShape, MotionType.Dynamic);
}
}
public static void Load(CollisionDomain collisionDomain)
{
// Create a box for the ground.
AddObject("Ground", new Pose(new Vector3(0, -5, 0)), new BoxShape(60, 10, 60), collisionDomain);
// Create a small flying sphere to visualize the approx. head height. - This is just
// for debugging so that we have a feeling for heights.
AddObject("Sphere", new Pose(new Vector3(0, 1.5f, 0)), new SphereShape(0.2f), collisionDomain);
// Create small walls at the level boundary.
AddObject("WallLeft", new Pose(new Vector3(-30, 1, 0)), new BoxShape(0.3f, 2, 60), collisionDomain);
AddObject("WallRight", new Pose(new Vector3(30, 1, 0)), new BoxShape(0.3f, 2, 60), collisionDomain);
AddObject("WallFront", new Pose(new Vector3(0, 1, -30)), new BoxShape(60, 2, 0.3f), collisionDomain);
AddObject("WallBack", new Pose(new Vector3(0, 1, 30)), new BoxShape(60, 2, 0.3f), collisionDomain);
// Create a few bigger objects.
// We position the boxes so that we have a few corners we can run into. Character controllers
// should be stable when the user runs into corners.
AddObject("House0", new Pose(new Vector3(10, 1, -10)), new BoxShape(8, 2, 8f), collisionDomain);
AddObject("House1", new Pose(new Vector3(13, 1, -4)), new BoxShape(2, 2, 4), collisionDomain);
AddObject("House2", new Pose(new Vector3(10, 2, -15), Matrix.CreateRotationY(-0.3f)), new BoxShape(8, 4, 2), collisionDomain);
//
// Create stairs with increasing step height.
//
// Each step is a box. With this object we can test if our character can climb up
// stairs. The character controller has a step height limit. Increasing step heights
// let us test if the step height limit works.
float startHeight = 0;
const float stepDepth = 1f;
for (int i = 0; i < 10; i++)
{
float stepHeight = 0.1f + i * 0.05f;
Pose pose = new Pose(new Vector3(0, startHeight + stepHeight / 2, -2 - i * stepDepth));
BoxShape shape = new BoxShape(2, stepHeight, stepDepth);
AddObject("Step" + i, pose, shape, collisionDomain);
startHeight += stepHeight;
}
//
// Create a height field.
//
// Terrain that is uneven is best modeled with a height field. Height fields are faster
// than general triangle meshes.
// The height direction is the y direction.
// The height field lies in the x/z plane.
var numberOfSamplesX = 20;
var numberOfSamplesZ = 20;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
// Create arbitrary height values.
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
if (x == 0 || z == 0 || x == 19 || z == 19)
{
// Set this boundary elements to a low height, so that the height field is connected
// to the ground.
samples[z * numberOfSamplesX + x] = -1;
}
else
{
// A sine/cosine function that creates some interesting waves.
samples[z * numberOfSamplesX + x] = 1.0f + (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 1.0f);
}
}
}
var heightField = new HeightField(0, 0, 20, 20, samples, numberOfSamplesX, numberOfSamplesZ);
AddObject("Heightfield", new Pose(new Vector3(10, 0, 10)), heightField, collisionDomain);
// Create rubble on the floor (small random objects on the floor).
// Our character should be able to move over small bumps on the ground.
for (int i = 0; i < 50; i++)
{
Pose pose = new Pose(
new Vector3(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20)),
RandomHelper.Random.NextQuaternion());
BoxShape shape = new BoxShape(RandomHelper.Random.NextVector3(0.05f, 0.8f));
AddObject("Stone" + i, pose, shape, collisionDomain);
}
// Create some slopes to see how our character performs on/under sloped surfaces.
// Here we can test how the character controller behaves if the head touches an inclined
// ceiling.
AddObject("SlopeGround", new Pose(new Vector3(-2, 1.8f, -12), Quaternion.CreateRotationX(0.4f)), new BoxShape(2, 0.5f, 10), collisionDomain);
AddObject("SlopeRoof", new Pose(new Vector3(-2, 5.6f, -12), Quaternion.CreateRotationX(-0.4f)), new BoxShape(2, 0.5f, 10), collisionDomain);
// Slopes with different tilt angles.
// The character controller has a slope limit. Only flat slopes should be climbable.
for (int i = 0; i < 10; i++)
{
float stepHeight = 0.1f + i * 0.1f;
Pose pose = new Pose(
new Vector3(-10, i * 0.5f, -i * 2),
Matrix.CreateRotationX(MathHelper.ToRadians(10) + i * MathHelper.ToRadians(10)));
BoxShape shape = new BoxShape(8 * (1 - i * 0.1f), 0.5f, 30);
AddObject("Slope" + i, pose, shape, collisionDomain);
startHeight += stepHeight;
}
// Create a slope with a wall on one side.
// This objects let's us test how the character controller behaves while falling and
// sliding along a vertical wall. (Run up the slope and then jump down while moving into
// the wall.)
AddObject("LongSlope", new Pose(new Vector3(-20, 3, -10), Matrix.CreateRotationX(0.4f)), new BoxShape(4, 5f, 30), collisionDomain);
AddObject("LongSlopeWall", new Pose(new Vector3(-22, 5, -10)), new BoxShape(0.5f, 10f, 25), collisionDomain);
// Create a mesh object to test walking on triangle meshes.
// Normally, the mesh would be loaded from a file. Here, we make a composite shape and
// let DigitalRune Geometry compute a mesh for it. Then we throw away the composite
// shape and use only the mesh. - We do this to test triangle meshes. Using the composite
// shape instead of the triangle mesh would be a lot faster.
CompositeShape compositeShape = new CompositeShape();
compositeShape.Children.Add(new GeometricObject(heightField, Pose.Identity));
compositeShape.Children.Add(new GeometricObject(new CylinderShape(1, 2), new Pose(new Vector3(10, 1, 10))));
compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3(15, 0, 15))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Pose(new Vector3(15, 0, 5))));
ITriangleMesh mesh = compositeShape.GetMesh(0.01f, 3);
TriangleMeshShape meshShape = new TriangleMeshShape(mesh);
// Collision detection speed for triangle meshes can be improved by using a spatial
// partition. Here, we assign an AabbTree to the triangle mesh shape. The tree is
// built automatically when needed and it stores triangle indices (therefore the generic
// parameter of the AabbTree is int).
meshShape.Partition = new AabbTree <int>()
{
// The tree is automatically built using a mixed top-down/bottom-up approach. Bottom-up
// building is slower but produces better trees. If the tree building takes too long,
// we can lower the BottomUpBuildThreshold (default is 128).
BottomUpBuildThreshold = 0,
};
AddObject("Mesh", new Pose(new Vector3(-30, 0, 10)), meshShape, collisionDomain);
}
// Using a softness value > 0 is important to give the ragdoll a more natural movement and to
// avoid jittering.
public static void AddRagdoll(Simulation simulation, float scale, Vector3F ragdollPosition, float softness, bool addDamping)
{
// Ragdolls are usually used in games to create realistic death animations of
// characters. The character is usually rendered using a skinned triangle mesh.
// But in the physics simulation the body parts of the character are represented
// using simple shapes, such as spheres, capsules, boxes, or convex polyhedra,
// which are connected with joints.
// The physics simulations computes how these parts collide and fall. The positions
// and orientations are then read back each frame to update the animation of the
// triangle mesh.
// In this example the ragdoll is built from spheres, capsules and boxes. The
// rigid bodies are created in code. In practice, ragdolls should be built using
// external tools, such as a 3D modeler or a game editor.
#region ----- Create rigid bodies for the most relevant body parts -----
// The density used for all bodies.
const float density = 1000;
BoxShape pelvisShape = new BoxShape(0.3f * scale, 0.22f * scale, 0.20f * scale);
MassFrame pelvisMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, density, 0.01f, 3);
RigidBody pelvis = new RigidBody(pelvisShape, pelvisMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.03f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(pelvis);
BoxShape torsoShape = new BoxShape(0.35f * scale, 0.22f * scale, 0.44f * scale);
MassFrame torsoMass = MassFrame.FromShapeAndDensity(torsoShape, Vector3F.One, density, 0.01f, 3);
RigidBody torso = new RigidBody(torsoShape, torsoMass, null)
{
Pose = new Pose(new Vector3F(0f, 0.01f * scale, -0.4f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(torso);
SphereShape headShape = new SphereShape(0.13f * scale);
MassFrame headMass = MassFrame.FromShapeAndDensity(headShape, Vector3F.One, density, 0.01f, 3);
RigidBody head = new RigidBody(headShape, headMass, null)
{
Pose = new Pose(new Vector3F(0f * scale, 0f, -0.776f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(head);
CapsuleShape upperArmShape = new CapsuleShape(0.08f * scale, 0.3f * scale);
MassFrame upperArmMass = MassFrame.FromShapeAndDensity(upperArmShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(-0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftUpperArm);
RigidBody rightUpperArm = new RigidBody(upperArmShape, upperArmMass, null)
{
Pose = new Pose(new Vector3F(0.32f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightUpperArm);
CapsuleShape lowerArmShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerArmMass = MassFrame.FromShapeAndDensity(lowerArmShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(-0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftLowerArm);
RigidBody rightLowerArm = new RigidBody(lowerArmShape, lowerArmMass, null)
{
Pose = new Pose(new Vector3F(0.62f * scale, 0.06f * scale, -0.53f * scale) + ragdollPosition, Matrix33F.CreateRotationZ(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightLowerArm);
CapsuleShape upperLegShape = new CapsuleShape(0.09f * scale, 0.5f * scale);
MassFrame upperLegMass = MassFrame.FromShapeAndDensity(upperLegShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(-0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftUpperLeg);
RigidBody rightUpperLeg = new RigidBody(upperLegShape, upperLegMass, null)
{
Pose = new Pose(new Vector3F(0.10f * scale, 0.01f * scale, 0.233f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightUpperLeg);
CapsuleShape lowerLegShape = new CapsuleShape(0.08f * scale, 0.4f * scale);
MassFrame lowerLegMass = MassFrame.FromShapeAndDensity(pelvisShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(leftLowerLeg);
RigidBody rightLowerLeg = new RigidBody(lowerLegShape, lowerLegMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, 0.01f * scale, 0.7f * scale) + ragdollPosition, Matrix33F.CreateRotationX(ConstantsF.PiOver2)),
};
simulation.RigidBodies.Add(rightLowerLeg);
BoxShape footShape = new BoxShape(0.12f * scale, 0.28f * scale, 0.07f * scale);
MassFrame footMass = MassFrame.FromShapeAndDensity(footShape, Vector3F.One, density, 0.01f, 3);
RigidBody leftFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(-0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(leftFoot);
RigidBody rightFoot = new RigidBody(footShape, footMass, null)
{
Pose = new Pose(new Vector3F(0.11f * scale, -0.06f * scale, 0.94f * scale) + ragdollPosition),
};
simulation.RigidBodies.Add(rightFoot);
#endregion
#region ----- Add joints between body parts -----
float errorReduction = 0.3f;
float maxForce = float.PositiveInfinity;
Vector3F pelvisJointPosition = new Vector3F(0f, 0.026f * scale, -0.115f * scale) + ragdollPosition;
HingeJoint pelvisJoint = new HingeJoint
{
BodyA = torso,
BodyB = pelvis,
AnchorPoseALocal = new Pose(torso.Pose.ToLocalPosition(pelvisJointPosition)),
AnchorPoseBLocal = new Pose(pelvis.Pose.ToLocalPosition(pelvisJointPosition)),
Minimum = -0.5f,
Maximum = 1.1f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(pelvisJoint);
Vector3F neckJointPosition = new Vector3F(0f, 0.026f * scale, -0.690f * scale) + ragdollPosition;
HingeJoint neckJoint = new HingeJoint
{
BodyA = head,
BodyB = torso,
AnchorPoseALocal = new Pose(head.Pose.ToLocalPosition(neckJointPosition)),
AnchorPoseBLocal = new Pose(torso.Pose.ToLocalPosition(neckJointPosition)),
Minimum = -1f,
Maximum = 1f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(neckJoint);
Vector3F leftShoulderJointPosition = new Vector3F(-0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F leftShoulderJointAxis = new Vector3F(0, -1, -1).Normalized;
Matrix33F leftShoulderJointOrientation = new Matrix33F();
leftShoulderJointOrientation.SetColumn(0, leftShoulderJointAxis);
leftShoulderJointOrientation.SetColumn(1, leftShoulderJointAxis.Orthonormal1);
leftShoulderJointOrientation.SetColumn(2, leftShoulderJointAxis.Orthonormal2);
BallJoint leftShoulderJoint = new BallJoint
{
BodyA = leftUpperArm,
BodyB = torso,
AnchorPositionALocal = leftUpperArm.Pose.ToLocalPosition(leftShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(leftShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftShoulderJoint);
Vector3F rightShoulderJointPosition = new Vector3F(0.193f * scale, 0.056f * scale, -0.528f * scale) + ragdollPosition;
Vector3F rightShoulderJointAxis = new Vector3F(0, 1, 1).Normalized;
Matrix33F rightShoulderJointOrientation = new Matrix33F();
rightShoulderJointOrientation.SetColumn(0, rightShoulderJointAxis);
rightShoulderJointOrientation.SetColumn(1, rightShoulderJointAxis.Orthonormal1);
rightShoulderJointOrientation.SetColumn(2, rightShoulderJointAxis.Orthonormal2);
BallJoint rightShoulderJoint = new BallJoint
{
BodyA = rightUpperArm,
BodyB = torso,
AnchorPositionALocal = rightUpperArm.Pose.ToLocalPosition(rightShoulderJointPosition),
AnchorPositionBLocal = torso.Pose.ToLocalPosition(rightShoulderJointPosition),
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightShoulderJoint);
Vector3F leftElbowJointPosition = new Vector3F(-0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
Matrix33F elbowAxisOrientation = new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0);
HingeJoint leftElbowJoint = new HingeJoint
{
BodyA = leftLowerArm,
BodyB = leftUpperArm,
AnchorPoseALocal = new Pose(leftLowerArm.Pose.ToLocalPosition(leftElbowJointPosition), leftLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(leftUpperArm.Pose.ToLocalPosition(leftElbowJointPosition), leftUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = -2,
Maximum = 0,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftElbowJoint);
Vector3F rightElbowJointPosition = new Vector3F(0.451f * scale, 0.071f * scale, -0.538f * scale) + ragdollPosition;
HingeJoint rightElbowJoint = new HingeJoint
{
BodyA = rightLowerArm,
BodyB = rightUpperArm,
AnchorPoseALocal = new Pose(rightLowerArm.Pose.ToLocalPosition(rightElbowJointPosition), rightLowerArm.Pose.Orientation.Inverse * elbowAxisOrientation),
AnchorPoseBLocal = new Pose(rightUpperArm.Pose.ToLocalPosition(rightElbowJointPosition), rightUpperArm.Pose.Orientation.Inverse * elbowAxisOrientation),
Minimum = 0,
Maximum = 2,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightElbowJoint);
Vector3F leftHipJointPosition = new Vector3F(-0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint leftHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(leftHipJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftHipJointPosition), leftUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftHipJoint);
Vector3F rightHipJointPosition = new Vector3F(0.107f * scale, 0.049f * scale, 0.026f * scale) + ragdollPosition;
HingeJoint rightHipJoint = new HingeJoint
{
BodyA = pelvis,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(pelvis.Pose.ToLocalPosition(rightHipJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightHipJointPosition), rightUpperLeg.Pose.Orientation.Inverse),
Minimum = -0.1f,
Maximum = 1.2f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightHipJoint);
Vector3F leftKneeJointPosition = new Vector3F(-0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint leftKneeJoint = new HingeJoint
{
BodyA = leftLowerLeg,
BodyB = leftUpperLeg,
AnchorPoseALocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
AnchorPoseBLocal = new Pose(leftUpperLeg.Pose.ToLocalPosition(leftKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftKneeJoint);
Vector3F rightKneeJointPosition = new Vector3F(0.118f * scale, -0.012f * scale, 0.439f * scale) + ragdollPosition;
HingeJoint rightKneeJoint = new HingeJoint
{
BodyA = rightLowerLeg,
BodyB = rightUpperLeg,
AnchorPoseALocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
AnchorPoseBLocal = new Pose(rightUpperLeg.Pose.ToLocalPosition(rightKneeJointPosition)),
Minimum = 0,
Maximum = 1.7f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightKneeJoint);
Vector3F leftAnkleJointPosition = new Vector3F(-0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint leftAnkleJoint = new HingeJoint
{
BodyA = leftFoot,
BodyB = leftLowerLeg,
AnchorPoseALocal = new Pose(leftFoot.Pose.ToLocalPosition(leftAnkleJointPosition)),
AnchorPoseBLocal = new Pose(leftLowerLeg.Pose.ToLocalPosition(leftAnkleJointPosition), leftLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(leftAnkleJoint);
Vector3F rightAnkleJointPosition = new Vector3F(0.118f * scale, -0.016f * scale, 0.861f * scale) + ragdollPosition;
HingeJoint rightAnkleJoint = new HingeJoint
{
BodyA = rightFoot,
BodyB = rightLowerLeg,
AnchorPoseALocal = new Pose(rightFoot.Pose.ToLocalPosition(rightAnkleJointPosition)),
AnchorPoseBLocal = new Pose(rightLowerLeg.Pose.ToLocalPosition(rightAnkleJointPosition), rightLowerLeg.Pose.Orientation.Inverse),
Minimum = -0.4f,
Maximum = 0.9f,
CollisionEnabled = false,
ErrorReduction = errorReduction,
Softness = softness,
MaxForce = maxForce,
};
simulation.Constraints.Add(rightAnkleJoint);
#endregion
#region ----- Add damping to improve stability -----
if (addDamping)
{
// Damping removes jiggling and improves stability.
softness = 0.05f;
maxForce = float.PositiveInfinity;
AddDamping(simulation, pelvis, torso, softness, maxForce);
AddDamping(simulation, torso, head, softness, maxForce);
AddDamping(simulation, torso, leftUpperArm, softness, maxForce);
AddDamping(simulation, leftUpperArm, leftLowerArm, softness, maxForce);
AddDamping(simulation, torso, rightUpperArm, softness, maxForce);
AddDamping(simulation, rightUpperArm, rightLowerArm, softness, maxForce);
AddDamping(simulation, pelvis, leftUpperLeg, softness, maxForce);
AddDamping(simulation, pelvis, rightUpperLeg, softness, maxForce);
AddDamping(simulation, leftUpperLeg, leftLowerLeg, softness, maxForce);
AddDamping(simulation, rightUpperLeg, rightLowerLeg, softness, maxForce);
AddDamping(simulation, leftLowerLeg, leftFoot, softness, maxForce);
AddDamping(simulation, rightLowerLeg, rightFoot, softness, maxForce);
}
#endregion
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="localPose"></param>
public SonarEntity(Pose localPose)
{
// create a new instance of the sonar pose so we dont re-use the raycast reference
// That reference will be updated regularly
BoxShapeProperties box = new BoxShapeProperties("Sonar", 0.5f,
localPose,
new Vector3(0.1f, 0.1f, 0.1f));
_sonarBox = new BoxShape(box);
State.Assets.Effect = "LaserRangeFinder.fx";
}
public AdvancedAvatarRagdollSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// This sample uses for a DebugRenderer to draw text and rigid bodies.
_debugRenderer = new DebugRenderer(GraphicsService, SpriteFont)
{
DefaultColor = Color.Black,
DefaultTextPosition = new Vector2F(10),
};
// Add a custom game object which controls the camera.
_cameraObject = new CameraObject(Services);
_cameraObject.ResetPose(new Vector3F(0, 1, -3), ConstantsF.Pi, 0);
GameObjectService.Objects.Add(_cameraObject);
// Add some objects which allow the user to interact with the rigid bodies.
_grabObject = new GrabObject(Services);
_ballShooterObject = new BallShooterObject(Services)
{
Speed = 20
};
GameObjectService.Objects.Add(_grabObject);
GameObjectService.Objects.Add(_ballShooterObject);
// Add some default force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Create a random avatar.
_avatarDescription = AvatarDescription.CreateRandom();
_avatarRenderer = new AvatarRenderer(_avatarDescription);
// Use the "Wave" animation preset.
var avatarAnimation = new AvatarAnimation(AvatarAnimationPreset.Wave);
_expressionAnimation = new AnimationClip <AvatarExpression>(new WrappedAvatarExpressionAnimation(avatarAnimation))
{
LoopBehavior = LoopBehavior.Cycle,
Duration = TimeSpan.MaxValue,
};
_skeletonAnimation = new AnimationClip <SkeletonPose>(new WrappedAvatarSkeletonAnimation(avatarAnimation))
{
LoopBehavior = LoopBehavior.Cycle,
Duration = TimeSpan.MaxValue,
};
// Add a ground plane in the simulation.
Simulation.RigidBodies.Add(new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
MotionType = MotionType.Static
});
// Distribute a few dynamic spheres and boxes across the landscape.
SphereShape sphereShape = new SphereShape(0.3f);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-10, 10);
position.Y = 1;
Simulation.RigidBodies.Add(new RigidBody(sphereShape)
{
Pose = new Pose(position)
});
}
BoxShape boxShape = new BoxShape(0.6f, 0.6f, 0.6f);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-10, 10);
position.Y = 1;
Simulation.RigidBodies.Add(new RigidBody(boxShape)
{
Pose = new Pose(position)
});
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
// create a few dynamic rigidbodies
const float mass = 1.0f;
BoxShape colShape = new BoxShape(1);
CollisionShapes.Add(colShape);
Vector3 localInertia = colShape.CalculateLocalInertia(mass);
const float start_x = StartPosX - ArraySizeX / 2;
const float start_y = StartPosY;
const float start_z = StartPosZ - ArraySizeZ / 2;
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.Translation(
3 * i + start_x,
3 * k + start_y,
3 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
rbInfo.Dispose();
World.AddRigidBody(body);
}
}
}
var settings = new MultiBodySettings()
{
BasePosition = new Vector3(60, 29.5f, -2) * Scaling,
CanSleep = true,
CreateConstraints = true,
DisableParentCollision = true, // the self-collision has conflicting/non-resolvable contact normals
IsFixedBase = false,
NumLinks = 2,
UsePrismatic = true
};
var multiBodyA = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.NumLinks = 10;
settings.BasePosition = new Vector3(0, 29.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = false;
var multiBodyB = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20 * Scaling, 29.5f * Scaling, -settings.NumLinks * 4 * Scaling);
settings.IsFixedBase = false;
var multiBodyC = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
settings.BasePosition = new Vector3(-20, 9.5f, -settings.NumLinks * 4);
settings.IsFixedBase = true;
settings.UsePrismatic = true;
settings.DisableParentCollision = true;
var multiBodyPrim = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, settings);
}
public IBoxShapeImp AddBoxShape(float3 boxHalfExtents)
{
var btBoxShape = new BoxShape(Translater.Float3ToBtVector3(boxHalfExtents));
BtCollisionShapes.Add(btBoxShape);
var retval = new BoxShapeImp();
retval.BtBoxShape = btBoxShape;
btBoxShape.UserObject = retval;
return retval;
}
public Physics()
{
// collision configuration contains default setup for memory, collision setup
DefaultCollisionConstructionInfo cci = new DefaultCollisionConstructionInfo();
cci.DefaultMaxPersistentManifoldPoolSize = 32768;
CollisionConf = new DefaultCollisionConfiguration(cci);
if (UseParallelDispatcherBenchmark)
{
int maxNumOutstandingTasks = 4;
Win32ThreadConstructionInfo info = new Win32ThreadConstructionInfo("collision",
Win32ThreadFunc.ProcessCollisionTask, Win32LSMemorySetupFunc.CreateCollisionLocalStoreMemory,
maxNumOutstandingTasks);
Win32ThreadSupport threadSupportCollision = new Win32ThreadSupport(info);
Dispatcher = new SpuGatheringCollisionDispatcher(threadSupportCollision, 1, CollisionConf);
}
else
{
Dispatcher = new CollisionDispatcher(CollisionConf);
Dispatcher.DispatcherFlags = DispatcherFlags.DisableContactPoolDynamicAllocation;
}
// the maximum size of the collision world. Make sure objects stay within these boundaries
// Don't make the world AABB size too large, it will harm simulation quality and performance
Vector3 worldAabbMin = new Vector3(-1000, -1000, -1000);
Vector3 worldAabbMax = new Vector3(1000, 1000, 1000);
HashedOverlappingPairCache pairCache = new HashedOverlappingPairCache();
Broadphase = new AxisSweep3(worldAabbMin, worldAabbMax, 3500, pairCache);
//Broadphase = new DbvtBroadphase();
if (UseParallelDispatcherBenchmark)
{
ThreadSupportInterface thread = CreateSolverThreadSupport(4);
Solver = new ParallelConstraintSolver(thread);
}
else
{
Solver = new SequentialImpulseConstraintSolver();
}
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
if (UseParallelDispatcherBenchmark)
{
((DiscreteDynamicsWorld)World).SimulationIslandManager.SplitIslands = false;
}
World.SolverInfo.SolverMode |= SolverModes.EnableFrictionDirectionCaching;
World.SolverInfo.NumIterations = 5;
if (benchmark < 5)
{
// create the ground
CollisionShape groundShape = new BoxShape(250, 50, 250);
CollisionShapes.Add(groundShape);
CollisionObject ground = base.LocalCreateRigidBody(0, Matrix.Translation(0, -50, 0), groundShape);
ground.UserObject = "Ground";
}
float cubeSize = 1.0f;
float spacing = cubeSize;
float mass = 1.0f;
int size = 8;
Vector3 localInertia;
Vector3 pos = new Vector3(0.0f, cubeSize * 2, 0.0f);
float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
switch (benchmark)
{
case 1:
// 3000
BoxShape blockShape = new BoxShape(cubeSize - collisionRadius);
mass = 2.0f;
blockShape.CalculateLocalInertia(mass, out localInertia);
for (int k = 0; k < 47; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
RigidBody cmbody = LocalCreateRigidBody(mass, Matrix.Translation(pos), blockShape);
}
}
offset -= 0.05f * spacing * (size - 1);
// spacing *= 1.01f;
pos[1] += (cubeSize * 2.0f + spacing);
}
break;
case 2:
CreatePyramid(new Vector3(-20, 0, 0), 12, new Vector3(cubeSize));
CreateWall(new Vector3(-2.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateWall(new Vector3(4.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateWall(new Vector3(10.0f, 0.0f, 0.0f), 12, new Vector3(cubeSize));
CreateTowerCircle(new Vector3(25.0f, 0.0f, 0.0f), 8, 24, new Vector3(cubeSize));
break;
case 3:
// TODO: Ragdolls
break;
case 4:
cubeSize = 1.5f;
ConvexHullShape convexHullShape = new ConvexHullShape();
float scaling = 1;
convexHullShape.LocalScaling = new Vector3(scaling);
for (int i = 0; i < Taru.Vtx.Length / 3; i++)
{
Vector3 vtx = new Vector3(Taru.Vtx[i * 3], Taru.Vtx[i * 3 + 1], Taru.Vtx[i * 3 + 2]);
convexHullShape.AddPoint(vtx * (1.0f / scaling));
}
//this will enable polyhedral contact clipping, better quality, slightly slower
convexHullShape.InitializePolyhedralFeatures();
convexHullShape.CalculateLocalInertia(mass, out localInertia);
for (int k = 0; k < 15; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
LocalCreateRigidBody(mass, Matrix.Translation(pos), convexHullShape);
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.01f;
pos[1] += (cubeSize * 2.0f + spacing);
}
break;
case 5:
Vector3 boxSize = new Vector3(1.5f);
float boxMass = 1.0f;
float sphereRadius = 1.5f;
float sphereMass = 1.0f;
float capsuleHalf = 2.0f;
float capsuleRadius = 1.0f;
float capsuleMass = 1.0f;
size = 10;
int height = 10;
cubeSize = boxSize[0];
spacing = 2.0f;
pos = new Vector3(0.0f, 20.0f, 0.0f);
offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
int numBodies = 0;
Random random = new Random();
for (int k = 0; k < height; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
Vector3 bpos = new Vector3(0, 25, 0) + new Vector3(5.0f * pos.X, pos.Y, 5.0f * pos.Z);
int idx = random.Next(10);
Matrix trans = Matrix.Translation(bpos);
switch (idx)
{
case 0:
case 1:
case 2:
{
float r = 0.5f * (idx + 1);
BoxShape boxShape = new BoxShape(boxSize * r);
LocalCreateRigidBody(boxMass * r, trans, boxShape);
}
break;
case 3:
case 4:
case 5:
{
float r = 0.5f * (idx - 3 + 1);
SphereShape sphereShape = new SphereShape(sphereRadius * r);
LocalCreateRigidBody(sphereMass * r, trans, sphereShape);
}
break;
case 6:
case 7:
case 8:
{
float r = 0.5f * (idx - 6 + 1);
CapsuleShape capsuleShape = new CapsuleShape(capsuleRadius * r, capsuleHalf * r);
LocalCreateRigidBody(capsuleMass * r, trans, capsuleShape);
}
break;
}
numBodies++;
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.1f;
pos[1] += (cubeSize * 2.0f + spacing);
}
//CreateLargeMeshBody();
break;
case 6:
boxSize = new Vector3(1.5f, 1.5f, 1.5f);
convexHullShape = new ConvexHullShape();
for (int i = 0; i < Taru.Vtx.Length / 3; i++)
{
Vector3 vtx = new Vector3(Taru.Vtx[i * 3], Taru.Vtx[i * 3 + 1], Taru.Vtx[i * 3 + 2]);
convexHullShape.AddPoint(vtx);
}
convexHullShape.CalculateLocalInertia(mass, out localInertia);
size = 10;
height = 10;
cubeSize = boxSize[0];
spacing = 2.0f;
pos = new Vector3(0.0f, 20.0f, 0.0f);
offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
for (int k = 0; k < height; k++)
{
for (int j = 0; j < size; j++)
{
pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
for (int i = 0; i < size; i++)
{
pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
Vector3 bpos = new Vector3(0, 25, 0) + new Vector3(5.0f * pos.X, pos.Y, 5.0f * pos.Z);
LocalCreateRigidBody(mass, Matrix.Translation(bpos), convexHullShape);
}
}
offset -= 0.05f * spacing * (size - 1);
spacing *= 1.1f;
pos[1] += (cubeSize * 2.0f + spacing);
}
//CreateLargeMeshBody();
break;
case 7:
// TODO
//CreateTest6();
//InitRays();
break;
}
}
public void ExtentZException()
{
BoxShape b = new BoxShape();
b.Extent = new Vector3(1, 1, -1);
}
// Creates a lot of random objects.
private void CreateRandomObjects()
{
var random = new Random();
var isFirstHeightField = true;
int currentShape = 0;
int numberOfObjects = 0;
while (true)
{
numberOfObjects++;
if (numberOfObjects > ObjectsPerType)
{
currentShape++;
numberOfObjects = 0;
}
Shape shape;
switch (currentShape)
{
case 0:
// Box
shape = new BoxShape(ObjectSize, ObjectSize * 2, ObjectSize * 3);
break;
case 1:
// Capsule
shape = new CapsuleShape(0.3f * ObjectSize, 2 * ObjectSize);
break;
case 2:
// Cone
shape = new ConeShape(1 * ObjectSize, 2 * ObjectSize);
break;
case 3:
// Cylinder
shape = new CylinderShape(0.4f * ObjectSize, 2 * ObjectSize);
break;
case 4:
// Sphere
shape = new SphereShape(ObjectSize);
break;
case 5:
// Convex hull of several points.
ConvexHullOfPoints hull = new ConvexHullOfPoints();
hull.Points.Add(new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize));
shape = hull;
break;
case 6:
// A composite shape: two boxes that form a "T" shape.
var composite = new CompositeShape();
composite.Children.Add(
new GeometricObject(
new BoxShape(ObjectSize, 3 * ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3F(0, 2 * ObjectSize, 0))));
shape = composite;
break;
case 7:
shape = new CircleShape(ObjectSize);
break;
case 8:
{
var compBvh = new CompositeShape();
compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
compBvh.Partition = new AabbTree <int>();
shape = compBvh;
break;
}
case 9:
CompositeShape comp = new CompositeShape();
comp.Children.Add(new GeometricObject(new BoxShape(0.5f * ObjectSize, 1 * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0, 0.5f * ObjectSize, 0), QuaternionF.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3F(0.3f * ObjectSize, 0.7f * ObjectSize, 0), QuaternionF.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 1.15f * ObjectSize, 0), QuaternionF.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 11:
ConvexHullOfShapes shapeHull = new ConvexHullOfShapes();
shapeHull.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3F(0, 2 * ObjectSize, 0), Matrix33F.Identity)));
shapeHull.Children.Add(new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize), Pose.Identity));
shape = shapeHull;
break;
case 12:
shape = Shape.Empty;
break;
case 13:
var numberOfSamplesX = 10;
var numberOfSamplesZ = 10;
var samples = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
samples[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 3f) * Math.Sin(x / 2f) * BoxSize / 6);
}
}
HeightField heightField = new HeightField(0, 0, 2 * BoxSize, 2 * BoxSize, samples, numberOfSamplesX, numberOfSamplesZ);
shape = heightField;
break;
//case 14:
//shape = new LineShape(new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3F(0.1f, 0.2f, 0.3f), new Vector3F(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3F(0.1f, 0.2f, -0.3f));
break;
case 16:
shape = new MinkowskiDifferenceShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize))
};
break;
case 17:
shape = new MinkowskiSumShape
{
ObjectA = new GeometricObject(new SphereShape(0.1f * ObjectSize)),
ObjectB = new GeometricObject(new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize)),
};
break;
case 18:
shape = new OrthographicViewVolume(0, ObjectSize, 0, ObjectSize, ObjectSize / 2, ObjectSize * 2);
break;
case 19:
shape = new PerspectiveViewVolume(MathHelper.ToRadians(60f), 16f / 10, ObjectSize / 2, ObjectSize * 3);
break;
case 20:
shape = new PointShape(0.1f, 0.3f, 0.2f);
break;
case 21:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3F(0.2f, 0, -0.12f), new Vector3F(1, 2, 3).Normalized, ObjectSize * 2)
{
StopsAtFirstHit = true
};
break;
case 23:
shape = new RectangleShape(ObjectSize, ObjectSize * 2);
break;
case 24:
shape = new TransformedShape(
new GeometricObject(
new BoxShape(1 * ObjectSize, 2 * ObjectSize, 3 * ObjectSize),
new Pose(new Vector3F(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3F(ObjectSize, 0, 0), new Vector3F(0, ObjectSize, 0), new Vector3F(ObjectSize, ObjectSize, ObjectSize));
break;
//case 26:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), Matrix33F.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), Matrix33F.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), Matrix33F.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
//case 27:
// {
// // Create a composite object from which we get the mesh.
// CompositeShape compBvh = new CompositeShape();
// compBvh.Children.Add(new GeometricObject(new BoxShape(0.5f, 1, 0.5f), new Pose(new Vector3F(0, 0.5f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3F(0.5f, 0.7f, 0), QuaternionF.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3F(0, 1.15f, 0), QuaternionF.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3F(0.6f, 1.15f, 0), QuaternionF.CreateRotationX(0.3f))));
// TriangleMeshShape meshBvhShape = new TriangleMeshShape { Mesh = compBvh.GetMesh(0.01f, 3) };
// meshBvhShape.Partition = new AabbTree<int>();
// shape = meshBvhShape;
// break;
// }
case 28:
shape = new ConvexPolyhedron(new[]
{
new Vector3F(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3F(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3F(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3F(-1 * ObjectSize, 0.7f * ObjectSize, -0.6f * ObjectSize)
});
break;
case 29:
return;
default:
currentShape++;
continue;
}
// Create an object with the random shape, pose, color and velocity.
Pose randomPose = new Pose(
random.NextVector3F(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternionF());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternionF().Rotate(new Vector3F(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternionF().Rotate(Vector3F.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
{
newObject.LinearVelocity = Vector3F.Zero;
}
if (RandomHelper.Random.NextBool())
{
newObject.AngularVelocity = Vector3F.Zero;
}
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3F.Zero;
newObject.AngularVelocity = Vector3F.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3F(-BoxSize, -BoxSize, -BoxSize));
}
else
{
currentShape++;
numberOfObjects = 0;
continue;
}
}
// Add collision object to collision domain.
_domain.CollisionObjects.Add(new CollisionObject(newObject));
//co.Type = CollisionObjectType.Trigger;
//co.Name = "Object" + shape.GetType().Name + "_" + i;
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create a few basic rigid bodies
BoxShape groundShape = new BoxShape(50, 50, 50);
//groundShape.InitializePolyhedralFeatures();
//CollisionShape groundShape = new StaticPlaneShape(new Vector3(0,1,0), 50);
CollisionShapes.Add(groundShape);
CollisionObject ground = LocalCreateRigidBody(0, Matrix.Translation(0, -51.55f, 0), groundShape);
ground.UserObject = "Ground";
int numLinks = 5;
bool spherical = true;
bool floatingBase = false;
Vector3 basePosition = new Vector3(-0.4f, 3.0f, 0.0f);
Vector3 baseHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
Vector3 linkHalfExtents = new Vector3(0.05f, 0.37f, 0.1f);
var mb = CreateFeatherstoneMultiBody(World as MultiBodyDynamicsWorld, numLinks, basePosition, baseHalfExtents, linkHalfExtents, spherical, floatingBase);
floatingBase = !floatingBase;
mb.CanSleep = true;
mb.HasSelfCollision = false;
mb.UseGyroTerm = true;
bool damping = true;
if (damping)
{
mb.LinearDamping = 0.1f;
mb.AngularDamping = 0.9f;
}
else
{
mb.LinearDamping = 0;
mb.AngularDamping = 0;
}
if (numLinks > 0)
{
float q0 = 45.0f * (float)Math.PI / 180.0f;
if (spherical)
{
Quaternion quat0 = Quaternion.RotationAxis(new Vector3(1, 1, 0).Normalized, q0);
quat0.Normalize();
mb.SetJointPosMultiDof(0, new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W });
}
else
{
mb.SetJointPosMultiDof(0, new float[] { q0 });
}
}
AddColliders(mb, baseHalfExtents, linkHalfExtents);
LocalCreateRigidBody(1, Matrix.Translation(0, -0.95f, 0), new BoxShape(0.5f, 0.5f, 0.5f));
}
public void ExtentXException()
{
BoxShape b = new BoxShape();
b.Extent = new Vector3F(-1, 1, 1);
}
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();
}
public override void InitPhysics()
{
int i;
shootBoxInitialSpeed = 4000;
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Dispatcher.RegisterCollisionCreateFunc(BroadphaseNativeType.BoxShape, BroadphaseNativeType.BoxShape,
CollisionConf.GetCollisionAlgorithmCreateFunc(BroadphaseNativeType.ConvexShape, BroadphaseNativeType.ConvexShape));
Broadphase = new DbvtBroadphase();
// the default constraint solver.
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.SolverInfo.SplitImpulse = 1;
World.SolverInfo.NumIterations = 20;
World.DispatchInfo.UseContinuous = ccdMode;
World.Gravity = new Vector3(0, -10, 0);
BoxShape ground = new BoxShape(200, 1, 200);
// ground.InitializePolyhedralFeatures();
CollisionShapes.Add(ground);
RigidBody body = LocalCreateRigidBody(0, Matrix.Identity, ground);
body.UserObject = "Ground";
CollisionShape shape = new CylinderShape(CubeHalfExtents, CubeHalfExtents, CubeHalfExtents);
CollisionShapes.Add(shape);
int numObjects = 120;
for (i = 0; i < numObjects; i++)
{
//stack them
int colsize = 10;
int row = (int)((i * CubeHalfExtents * 2) / (colsize * 2 * CubeHalfExtents));
int row2 = row;
int col = (i) % (colsize) - colsize / 2;
if (col > 3)
{
col = 11;
row2 |= 1;
}
Matrix trans = Matrix.Translation(col * 2 * CubeHalfExtents + (row2 % 2) * CubeHalfExtents,
row * 2 * CubeHalfExtents + CubeHalfExtents + ExtraHeight, 0);
body = LocalCreateRigidBody(1, trans, shape);
if (ccdMode)
{
body.CcdMotionThreshold = CubeHalfExtents;
body.CcdSweptSphereRadius = 0.9f * CubeHalfExtents;
}
}
}
/*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);
*
* }
* }
* }
* }*/
/// <summary>
/// Adds the rigid body.
/// </summary>
/// <param name="mass">The mass.</param>
/// <param name="worldTransform">The world transform.</param>
/// <param name="orientation">The orientation.</param>
/// <param name="colShape">The col shape.</param>
/// <returns></returns>
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 = Translator.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 = Translator.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 = Translator.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] = Translator.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 = Translator.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);
}
public void Volume()
{
var box = new BoxShape(2, 3, 7);
Assert.AreEqual(2 * 3 * 7, box.GetVolume(0.1f, 1));
}
public static UnityEngine.Vector3[] CreateBox(BoxShape shape)
{
return(null);
}
public void WidthYException()
{
BoxShape b = new BoxShape();
b.WidthY = -1;
}
public void WidthZException()
{
BoxShape b = new BoxShape();
b.WidthZ = -1;
}
public BaseDisposable SharedComponentCreate(string id, int classId)
{
BaseDisposable disposableComponent;
if (disposableComponents.TryGetValue(id, out disposableComponent))
{
return(disposableComponent);
}
BaseDisposable newComponent = null;
switch ((CLASS_ID)classId)
{
case CLASS_ID.BOX_SHAPE:
{
newComponent = new BoxShape(this);
break;
}
case CLASS_ID.SPHERE_SHAPE:
{
newComponent = new SphereShape(this);
break;
}
case CLASS_ID.CONE_SHAPE:
{
newComponent = new ConeShape(this);
break;
}
case CLASS_ID.CYLINDER_SHAPE:
{
newComponent = new CylinderShape(this);
break;
}
case CLASS_ID.PLANE_SHAPE:
{
newComponent = new PlaneShape(this);
break;
}
case CLASS_ID.GLTF_SHAPE:
{
newComponent = new GLTFShape(this);
break;
}
case CLASS_ID.NFT_SHAPE:
{
newComponent = new NFTShape(this);
break;
}
case CLASS_ID.OBJ_SHAPE:
{
newComponent = new OBJShape(this);
break;
}
case CLASS_ID.BASIC_MATERIAL:
{
newComponent = new BasicMaterial(this);
break;
}
case CLASS_ID.PBR_MATERIAL:
{
newComponent = new PBRMaterial(this);
break;
}
case CLASS_ID.AUDIO_CLIP:
{
newComponent = new DCLAudioClip(this);
break;
}
case CLASS_ID.TEXTURE:
{
newComponent = new DCLTexture(this);
break;
}
case CLASS_ID.UI_INPUT_TEXT_SHAPE:
{
newComponent = new UIInputText(this);
break;
}
case CLASS_ID.UI_FULLSCREEN_SHAPE:
case CLASS_ID.UI_SCREEN_SPACE_SHAPE:
{
if (GetSharedComponent <UIScreenSpace>() == null)
{
newComponent = new UIScreenSpace(this);
}
break;
}
case CLASS_ID.UI_CONTAINER_RECT:
{
newComponent = new UIContainerRect(this);
break;
}
case CLASS_ID.UI_SLIDER_SHAPE:
{
newComponent = new UIScrollRect(this);
break;
}
case CLASS_ID.UI_CONTAINER_STACK:
{
newComponent = new UIContainerStack(this);
break;
}
case CLASS_ID.UI_IMAGE_SHAPE:
{
newComponent = new UIImage(this);
break;
}
case CLASS_ID.UI_TEXT_SHAPE:
{
newComponent = new UIText(this);
break;
}
case CLASS_ID.VIDEO_CLIP:
{
newComponent = new DCLVideoClip(this);
break;
}
case CLASS_ID.VIDEO_TEXTURE:
{
newComponent = new DCLVideoTexture(this);
break;
}
case CLASS_ID.FONT:
{
newComponent = new DCLFont(this);
break;
}
case CLASS_ID.NAME:
{
newComponent = new DCLName(this);
break;
}
case CLASS_ID.LOCKED_ON_EDIT:
{
newComponent = new DCLLockedOnEdit(this);
break;
}
case CLASS_ID.VISIBLE_ON_EDIT:
{
newComponent = new DCLVisibleOnEdit(this);
break;
}
default:
Debug.LogError($"Unknown classId");
break;
}
if (newComponent != null)
{
newComponent.id = id;
disposableComponents.Add(id, newComponent);
OnAddSharedComponent?.Invoke(id, newComponent);
}
return(newComponent);
}
private BumperArrayEntity CreateBumperArray()
{
// Create a bumper array entity with two bumpers
// TT -- I changed the thickness of the bumpers to try to
// reduce the problem where both front and back seem
// to trigger when it "climbs" a wall. Moving the
// bumper up higher on the robot did not work because
// it simply got stuck on the wall!
// TT Jul-2007 - There is some problem with the bumpers
// not being recognised during collisions. Put them back
// the way they were before.
BoxShape frontBumper = new BoxShape(
new BoxShapeProperties("front",
0.001f,
new Pose(new Vector3(0, 0.05f, -0.25f)),
new Vector3(0.40f, 0.03f, 0.03f)
//new Vector3(0.40f, 0.01f, 0.03f)
)
);
// TT - From Oct CTP
frontBumper.State.DiffuseColor = new Vector4(0.1f, 0.1f, 0.1f, 1.0f);
BoxShape rearBumper = new BoxShape(
new BoxShapeProperties("rear",
0.001f,
new Pose(new Vector3(0, 0.05f, 0.25f)),
new Vector3(0.40f, 0.03f, 0.03f)
//new Vector3(0.40f, 0.01f, 0.03f)
)
);
// TT - From Oct CTP
rearBumper.State.DiffuseColor = new Vector4(0.1f, 0.1f, 0.1f, 1.0f);
// The physics engine will issue contact notifications only
// if we enable them per shape
frontBumper.State.EnableContactNotifications = true;
rearBumper.State.EnableContactNotifications = true;
BumperArrayEntity
bumperArray = new BumperArrayEntity(frontBumper, rearBumper);
// entity name, must match manifest partner name
// TT Dec-2006 - Remove slash for V1.0
bumperArray.State.Name = "P3DXBumpers";
// start simulated bumper service
// TT Dec-2006 - Subtle change for V1.0
//CreateService(
// bumper.Contract.Identifier,
// Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
// "http://localhost" + bumperArray.State.Name));
bumper.Contract.CreateService(
ConstructorPort,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + bumperArray.State.Name));
return bumperArray;
}
public BoxShapeEventArgs(BoxShape box)
{
this.box = box;
}
public BoxShape value; //种类
#endregion Fields
#region Constructors
public Square(Position p, BoxShape v)
{
this.pos = p;
this.value = v;
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
//Broadphase = new SimpleBroadphase();
Broadphase = new AxisSweep3_32Bit(new Vector3(-10000, -10000, -10000), new Vector3(10000, 10000, 10000), 1024);
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.Gravity = new Vector3(0, -10, 0);
// create trimesh model and shape
InitGImpactCollision();
// Create Scene
float mass = 0.0f;
CollisionShape staticboxShape1 = new BoxShape(200, 1, 200);//floor
CollisionShapes.Add(staticboxShape1);
LocalCreateRigidBody(mass, Matrix.Translation(0, -10, 0), staticboxShape1);
CollisionShape staticboxShape2 = new BoxShape(1, 50, 200);//left wall
CollisionShapes.Add(staticboxShape2);
LocalCreateRigidBody(mass, Matrix.Translation(-200, 15, 0), staticboxShape2);
CollisionShape staticboxShape3 = new BoxShape(1, 50, 200);//right wall
CollisionShapes.Add(staticboxShape3);
LocalCreateRigidBody(mass, Matrix.Translation(200, 15, 0), staticboxShape3);
CollisionShape staticboxShape4 = new BoxShape(200, 50, 1);//front wall
CollisionShapes.Add(staticboxShape4);
LocalCreateRigidBody(mass, Matrix.Translation(0, 15, 200), staticboxShape4);
CollisionShape staticboxShape5 = new BoxShape(200, 50, 1);//back wall
CollisionShapes.Add(staticboxShape5);
LocalCreateRigidBody(mass, Matrix.Translation(0, 15, -200), staticboxShape5);
//static plane
Vector3 normal = new Vector3(-0.5f, 0.5f, 0.0f);
normal.Normalize();
CollisionShape staticplaneShape6 = new StaticPlaneShape(normal, 0.5f);// A plane
CollisionShapes.Add(staticplaneShape6);
RigidBody staticBody2 = LocalCreateRigidBody(mass, Matrix.Translation(0, -9, 0), staticplaneShape6);
//another static plane
normal = new Vector3(0.5f, 0.7f, 0.0f);
//normal.Normalize();
CollisionShape staticplaneShape7 = new StaticPlaneShape(normal, 0.0f);// A plane
CollisionShapes.Add(staticplaneShape7);
staticBody2 = LocalCreateRigidBody(mass, Matrix.Translation(0, -10, 0), staticplaneShape7);
// Create Static Torus
float height = 28;
const float step = 2.5f;
const float massT = 1.0f;
Matrix startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll((float)Math.PI * 0.5f, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
#if BULLET_GIMPACT
kinematicTorus = LocalCreateRigidBody(0, startTransform, trimeshShape);
#else
//kinematicTorus = LocalCreateRigidBody(0, startTransform, CreateTorusShape());
#endif
//kinematicTorus.CollisionFlags = kinematicTorus.CollisionFlags | CollisionFlags.StaticObject;
//kinematicTorus.ActivationState = ActivationState.IslandSleeping;
kinematicTorus.CollisionFlags = kinematicTorus.CollisionFlags | CollisionFlags.KinematicObject;
kinematicTorus.ActivationState = ActivationState.DisableDeactivation;
// Kinematic
kinTorusTran = new Vector3(-0.1f, 0, 0);
kinTorusRot = Quaternion.RotationYawPitchRoll(0, (float)Math.PI * 0.01f, 0);
#if TEST_GIMPACT_TORUS
#if BULLET_GIMPACT
// Create dynamic Torus
for (int i = 0; i < 6; i++)
{
height -= step;
startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll(0, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
RigidBody bodyA = LocalCreateRigidBody(massT, startTransform, trimeshShape);
height -= step;
startTransform =
Matrix.RotationQuaternion(Quaternion.RotationYawPitchRoll((float)Math.PI * 0.5f, 0, (float)Math.PI * 0.5f)) *
Matrix.Translation(0, height, -5);
RigidBody bodyB = LocalCreateRigidBody(massT, startTransform, trimeshShape);
}
#else
/*
* // Create dynamic Torus
* for (int i = 0; i < 6; i++)
* {
* height -= step;
* startTransform.setOrigin(btVector3(0, height, -5));
* startTransform.setRotation(btQuaternion(0, 0, 3.14159265 * 0.5));
*
* btRigidBody* bodyA = localCreateRigidBody(massT, startTransform, createTorusShape());
*
* height -= step;
* startTransform.setOrigin(btVector3(0, height, -5));
* startTransform.setRotation(btQuaternion(3.14159265 * 0.5, 0, 3.14159265 * 0.5));
* btRigidBody* bodyB = localCreateRigidBody(massT, startTransform, createTorusShape());
* }
*/
#endif
#endif
// Create Dynamic Boxes
for (int i = 0; i < 8; i++)
{
CollisionShape boxShape = new BoxShape(new Vector3(1, 1, 1));
CollisionShapes.Add(boxShape);
LocalCreateRigidBody(1, Matrix.Translation(2 * i - 5, 2, -3), boxShape);
}
}
private void CreateShapesGravity()
{
const float cubeHalfExtent = 1.5f;
const float cubeWidth = cubeHalfExtent * 2;
var boxSize = new Vector3(cubeHalfExtent);
float boxMass = 1.0f;
float sphereRadius = 1.5f;
float sphereMass = 1.0f;
float capsuleHalf = 2.0f;
float capsuleRadius = 1.0f;
float capsuleMass = 1.0f;
int stackSize = 10;
int stackHeight = 10;
float spacing = 2.0f;
var position = new Vector3(0.0f, 20.0f, 0.0f);
float offset = -stackSize * (cubeWidth + spacing) * 0.5f;
int numBodies = 0;
var random = new Random();
for (int k = 0; k < stackHeight; k++)
{
for (int j = 0; j < stackSize; j++)
{
position.Z = offset + j * (cubeWidth + spacing);
for (int i = 0; i < stackSize; i++)
{
position.X = offset + i * (cubeWidth + spacing);
Vector3 bpos = new Vector3(0, 25, 0) + new Vector3(5.0f * position.X, position.Y, 5.0f * position.Z);
int idx = random.Next(10);
Matrix trans = Matrix.Translation(bpos);
switch (idx)
{
case 0:
case 1:
case 2:
{
float r = 0.5f * (idx + 1);
var boxShape = new BoxShape(boxSize * r);
LocalCreateRigidBody(boxMass * r, trans, boxShape);
}
break;
case 3:
case 4:
case 5:
{
float r = 0.5f * (idx - 3 + 1);
var sphereShape = new SphereShape(sphereRadius * r);
LocalCreateRigidBody(sphereMass * r, trans, sphereShape);
}
break;
case 6:
case 7:
case 8:
{
float r = 0.5f * (idx - 6 + 1);
var capsuleShape = new CapsuleShape(capsuleRadius * r, capsuleHalf * r);
LocalCreateRigidBody(capsuleMass * r, trans, capsuleShape);
}
break;
}
numBodies++;
}
}
offset -= 0.05f * spacing * (stackSize - 1);
spacing *= 1.1f;
position.Y += cubeWidth + spacing;
}
//CreateLargeMeshBody();
}
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="initialPos"></param>
public MotorBase(Vector3 initialPos, int size)
{
MASS = size * 9;
CHASSIS_DIMENSIONS = new Vector3(size * 0.393f, size *
0.18f, size * 0.40f);
CHASSIS_CLEARANCE = size * 0.05f;
FRONT_WHEEL_RADIUS = size * 0.08f;
CASTER_WHEEL_RADIUS = size * 0.025f; // =
CHASSIS_CLEARANCE / 2; // to keep things simple we make caster a bit
bigger
FRONT_WHEEL_WIDTH = size * 4.74f; //not used
CASTER_WHEEL_WIDTH = size * 0.02f; //not used
FRONT_AXLE_DEPTH_OFFSET = size * -0.05f; // distance of
the axle from the center of robot
base.State.Name = "MotorBaseWithThreeWheels";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y =
0, or ground level)
// chassis position
BoxShapeProperties motorBaseDesc = new
BoxShapeProperties("chassis", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use
zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, //
chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // no offset in the z/depth axis, since again,
its center is the robot center
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high
friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS); //
all the way at the back of the robot
// NOTE: right/left is from the perspective of the robot,
looking forward
FRONT_WHEEL_MASS = size * 0.10f;
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
CHASSIS_DIMENSIONS.X / 2 + 0.01f - 0.05f,// left of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on
the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - 0.01f + 0.05f,// right of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on
the z-axis
MotorTorqueScaling = 20;
ConstructCasterWheelShape();
ConstructWheels();
}
protected override void OnInitializePhysics()
{
SetupEmptyDynamicsWorld();
CollisionShape groundShape = new BoxShape(50, 1, 50);
//CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
CollisionShapes.Add(groundShape);
RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);
body.UserObject = "Ground";
CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));
CollisionShapes.Add(shape);
const float THETA = (float)Math.PI / 4.0f;
float L_1 = 2 - (float)Math.Tan(THETA);
float L_2 = 1 / (float)Math.Cos(THETA);
float RATIO = L_2 / L_1;
RigidBody bodyA;
RigidBody bodyB;
CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
CompoundShape cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
float mass = 6.28f;
Vector3 localInertia;
cyl0.CalculateLocalInertia(mass, out localInertia);
RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);
body = new RigidBody(ci); //1,0,cyl0,localInertia);
World.AddRigidBody(body);
body.LinearFactor = Vector3.Zero;
body.AngularFactor = new Vector3(0, 1, 0);
bodyA = body;
cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
cylB = new CylinderShape(L_2, 0.025f, L_2);
cyl0 = new CompoundShape();
cyl0.AddChildShape(Matrix.Identity, cylA);
cyl0.AddChildShape(Matrix.Identity, cylB);
mass = 6.28f;
cyl0.CalculateLocalInertia(mass, out localInertia);
ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);
body = new RigidBody(ci);//1,0,cyl0,localInertia);
body.LinearFactor = Vector3.Zero;
HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);
World.AddConstraint(hinge);
bodyB = body;
body.AngularVelocity = new Vector3(0, 3, 0);
World.AddRigidBody(body);
Vector3 axisA = new Vector3(0, 1, 0);
Vector3 axisB = new Vector3(0, 1, 0);
orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
Matrix mat = Matrix.RotationQuaternion(orn);
axisB = new Vector3(mat.M21, mat.M22, mat.M23);
GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);
World.AddConstraint(gear, true);
mass = 1.0f;
RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);
//RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
//RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
//body1.ActivationState = ActivationState.DisableDeactivation;
//body1.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
/*
* Vector3 pivotInB;
* if (body1 != null)
* {
* Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
* pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
* }
* else
* {
* pivotInB = pivotInA;
* }
*/
/*
* Vector3 axisInB;
* if (body1 != null)
* {
* Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
* axisInB = Vector3.TransformCoordinate(axisInA, transform);
* }
* else
* {
* axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
* }
*/
#if P2P
{
TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
//TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
World.AddConstraint(p2p);
p2p.DebugDrawSize = 5;
}
#else
{
hinge = new HingeConstraint(body0, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.f;
//float maxMotorImpulse = 0.01;
const float targetVelocity = 1.0f;
const float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
World.AddConstraint(hinge);
hinge.DebugDrawSize = 5;
}
#endif
RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbA1.ActivationState = ActivationState.DisableDeactivation;
// add dynamic rigid body B1
RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
//RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
pRbB1.ActivationState = ActivationState.DisableDeactivation;
// create slider constraint between A1 and B1 and add it to world
SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
//spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
spSlider1.LowerLinearLimit = -15.0f;
spSlider1.UpperLinearLimit = -5.0f;
spSlider1.LowerLinearLimit = 5.0f;
spSlider1.UpperLinearLimit = 15.0f;
spSlider1.LowerLinearLimit = -10.0f;
spSlider1.UpperLinearLimit = -10.0f;
spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;
World.AddConstraint(spSlider1, true);
spSlider1.DebugDrawSize = 5.0f;
//create a slider, using the generic D6 constraint
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = Vector3.UnitX;
const float angle = 0; //SIMD_RADS_PER_DEG * 10.f;
Matrix trans = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
d6body0 = LocalCreateRigidBody(mass, trans, shape);
d6body0.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
World.AddRigidBody(fixedBody1);
Matrix frameInA = Matrix.Translation(0, 5, 0);
Matrix frameInB = Matrix.Translation(0, 5, 0);
//bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
{
LinearLowerLimit = lowerSliderLimit,
LinearUpperLimit = hiSliderLimit,
//range should be small, otherwise singularities will 'explode' the constraint
//AngularLowerLimit = new Vector3(-1.5f,0,0),
//AngularUpperLimit = new Vector3(1.5f,0,0),
//AngularLowerLimit = new Vector3(0,0,0),
//AngularUpperLimit = new Vector3(0,0,0),
AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
AngularUpperLimit = new Vector3(1.5f, 0, 0)
};
//spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
spSlider6Dof.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
World.AddConstraint(spSlider6Dof);
spSlider6Dof.DebugDrawSize = 5;
// create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
CollisionShapes.Add(pDoorShape);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
pDoorBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
Vector3 btAxisA = Vector3.UnitY; // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);
//spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
// test problem values
//spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
//spDoorHinge.SetLimit(1, -1);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
//spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
//spDoorHinge.SetLimit(0, 0);
World.AddConstraint(spDoorHinge);
spDoorHinge.DebugDrawSize = 5;
/*RigidBody pDropBody =*/ LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);
// create a generic 6DOF constraint
//RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
//RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
//RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(-5, 0, 0);
frameInB = Matrix.Translation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
//Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
//pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
//pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
//pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);
//pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
//pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
//pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);
pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
//pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
//pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
//pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
//pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);
// create a ConeTwist constraint
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
//pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
//pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);
frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInA *= Matrix.Translation(0, -5, 0);
frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
frameInB *= Matrix.Translation(0, 5, 0);
coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
//coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
//coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
World.AddConstraint(coneTwist, true);
coneTwist.DebugDrawSize = 5;
// Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
pBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
btAxisA = new Vector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
//pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
World.AddConstraint(pHinge);
pHinge.DebugDrawSize = 5;
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some (arbitrary) data to build constraint frames
Vector3 parentAxis = new Vector3(1, 0, 0);
Vector3 childAxis = new Vector3(0, 0, 1);
Vector3 anchor = new Vector3(20, 2, 0);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.DebugDrawSize = 5;
World.AddConstraint(pGen6DOF, true);
pGen6DOF.DebugDrawSize = 5;
// create a generic 6DOF constraint with springs
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
frameInA = Matrix.Translation(10, 0, 0);
frameInB = Matrix.Identity;
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
{
LinearUpperLimit = new Vector3(5, 0, 0),
LinearLowerLimit = new Vector3(-5, 0, 0),
AngularLowerLimit = new Vector3(0, 0, -1.5f),
AngularUpperLimit = new Vector3(0, 0, 1.5f),
DebugDrawSize = 5
};
World.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB (child) below it :
pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
parentAxis = new Vector3(0, 1, 0);
childAxis = new Vector3(1, 0, 0);
anchor = new Vector3(-20, 0, 0);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
pHinge2.SetLowerLimit(-(float)Math.PI / 4);
pHinge2.SetUpperLimit((float)Math.PI / 4);
// add constraint to world
World.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.DebugDrawSize = 5;
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
pBodyA.ActivationState = ActivationState.DisableDeactivation;
// dynamic bodyB:
pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
pBodyB.ActivationState = ActivationState.DisableDeactivation;
// add some data to build constraint frames
axisA = new Vector3(0, 1, 0);
axisB = new Vector3(0, 1, 0);
Vector3 pivotA2 = new Vector3(-5, 0, 0);
Vector3 pivotB = new Vector3(5, 0, 0);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
// add constraint to world
World.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.DebugDrawSize = 5;
}
public DoorSliding(int x, int y, Level level)
{
if (level != null)
{
Location = new Point2(x, y);
Level = level;
Type = (DoorType)level.Map.Planes[(int)Level.Planes.Walls][y, x];
level.Cells[y, x] = Level.Cell.Default();
// Adjust neighboring walls to show the door excavation.
if (!IsVertical)
{
if ((x - 1) > -1 &&
level.Cells[y, x - 1].East != Level.Cell.NoWall)
{
level.Cells[y, x - 1].East = DoorWestEastWall;
}
if ((x + 1) < level.Map.Width &&
level.Cells[y, x + 1].West != Level.Cell.NoWall)
{
level.Cells[y, x + 1].West = DoorWestEastWall;
}
}
else
{
if ((y + 1) < level.Map.Height &&
level.Cells[y + 1, x].North != Level.Cell.NoWall)
{
level.Cells[y + 1, x].North = DoorNorthSouthWall;
}
if ((y - 1) > -1 &&
level.Cells[y - 1, x].South != Level.Cell.NoWall)
{
level.Cells[y - 1, x].South = DoorNorthSouthWall;
}
}
// Create a static body for the cell so that
// we can block the player from entering or exiting the cell
// when the door is closed or moving.
_cellShape = new CollisionShape();
BoxShape box = new BoxShape();
box.Extents = new Vector3(Level.CellSize * 0.5f, Level.CellSize * 0.5f, Level.CellSize * 0.5f);
_cellShape.Shape = box;
_cellShape.Name = "CollisionShape";
_cellBody = new StaticBody();
_cellBody.CollisionLayer = (uint)Level.CollisionLayers.Static;
_cellBody.CollisionMask = (uint)(Level.CollisionLayers.Characters);
_cellBody.AddChild(_cellShape);
AddChild(_cellBody);
// Create the actual mesh for the door and set it up for
// this particular instance.
BuildDoorMesh();
_mesh = new MeshInstance();
_mesh.Mesh = _doorMesh;
_mesh.MaterialOverride = Assets.GetTexture(_doorTexture[Type]);
if (IsVertical)
{
_mesh.Transform = Transform.Identity.Rotated(Vector3.Up, Mathf.Pi * 0.5f);
}
else
{
_mesh.Transform = Transform.Identity.Rotated(Vector3.Up, Mathf.Pi);
}
// Setup a physics body for the door itself for the purposes
// of detecting collisions with projectiles since projectiles shouldn't
// be blocked by the cell's static body that is normally only used for
// ensuring characters can't enter or exit the cell if the door is closed
// or moving.
_doorShape = new CollisionShape();
box = new BoxShape();
box.Extents = new Vector3(Level.CellSize * 0.5f, Level.CellSize * 0.5f, Mathf.Epsilon);
_doorShape.Shape = box;
_doorShape.Name = "CollisionShape";
_doorBody = new RigidBody();
_doorBody.CollisionLayer = (uint)Level.CollisionLayers.Doors;
_doorBody.CollisionMask = (uint)Level.CollisionLayers.Projectiles;
_doorBody.AddChild(_doorShape);
_doorBody.Mode = RigidBody.ModeEnum.Static;
_mesh.AddChild(_doorBody);
AddChild(_mesh);
// Add myself to the world and set my position
// along with some default state variables.
level.AddChild(this);
Transform tform = this.Transform;
tform.origin = level.MapToWorld(x, y);
this.Transform = tform;
State = DoorState.Closed;
_openCloseDuration = 0.75f;
_canClose = true;
_openSound = Assets.GetSoundClip(Assets.DigitalSoundList.DoorOpening);
_closeSound = Assets.GetSoundClip(Assets.DigitalSoundList.DoorClosing);
// Add a tween node for controlling the animation of the
// door opening and closing.
_tween = new Tween();
_tween.Connect("tween_all_completed", this, "OnTweenCompleted");
_mesh.AddChild(_tween);
// Add an audio player so we can emit a sound
// when the door opens and closes.
_audioPlayer = new AudioStreamPlayer3D();
_audioPlayer.Name = "AudioPlayer";
AddChild(_audioPlayer);
SetProcess(true);
SetPhysicsProcess(true);
}
}
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;
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 + 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 (int i = 0; i < CollisionShapes.Count; i++)
{
serializer.RegisterNameForObject(CollisionShapes[i], $"name{i}");
}
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();
}
}
private bool _cullingEnabled = true; // True to use frustum culling. False to disable frustum culling.
public FrustumCullingSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.ClearBackground = true;
GraphicsScreen.BackgroundColor = Color.CornflowerBlue;
// The top-down camera.
var orthographicProjection = new OrthographicProjection();
orthographicProjection.Set(
LevelSize * 1.1f * GraphicsService.GraphicsDevice.Viewport.AspectRatio,
LevelSize * 1.1f,
1,
10000f);
var topDownCamera = new Camera(orthographicProjection);
_topDownCameraNode = new CameraNode(topDownCamera)
{
View = Matrix.CreateLookAt(new Vector3(0, 1000, 0), new Vector3(0, 0, 0), -Vector3.UnitZ),
};
// The perspective camera moving through the scene.
var perspectiveProjection = new PerspectiveProjection();
perspectiveProjection.SetFieldOfView(
MathHelper.ToRadians(45),
GraphicsService.GraphicsDevice.Viewport.AspectRatio,
1,
500);
var sceneCamera = new Camera(perspectiveProjection);
_sceneCameraNode = new CameraNode(sceneCamera);
// Initialize collision detection.
// We use one collision domain that manages all objects.
_domain = new CollisionDomain(new CollisionDetection())
{
// We exchange the default broad phase with a DualPartition. The DualPartition
// has special support for frustum culling.
BroadPhase = new DualPartition<CollisionObject>(),
};
// Create a lot of random objects and add them to the collision domain.
RandomHelper.Random = new Random(12345);
for (int i = 0; i < NumberOfObjects; i++)
{
// A real scene consists of a lot of complex objects such as characters, vehicles,
// buildings, lights, etc. When doing frustum culling we need to test each objects against
// the viewing frustum. If it intersects with the viewing frustum, the object is visible
// from the camera's point of view. However, in practice we do not test the exact object
// against the viewing frustum. Each objects is approximated by a simpler shape. In our
// example, we assume that each object is approximated with an oriented bounding box.
// (We could also use an other shape, such as a bounding sphere.)
// Create a random box.
Shape randomShape = new BoxShape(RandomHelper.Random.NextVector3(1, 10));
// Create a random position.
Vector3 randomPosition;
randomPosition.X = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2);
randomPosition.Y = RandomHelper.Random.NextFloat(0, 2);
randomPosition.Z = RandomHelper.Random.NextFloat(-LevelSize / 2, LevelSize / 2);
// Create a random orientation.
Quaternion randomOrientation = RandomHelper.Random.NextQuaternion();
// Create object and add it to collision domain.
var geometricObject = new GeometricObject(randomShape, new Pose(randomPosition, randomOrientation));
var collisionObject = new CollisionObject(geometricObject)
{
CollisionGroup = 0,
};
_domain.CollisionObjects.Add(collisionObject);
}
// Per default, the collision domain computes collision between all objects.
// In this sample we do not need this information and disable it with a collision
// filter.
// In a real application, we would use this collision information for rendering,
// for example, to find out which lights overlap with which meshes, etc.
var filter = new CollisionFilter();
// Disable collision between objects in collision group 0.
filter.Set(0, 0, false);
_domain.CollisionDetection.CollisionFilter = filter;
// Start with the scene camera.
GraphicsScreen.CameraNode = _sceneCameraNode;
// We will collect a few statistics for debugging.
Profiler.SetFormat("NoCull", 1000, "Time in ms to submit DebugRenderer draw jobs without frustum culling.");
Profiler.SetFormat("WithCull", 1000, "Time in ms to submit DebugRenderer draw jobs with frustum culling.");
}
protected override void OnInitializePhysics()
{
CollisionShape groundShape = new BoxShape(50, 3, 50);
CollisionShapes.Add(groundShape);
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Solver = new SequentialImpulseConstraintSolver();
Vector3 worldMin = new Vector3(-10000, -10000, -10000);
Vector3 worldMax = new Vector3(10000, 10000, 10000);
Broadphase = new AxisSweep3(worldMin, worldMax);
//Broadphase = new DbvtBroadphase();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
int i;
Matrix tr;
Matrix vehicleTr;
//if (UseTrimeshGround)
//{
const float scale = 20.0f;
//create a triangle-mesh ground
int vertStride = Vector3.SizeInBytes;
int indexStride = 3 * sizeof(int);
const int NUM_VERTS_X = 20;
const int NUM_VERTS_Y = 20;
const int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;
const int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
TriangleIndexVertexArray vertexArray = new TriangleIndexVertexArray();
IndexedMesh mesh = new IndexedMesh();
mesh.Allocate(totalTriangles, totalVerts, indexStride, vertStride, PhyScalarType.Int32, PhyScalarType.Single);
using (var data = mesh.LockVerts())
{
for (i = 0; i < NUM_VERTS_X; i++)
{
for (int j = 0; j < NUM_VERTS_Y; j++)
{
float wl = .2f;
float height = 20.0f * (float)(Math.Sin(i * wl) * Math.Cos(j * wl));
data.Write((i - NUM_VERTS_X * 0.5f) * scale);
data.Write(height);
data.Write((j - NUM_VERTS_Y * 0.5f) * scale);
}
}
}
int index = 0;
IntArray idata = mesh.TriangleIndices;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = j * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = j * NUM_VERTS_X + i;
idata[index++] = (j + 1) * NUM_VERTS_X + i + 1;
idata[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
vertexArray.AddIndexedMesh(mesh);
groundShape = new BvhTriangleMeshShape(vertexArray, true);
tr = Matrix.Identity;
vehicleTr = Matrix.Translation(0, -2, 0);
/*}
* else
* {
* // Use HeightfieldTerrainShape
*
* int width = 40, length = 40;
* //int width = 128, length = 128; // Debugging is too slow for this
* float maxHeight = 10.0f;
* float heightScale = maxHeight / 256.0f;
* Vector3 scale = new Vector3(20.0f, maxHeight, 20.0f);
*
* //PhyScalarType scalarType = PhyScalarType.PhyUChar;
* //FileStream file = new FileStream(heightfieldFile, FileMode.Open, FileAccess.Read);
*
* // Use float data
* PhyScalarType scalarType = PhyScalarType.PhyFloat;
* byte[] terr = new byte[width * length * 4];
* MemoryStream file = new MemoryStream(terr);
* BinaryWriter writer = new BinaryWriter(file);
* for (i = 0; i < width; i++)
* for (int j = 0; j < length; j++)
* writer.Write((float)((maxHeight / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
* writer.Flush();
* file.Position = 0;
*
* HeightfieldTerrainShape heightterrainShape = new HeightfieldTerrainShape(width, length,
* file, heightScale, 0, maxHeight, upIndex, scalarType, false);
* heightterrainShape.SetUseDiamondSubdivision(true);
*
* groundShape = heightterrainShape;
* groundShape.LocalScaling = new Vector3(scale.X, 1, scale.Z);
*
* tr = Matrix.Translation(new Vector3(-scale.X / 2, scale.Y / 2, -scale.Z / 2));
* vehicleTr = Matrix.Translation(new Vector3(20, 3, -3));
*
*
* // Create graphics object
*
* file.Position = 0;
* BinaryReader reader = new BinaryReader(file);
*
* int totalTriangles = (width - 1) * (length - 1) * 2;
* int totalVerts = width * length;
*
* game.groundMesh = new Mesh(game.Device, totalTriangles, totalVerts,
* MeshFlags.SystemMemory | MeshFlags.Use32Bit, VertexFormat.Position | VertexFormat.Normal);
* SlimDX.DataStream data = game.groundMesh.LockVertexBuffer(LockFlags.None);
* for (i = 0; i < width; i++)
* {
* for (int j = 0; j < length; j++)
* {
* float height;
* if (scalarType == PhyScalarType.PhyFloat)
* {
* // heightScale isn't applied internally for float data
* height = reader.ReadSingle();
* }
* else if (scalarType == PhyScalarType.PhyUChar)
* {
* height = file.ReadByte() * heightScale;
* }
* else
* {
* height = 0.0f;
* }
*
* data.Write((j - length * 0.5f) * scale.X);
* data.Write(height);
* data.Write((i - width * 0.5f) * scale.Z);
*
* // Normals will be calculated later
* data.Position += 12;
* }
* }
* game.groundMesh.UnlockVertexBuffer();
* file.Close();
*
* data = game.groundMesh.LockIndexBuffer(LockFlags.None);
* for (i = 0; i < width - 1; i++)
* {
* for (int j = 0; j < length - 1; j++)
* {
* // Using diamond subdivision
* if ((j + i) % 2 == 0)
* {
* data.Write(j * width + i);
* data.Write((j + 1) * width + i + 1);
* data.Write(j * width + i + 1);
*
* data.Write(j * width + i);
* data.Write((j + 1) * width + i);
* data.Write((j + 1) * width + i + 1);
* }
* else
* {
* data.Write(j * width + i);
* data.Write((j + 1) * width + i);
* data.Write(j * width + i + 1);
*
* data.Write(j * width + i + 1);
* data.Write((j + 1) * width + i);
* data.Write((j + 1) * width + i + 1);
* }
*
* / *
* // Not using diamond subdivision
* data.Write(j * width + i);
* data.Write((j + 1) * width + i);
* data.Write(j * width + i + 1);
*
* data.Write(j * width + i + 1);
* data.Write((j + 1) * width + i);
* data.Write((j + 1) * width + i + 1);
* /
* }
* }
* game.groundMesh.UnlockIndexBuffer();
*
* game.groundMesh.ComputeNormals();
* }*/
CollisionShapes.Add(groundShape);
//create ground object
RigidBody ground = LocalCreateRigidBody(0, tr, groundShape);
ground.UserObject = "Ground";
CollisionShape chassisShape = new BoxShape(1.0f, 0.5f, 2.0f);
CollisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
CollisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
Matrix localTrans = Matrix.Translation(Vector3.UnitY);
compound.AddChildShape(localTrans, chassisShape);
RigidBody carChassis = LocalCreateRigidBody(800, Matrix.Identity, compound);
carChassis.UserObject = "Chassis";
//carChassis.SetDamping(0.2f, 0.2f);
//CylinderShapeX wheelShape = new CylinderShapeX(wheelWidth, wheelRadius, wheelRadius);
// clientResetScene();
// create vehicle
RaycastVehicle.VehicleTuning tuning = new RaycastVehicle.VehicleTuning();
DefaultVehicleRaycaster vehicleRayCaster = new DefaultVehicleRaycaster(World);
vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
//vehicle = new CustomVehicle(tuning, carChassis, vehicleRayCaster);
carChassis.ActivationState = ActivationState.DisableDeactivation;
World.AddAction(vehicle);
const float connectionHeight = 1.2f;
bool isFrontWheel = true;
// choose coordinate system
vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
Vector3 connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2 * CUBE_HALF_EXTENTS - wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
isFrontWheel = false;
connectionPointCS0 = new Vector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
connectionPointCS0 = new Vector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2 * CUBE_HALF_EXTENTS + wheelRadius);
vehicle.AddWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
for (i = 0; i < vehicle.NumWheels; i++)
{
WheelInfo wheel = vehicle.GetWheelInfo(i);
wheel.SuspensionStiffness = suspensionStiffness;
wheel.WheelsDampingRelaxation = suspensionDamping;
wheel.WheelsDampingCompression = suspensionCompression;
wheel.FrictionSlip = wheelFriction;
wheel.RollInfluence = rollInfluence;
}
vehicle.RigidBody.WorldTransform = vehicleTr;
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public MPRTestDemo(DemosGame game)
: base(game)
{
var shapeA = new BoxShape(1, 1, 1);
shapeA.CollisionMargin = 0;
var shapeB = new BoxShape(1, 1, 1);
shapeB.CollisionMargin = 0;
var transformA = new RigidTransform(new Vector3(0, 0, 0));
var transformB = new RigidTransform(new Vector3(.5f, .5f, 0));
Vector3 overlap;
bool overlapped = MPRToolbox.GetLocalOverlapPosition(shapeA, shapeB, ref transformB, out overlap);
Vector3 normal;
float depth;
Vector3 direction = new Vector3(0, -1, 0);
MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref transformB, ref direction, out depth, out normal);
ContactData contactData;
//bool overlappedOld = MPRToolboxOld.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
//Random rand = new Random(0);
//for (int i = 0; i < 10000000; i++)
//{
// transformA = new RigidTransform(new Vector3((float)rand.NextDouble() * 10 - 5, (float)rand.NextDouble() * 10 - 5, (float)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((float)rand.NextDouble() * 1000, (float)rand.NextDouble() * 1000, (float)rand.NextDouble() * 1000));
// transformB = new RigidTransform(new Vector3((float)rand.NextDouble() * 10 - 5, (float)rand.NextDouble() * 10 - 5, (float)rand.NextDouble() * 10 - 5),
// Quaternion.CreateFromYawPitchRoll((float)rand.NextDouble() * 1000, (float)rand.NextDouble() * 1000, (float)rand.NextDouble() * 1000));
// overlapped = MPRTesting.GetOverlapPosition(shapeA, shapeB, ref transformA, ref transformB, out overlap);
// overlappedOld = MPRToolbox.AreObjectsColliding(shapeA, shapeB, ref transformA, ref transformB, out contactData);
// if (overlapped && !overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlappedOld && !overlapped &&
// (!MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
// if (overlapped && overlappedOld &&
// (!MPRToolbox.IsPointInsideShape(ref overlap, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref overlap, shapeB, ref transformB) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeA, ref transformA) ||
// !MPRToolbox.IsPointInsideShape(ref contactData.Position, shapeB, ref transformB)))
// Debug.WriteLine("Break.");
//}
//Do these tests with rotationally immobile objects.
CollisionDetectionSettings.DefaultMargin = 0;
groundWidth = 10;
groundHeight = .1f;
groundLength = 10;
//a = new Box(new Vector3(0, -5, 0), groundWidth, groundHeight, groundLength, 1);
//a = new TransformableEntity(new Vector3(0,0,0), new TriangleShape(new Vector3(-5, -5, -5), new Vector3(5, -5, -5), new Vector3(-5, -5, 5)), Matrix3X3.Identity);
a = new Triangle(new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), 1);
Space.Add(a);
Space.ForceUpdater.Gravity = new Vector3();
boxWidth = .25f;
boxHeight = .05f;
boxLength = 1f;
b = new TransformableEntity(new Vector3(0, 2, 0), new BoxShape(boxWidth, boxHeight, boxLength), Matrix3X3.Identity, 1);
//b = new Cone(new Vector3(0, 2, 0), .2f, .1f, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5f, 1);
//b = new Capsule(new Vector3(0, 2, 0), 1, .5f, 1);
b.LocalInertiaTensorInverse = new Matrix3X3();
CollisionRules.AddRule(b, a, CollisionRule.NoSolver);
b.ActivityInformation.IsAlwaysActive = true;
Space.Add(b);
//Space.Add(new TransformableEntity(new Vector3(0, 4, 0), new BoxShape(1, 1, 1), Matrix3X3.Identity, 1));
//Space.Add( new TransformableEntity(new Vector3(0, 6, 0), new BoxShape(1, 1, 1), Matrix3X3.Identity, 1));
//Vector3[] vertices = new Vector3[] { new Vector3(0, -5, 0), new Vector3(5, -5, 0), new Vector3(5, -5, 5), new Vector3(0, -60, 5) };
//int[] indices = new int[] { 0, 1, 2 , 0, 2, 3 };
//StaticMesh mesh = new StaticMesh(vertices, indices);
//Space.Add(mesh);
//mesh.ImproveBoundaryBehavior = true;
//mesh.Sidedness = TriangleSidedness.Counterclockwise;
//game.ModelDrawer.Add(mesh);
//mesh.CollisionRules.Personal = CollisionRule.NoSolver;
}
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this.
public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase)
{
BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[]
{
new BulletSharp.Math.Vector4(1, 0, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 0, 1),
new BulletSharp.Math.Vector4(0, 1, 1, 1),
new BulletSharp.Math.Vector4(1, 1, 0, 1),
};
int curColor = 0;
bool damping = false;
bool gyro = false;
int numLinks = 2;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f);
BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f);
//mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
float baseMass = fixedBase ? 0.0f : 10.0f;
if (baseMass != 0)
{
//CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape.CalculateLocalInertia(baseMass, out baseInertiaDiag);
shape.Dispose();
}
MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep);
pMultiBody.BaseWorldTransform = baseWorldTrans;
BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0);
// pMultiBody.setBaseVel(vel);
//init the links
BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0);
//y-axis assumed up
BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset
BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
float q0 = 1.0f * Mathf.PI / 180.0f;
BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0);
quat0.Normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
float linkMass = 1.0f;
//if (i==3 || i==2)
// linkMass= 1000;
BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f);
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//
}
else
{
shape = new SphereShape(radius);
}
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
shape.Dispose();
if (!spherical)
{
//pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
if (i == 0)
{
pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
}
else
{
parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset
currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset
parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1,
new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f),
parentComToCurrentPivot,
currentPivotToCurrentCom);
}
}
else
{
//pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
pMultiBody.FinalizeMultiDof();
world.AddMultiBody(pMultiBody);
MultiBody mbC = pMultiBody;
mbC.CanSleep = (canSleep);
mbC.HasSelfCollision = (selfCollide);
mbC.UseGyroTerm = (gyro);
//
if (!damping)
{
mbC.LinearDamping = (0.0f);
mbC.AngularDamping = (0.0f);
}
else
{
mbC.LinearDamping = (0.1f);
mbC.AngularDamping = (0.9f);
}
if (numLinks > 0)
{
q0 = 180.0f * Mathf.PI / 180.0f;
if (!spherical)
{
mbC.SetJointPosMultiDof(0, new float[] { q0 });
}
else
{
BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0);
vv.Normalize();
quat0 = new BulletSharp.Math.Quaternion(vv, q0);
quat0.Normalize();
float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W };
mbC.SetJointPosMultiDof(0, quat0fs);
}
}
///
BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion>
world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1];
BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3>
local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1];
world_to_local[0] = pMultiBody.WorldToBaseRot;
local_origin[0] = pMultiBody.BasePosition;
// double friction = 1;
{
if (true)
{
CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]);
// guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1);
col.CollisionShape = shape;
Matrix tr = new Matrix();
tr.ScaleVector = BulletSharp.Math.Vector3.One;
//if we don't set the initial pose of the btCollisionObject, the simulator will do this
//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
tr.Origin = local_origin[0];
BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f);
tr.Rotation = (orn);
col.WorldTransform = (tr);
bool isDynamic = (baseMass > 0 && !fixedBase);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2);
BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f);
//guiHelper.createCollisionObjectGraphicsObject(col, color);
// col.setFriction(friction);
pMultiBody.BaseCollider = (col);
}
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
int parent = pMultiBody.GetParent(i);
world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1];
BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i));
local_origin[i + 1] = local_origin[parent + 1] + vv;
}
for (int i = 0; i < pMultiBody.NumLinks; ++i)
{
BulletSharp.Math.Vector3 posr = local_origin[i + 1];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W };
CollisionShape shape = null;
if (i == 0)
{
shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
}
else
{
shape = new SphereShape(radius);
}
//guiHelper.createCollisionShapeGraphicsObject(shape);
MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i);
col.CollisionShape = (shape);
Matrix tr = new Matrix();
tr.ScaleVector = new BulletSharp.Math.Vector3();
tr.Origin = (posr);
tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3]));
col.WorldTransform = (tr);
// col.setFriction(friction);
bool isDynamic = true;//(linkMass > 0);
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter;
//if (i==0||i>numLinks-2)
{
world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2);
BulletSharp.Math.Vector4 color = colors[curColor];
curColor++;
curColor &= 3;
//guiHelper.createCollisionObjectGraphicsObject(col, color);
pMultiBody.GetLink(i).Collider = col;
}
}
return(pMultiBody);
}
unsafe void CreateBoxShape( BoxShape shape, float totalVolume )
{
Vec3 halfDimension = shape.Dimensions * 0.5f;
Vec3 position = shape.Position;
Quat rotation = shape.Rotation;
float mass = GetShapeMass( shape, totalVolume );
IntPtr material = CreateMaterial( shape );
PhysXNativeBody.CreateBoxShape( nativeBody, ref position, ref rotation, ref halfDimension, 1, (IntPtr*)&material,
mass, shape.ContactGroup );
}
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings)
{
int nLinks = settings.NumLinks;
float mass = 13.5f * Scaling;
Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling;
var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep);
//body.HasSelfCollision = false;
//Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI);
Quaternion orn = new Quaternion(0, 0, 0, 1);
body.BasePosition = settings.BasePosition;
body.WorldToBaseRot = orn;
body.BaseVelocity = Vector3.Zero;
Vector3 joint_axis_hinge = new Vector3(1, 0, 0);
Vector3 joint_axis_prismatic = new Vector3(0, 0, 1);
Quaternion parent_to_child = orn.Inverse;
Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic);
Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge);
int this_link_num = -1;
int link_num_counter = 0;
Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling;
Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling;
for (int i = 0; i < nLinks; i++)
{
float initial_joint_angle = 0.3f;
if (i > 0)
{
initial_joint_angle = -0.06f;
}
int child_link_num = link_num_counter++;
if (settings.UsePrismatic) // i == (nLinks - 1))
{
body.SetupPrismatic(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision);
}
else
{
body.SetupRevolute(child_link_num, mass, inertia, this_link_num,
parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision);
}
body.SetJointPos(child_link_num, initial_joint_angle);
this_link_num = i;
/*if (false) //!useGroundShape && i == 4)
* {
* Vector3 pivotInAworld = new Vector3(0, 20, 46);
* Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld);
* Vector3 pivotInBworld = pivotInAworld;
* MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld);
* (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
* }*/
if (settings.UsePrismatic)
{
//MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3);
if (settings.CreateConstraints)
{
MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
}
}
else
{
//if (true)
{
var con = new MultiBodyJointMotor(body, i, 0, 50000);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
}
var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1);
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2);
}
}
// Add a collider for the base
Quaternion[] worldToLocal = new Quaternion[nLinks + 1];
Vector3[] localOrigin = new Vector3[nLinks + 1];
worldToLocal[0] = body.WorldToBaseRot;
localOrigin[0] = body.BasePosition;
//Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f);
Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f);
float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 };
//float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W };
//if (true)
{
CollisionShape box = new BoxShape(halfExtents * Scaling);
var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia);
RigidBody bodyB = new RigidBody(bodyInfo);
var collider = new MultiBodyLinkCollider(body, -1);
collider.CollisionShape = box;
Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse) * Matrix.Translation(localOrigin[0]);
collider.WorldTransform = tr;
bodyB.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
collider.Friction = Friction;
body.BaseCollider = collider;
}
for (int i = 0; i < body.NumLinks; i++)
{
int parent = body.GetParent(i);
worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1];
localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse.Rotate(body.GetRVector(i)));
}
for (int i = 0; i < body.NumLinks; i++)
{
CollisionShape box = new BoxShape(halfExtents * Scaling);
var collider = new MultiBodyLinkCollider(body, i);
collider.CollisionShape = box;
Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse) * Matrix.Translation(localOrigin[i + 1]);
collider.WorldTransform = tr;
World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter,
CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter);
collider.Friction = Friction;
body.GetLink(i).Collider = collider;
//World.DebugDrawer.DrawBox(halfExtents, pos, quat);
}
(World as MultiBodyDynamicsWorld).AddMultiBody(body);
return(body);
}
/*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;
}
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();
}
}
public Square(string p)
{
string[] s = p.Split('.');
pos = new Position(System.Int32.Parse(s[0]), System.Int32.Parse(s[1]));
value = (BoxShape)(System.Int32.Parse(s[2]));
}
private BumperArrayEntity CreateBumperArray()
{
// Create a bumper array entity with two bumpers
BoxShape leftBumper = new BoxShape(
new BoxShapeProperties("front left",
0.001f,
new Pose(new Vector3(-0.036f, 0.03f, -0.09f)),
new Vector3(0.07f, 0.03f, 0.06f)
)
);
leftBumper.State.DiffuseColor = new Vector4(0.1f, 0.1f, 0.1f, 1.0f);
BoxShape rightBumper = new BoxShape(
new BoxShapeProperties("front right",
0.001f,
new Pose(new Vector3(0.036f, 0.03f, -0.09f)),
new Vector3(0.07f, 0.03f, 0.06f)
)
);
rightBumper.State.DiffuseColor = new Vector4(0.1f, 0.1f, 0.1f, 1.0f);
// The physics engine will issue contact notifications only
// if we enable them per shape
leftBumper.State.EnableContactNotifications = true;
rightBumper.State.EnableContactNotifications = true;
// put some force filtering so we only get notified for significant bumps
//frontBumper.State.ContactFilter = new ContactNotificationFilter(1,1);
//rearBumper.State.ContactFilter = new ContactNotificationFilter(1, 1);
BumperArrayEntity
bumperArray = new BumperArrayEntity(leftBumper, rightBumper);
// entity name, must match manifest partner name
bumperArray.State.Name = "ScribblerBumpers";
// start simulated bumper service
//bumper.Contract.CreateService(
// ConstructorPort,
// Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
// "http://localhost/" + bumperArray.State.Name));
return bumperArray;
}
//-----------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
//string filename = @"C:\users\man\bullett\xna-concave-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
m_animatedMesh = true;
base.InitializeDemo();
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
int indicesTotal = totalTriangles * 3;
gIndices = new ObjectArray <int>(indicesTotal);
//BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
SetVertexPositions(waveheight, 0f);
int vertStride = 1;
int indexStride = 3;
int index = 0;
for (int i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
if (BulletGlobals.g_streamWriter != null)
{
index = 0;
BulletGlobals.g_streamWriter.WriteLine("setIndexPositions");
for (int i = 0; i < gIndices.Count; i++)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("{0} {1}", i, gIndices[i]));
}
}
TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices, indexStride, totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
OptimizedBvh bvh = new OptimizedBvh();
IndexedVector3 aabbMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
//CollisionShape trimeshShape = new TriangleMeshShape(indexVertexArrays);
IndexedVector3 scaling = IndexedVector3.One;
//m_trimeshShape.SetOptimizedBvh(bvh, ref scaling);
//BulletWorldImporter import = new BulletWorldImporter(0);//don't store info into the world
//if (import.loadFile("myShape.bullet"))
//{
// int numBvh = import.getNumBvhs();
// if (numBvh != 0)
// {
// OptimizedBvh bvh = import.getBvhByIndex(0);
// IndexedVector3 aabbMin = new IndexedVector3(-1000,-1000,-1000);
// IndexedVector3 aabbMax = new IndexedVector3(1000,1000,1000);
// trimeshShape = new indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,false);
// IndexedVector3 scaling = IndexedVector3.One;
// trimeshShape.setOptimizedBvh(bvh, ref scaling);
// //trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
// //trimeshShape.setOptimizedBvh(bvh);
// }
// int numShape = import.getNumCollisionShapes();
// if (numShape != 0)
// {
// trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByIndex(0);
// //if you know the name, you can also try to get the shape by name:
// String meshName = import.getNameForPointer(trimeshShape);
// if (meshName != null)
// {
// trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByName(meshName);
// }
// }
//}
//CollisionShape groundShape = trimeshShape;//m_trimeshShape;
CollisionShape groundShape = m_trimeshShape;//m_trimeshShape;
//groundShape = new TriangleShape(new IndexedVector3(0,` 0, 100), new IndexedVector3(100, 0, 0),new IndexedVector3(-100, 0, -100));
//groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
//groundShape = new BoxShape(new IndexedVector3(100f, 0.1f, 100f));
IndexedVector3 up = new IndexedVector3(0.4f, 1, 0);
up.Normalize();
//groundShape = new StaticPlaneShape(up, 0f);
//groundShape = new TriangleMeshShape(indexVertexArrays);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
float mass = 0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2, -2, 0));
//CompoundShape colShape = new CompoundShape();
//IndexedVector3 halfExtents = new IndexedVector3(4, 1, 1);
//CollisionShape cylinderShape = new CylinderShapeX(ref halfExtents);
//CollisionShape boxShape = new BoxShape(new IndexedVector3(4, 1, 1));
//IndexedMatrix localTransform = IndexedMatrix.Identity;
//colShape.addChildShape(ref localTransform, boxShape);
//Quaternion orn = Quaternion.CreateFromYawPitchRoll(MathUtil.SIMD_HALF_PI, 0f, 0f);
//localTransform = IndexedMatrix.CreateFromQuaternion(orn);
//colShape.addChildShape(ref localTransform, cylinderShape);
////BoxShape colShape = new BoxShape(new IndexedVector3(1, 1, 1));
//int numCollideObjects = 1;
//m_collisionShapes.Add(colShape);
//{
// for (int i = 0; i < numCollideObjects; i++)
// {
// startTransform._origin = new IndexedVector3(4,10+i*2,1);
// localCreateRigidBody(1, ref startTransform,colShape);
// }
//}
CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
//CollisionShape boxShape = new SphereShape(1);
//CollisionShape boxShape = new SphereShape(1);
//CollisionShape boxShape = new CapsuleShapeZ(0.5f, 1);
m_collisionShapes.Add(boxShape);
for (int i = 0; i < 1; i++)
{
startTransform._origin = new IndexedVector3(2f * i, 5, 1);
LocalCreateRigidBody(1, ref startTransform, boxShape);
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, ref startTransform, groundShape);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//clientResetScene();
}
private void SpawnRandomPrimitive(JVector position, JVector velocity)
{
RigidBody body = null;
int rndn = rndn = random.Next(7);
// less of the more advanced objects
if (rndn == 5 || rndn == 6)
{
rndn = random.Next(7);
}
switch (rndn)
{
case 0:
body = new RigidBody(new ConeShape((float)random.Next(5, 50) / 20.0f, (float)random.Next(10, 20) / 20.0f));
break;
case 1:
body = new RigidBody(new BoxShape((float)random.Next(10, 30) / 20.0f, (float)random.Next(10, 30) / 20.0f, (float)random.Next(10, 30) / 20.0f));
break;
case 2:
body = new RigidBody(new SphereShape(0.4f));
break;
case 3:
body = new RigidBody(new CylinderShape(1.0f, 0.5f));
break;
case 4:
body = new RigidBody(new CapsuleShape(1.0f, 0.5f));
break;
case 5:
Shape b1 = new BoxShape(new JVector(3, 1, 1));
Shape b2 = new BoxShape(new JVector(1, 1, 3));
Shape b3 = new CylinderShape(3.0f, 0.5f);
CompoundShape.TransformedShape t1 = new CompoundShape.TransformedShape(b1, JMatrix.Identity, JVector.Zero);
CompoundShape.TransformedShape t2 = new CompoundShape.TransformedShape(b2, JMatrix.Identity, JVector.Zero);
CompoundShape.TransformedShape t3 = new CompoundShape.TransformedShape(b3, JMatrix.Identity, new JVector(0, 0, 0));
CompoundShape ms = new CompoundShape(new CompoundShape.TransformedShape[3] {
t1, t2, t3
});
body = new RigidBody(ms);
break;
case 6:
ConvexHullObject obj2 = new ConvexHullObject(this);
Components.Add(obj2);
body = obj2.body;
body.Material.Restitution = 0.2f;
body.Material.StaticFriction = 0.8f;
break;
}
World.AddBody(body);
//body.IsParticle = true;
// body.EnableSpeculativeContacts = true;
body.Position = position;
body.LinearVelocity = velocity;
lastBody = body;
}
public void Width()
{
BoxShape b = new BoxShape();
Assert.AreEqual(0, b.WidthX);
Assert.AreEqual(0, b.WidthY);
Assert.AreEqual(0, b.WidthZ);
b.WidthX = 10;
Assert.AreEqual(10, b.WidthX);
Assert.AreEqual(0, b.WidthY);
Assert.AreEqual(0, b.WidthZ);
b.WidthY = 11;
Assert.AreEqual(10, b.WidthX);
Assert.AreEqual(11, b.WidthY);
Assert.AreEqual(0, b.WidthZ);
b.WidthZ = 12;
Assert.AreEqual(10, b.WidthX);
Assert.AreEqual(11, b.WidthY);
Assert.AreEqual(12, b.WidthZ);
}
/// <summary>
/// Custom constructor for building model from hardcoded values. Used to create entity programmatically
/// </summary>
/// <param name="initialPos"></param>
public ScribblerRobot(Vector3 initialPos)
{
MASS = 0.68f; //kg (1.5 lbs)
// the default settings approximate the Scribbler chassis
CHASSIS_DIMENSIONS = new Vector3(0.1524f, //meters wide (6 in)
0.0508f, //meters high (2 in)
0.1905f); //meters long (7.5 in)
FRONT_WHEEL_MASS = 0.01f;
CHASSIS_CLEARANCE = 0.006f; //0.25 in
FRONT_WHEEL_RADIUS = 0.0381f; //1.5 in
CASTER_WHEEL_RADIUS = 0.0127f; //0.5 in
FRONT_WHEEL_WIDTH = 0.006f; //0.25 in
//CASTER_WHEEL_WIDTH = 0.008f; //not currently used
FRONT_AXLE_DEPTH_OFFSET = 0; // distance of the axle from the center of robot //3.75 in
base.State.Name = "ScribblerRobot";
base.State.MassDensity.Mass = MASS;
base.State.Pose.Position = initialPos;
//this is because the robot is hevily back weighted
base.State.MassDensity.CenterOfMass = new Pose(new Vector3(-1f, -1f, -1f));
// reference point for all shapes is the projection of
// the center of mass onto the ground plane
// (basically the spot under the center of mass, at Y = 0, or ground level)
// NOTE: right/left is from the perspective of the robot, looking forward
// NOTE: X = width of robot (right to left), Y = height, Z = length
// chassis position
BoxShapeProperties motorBaseDesc = new BoxShapeProperties("Scribbler Body", MASS,
new Pose(new Vector3(
0, // Chassis center is also the robot center, so use zero for the X axis offset
CHASSIS_CLEARANCE + CHASSIS_DIMENSIONS.Y / 2, // chassis is off the ground and its center is DIM.Y/2 above the clearance
0)), // any offset in the z/length axis
CHASSIS_DIMENSIONS);
motorBaseDesc.Material = new MaterialProperties("high friction", 0.0f, 1.0f, 20.0f);
motorBaseDesc.Name = "Chassis";
ChassisShape = new BoxShape(motorBaseDesc);
// rear wheel is also called the caster
CASTER_WHEEL_POSITION = new Vector3(0, // center of chassis widthwise
CASTER_WHEEL_RADIUS, // distance from ground
CHASSIS_DIMENSIONS.Z / 2 - CASTER_WHEEL_RADIUS / 2); // all the way at the back of the robot
RIGHT_FRONT_WHEEL_POSITION = new Vector3(
+CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2,// left of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
LEFT_FRONT_WHEEL_POSITION = new Vector3(
-CHASSIS_DIMENSIONS.X / 2 - FRONT_WHEEL_WIDTH / 2,// right of center
FRONT_WHEEL_RADIUS,// distance from ground of axle
FRONT_AXLE_DEPTH_OFFSET); // distance from center, on the z-axis
MotorTorqueScaling = 10;
}
public void Volume()
{
var box = new BoxShape(2, 3, 7);
Assert.AreEqual(2 * 3 * 7, box.GetVolume(0.1f, 1));
}
private BumperArrayEntity CreateLegoNxtBumper()
{
// create a little bumper shape that models the NXT bumper
BoxShape frontBumper = new BoxShape(
new BoxShapeProperties(
"front", 0.001f, //mass
new Pose(new Vector3(0, 0.063f, -0.09f)), //position
new Vector3(0.023f, 0.023f, 0.045f)));
// The physics engine will issue contact notifications only
// if we enable them per shape
frontBumper.State.EnableContactNotifications = true;
BumperArrayEntity
bumperArray = new BumperArrayEntity(frontBumper);
// entity name, must match manifest partner name
bumperArray.State.Name = "LegoNXTBumpers";
// start simulated bumper service
CreateService(
bumper.Contract.Identifier,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + bumperArray.State.Name));
return bumperArray;
}
public void SerializationXml()
{
var a = new BoxShape(11, 22, 33);
// Serialize object.
var stream = new MemoryStream();
var serializer = new XmlSerializer(typeof(Shape));
serializer.Serialize(stream, a);
// Output generated xml. Can be manually checked in output window.
stream.Position = 0;
var xml = new StreamReader(stream).ReadToEnd();
Trace.WriteLine("Serialized BoxShape:\n" + xml);
// Deserialize object.
stream.Position = 0;
var deserializer = new XmlSerializer(typeof(Shape));
var b = (BoxShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.WidthX, b.WidthX);
Assert.AreEqual(a.WidthY, b.WidthY);
Assert.AreEqual(a.WidthZ, b.WidthZ);
}
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);
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));
MogreMotionState myMotionState = new MogreMotionState(box, boxNode, startTransform);
RigidBodyConstructionInfo rbInfo =
new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
myMotionState.Body = body;
// make it drop from a height
body.Translate(new Vector3(0, 20, 0));
World.AddRigidBody(body);
}
}
}
}
public void ExtentZException()
{
BoxShape b = new BoxShape();
b.Extent = new Vector3F(1, 1, -1);
}
