public void Test_New()
{
var sph = new SphereShape (1);
Assert.AreEqual (1, sph.Radius);
Assert.AreEqual (2, sph.Diameter);
}
public void AddChildShape(float4x4 localTransform, ISphereShapeImp shape)
{
Debug.WriteLine("AddSphere");
var btChildShape = new SphereShape(shape.Radius);
var btLocalTransform = Translater.Float4X4ToBtMatrix(localTransform);
BtCompoundShape.AddChildShape(btLocalTransform, btChildShape);
}
public void Test_CreateBulletObject()
{
var sph = new SphereShape (1);
Assert.IsNotNull (sph.CreateGhostObject ());
Assert.IsNotNull (sph.CreateRigidBody (1));
Assert.IsNotNull (sph.CreateBulletShape ());
}
public void Clone()
{
SphereShape sphere = new SphereShape(0.1234f);
SphereShape clone = sphere.Clone() as SphereShape;
Assert.IsNotNull(clone);
Assert.AreEqual(sphere.Radius, clone.Radius);
Assert.AreEqual(sphere.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(sphere.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public void GetMesh()
{
var s = new SphereShape(3);
var mesh = s.GetMesh(0.05f, 3);
Assert.Greater(mesh.NumberOfTriangles, 1);
for (int i = 0; i < mesh.Vertices.Count; i++)
Assert.IsTrue(Numeric.AreEqual(3, mesh.Vertices[i].Length));
}
public void Test_CreateShape()
{
var box = new SphereShape (1);
var shp = box.CreateBulletShape () as BulletSharp.SphereShape;
var radius = shp.Radius * PhysicsSimulator.PPM;
Assert.AreEqual (1, radius);
}
public void ContactWelding()
{
var mesh = new SphereShape(0.5f).GetMesh(0.0001f, 7);
Stopwatch watch = Stopwatch.StartNew();
var meshShape = new TriangleMeshShape(mesh, true, null);
watch.Stop();
//Assert.AreEqual(0, watch.Elapsed.TotalMilliseconds);
Assert.AreEqual(mesh.NumberOfTriangles * 3, meshShape.TriangleNeighbors.Count);
for (int i = 0; i < mesh.NumberOfTriangles; i++)
{
Assert.AreNotEqual(-1, meshShape.TriangleNeighbors[i * 3 + 0]);
Assert.AreNotEqual(-1, meshShape.TriangleNeighbors[i * 3 + 1]);
Assert.AreNotEqual(-1, meshShape.TriangleNeighbors[i * 3 + 2]);
var triangle = mesh.GetTriangle(i);
// Check if each edge neighbor shares two vertices with this triangle.
for (int e = 0; e < 3; e++)
{
var vertex0 = triangle[(e + 1) % 3];
var vertex1 = triangle[(e + 2) % 3];
var neighbor = mesh.GetTriangle(meshShape.TriangleNeighbors[i * 3 + e]);
int sharedCount = 0;
// Count shared vertices.
for (int j = 0; j < 3; j++)
{
if (Vector3F.AreNumericallyEqual(vertex0, neighbor[j]))
sharedCount++;
if (Vector3F.AreNumericallyEqual(vertex1, neighbor[j]))
sharedCount++;
}
Assert.AreEqual(2, sharedCount);
}
}
}
public void ProcessTriangle(ObjectArray<Vector3> triangle, int partId, int triangleIndex)
{
//do a swept sphere for now
Matrix ident = Matrix.Identity;
CastResult castResult = new CastResult();
castResult.m_fraction = m_hitFraction;
SphereShape pointShape = new SphereShape(m_ccdSphereRadius);
TriangleShape triShape = new TriangleShape(triangle[0],triangle[1],triangle[2]);
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(pointShape,triShape,simplexSolver);
//GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
//local space?
if (convexCaster.CalcTimeOfImpact(ref m_ccdSphereFromTrans,ref m_ccdSphereToTrans,
ref ident,ref ident,castResult))
{
if (m_hitFraction > castResult.m_fraction)
{
m_hitFraction = castResult.m_fraction;
}
}
}
private void CreateMesh(string filePath)
{
Bodies = new List <RigidBody>();
VisualMeshes = new List <Mesh>();
BXDAMesh mesh = new BXDAMesh();
mesh.ReadFromFile(filePath);
foreach (FieldNode node in NodeGroup.EnumerateAllLeafFieldNodes())
{
if (!GetPropertySets().ContainsKey(node.PropertySetID))
{
return;
}
PropertySet current = GetPropertySets()[node.PropertySetID];
CollisionShape subShape = null;
switch (current.Collider.CollisionType)
{
case PropertySet.PropertySetCollider.PropertySetCollisionType.BOX:
{
//Create a box shape
//This is a mess, though I was told that this is how it works
Vector3[] vertices = MeshUtilities.DataToVector(mesh.meshes[node.SubMeshID].verts);
StridingMeshInterface temp = MeshUtilities.BulletShapeFromSubMesh(mesh.meshes[node.SubMeshID]);
Vector3 min, max;
temp.CalculateAabbBruteForce(out min, out max);
PropertySet.BoxCollider colliderInfo = (PropertySet.BoxCollider)current.Collider;
subShape = new BoxShape((max - min) * colliderInfo.Scale.Convert() * 0.5f);
if (debug)
{
Console.WriteLine("Created Box");
}
break;
}
case PropertySet.PropertySetCollider.PropertySetCollisionType.SPHERE:
{
//Create a sphere shape
PropertySet.SphereCollider colliderInfo = (PropertySet.SphereCollider)current.Collider;
subShape = new SphereShape(colliderInfo.Scale);
if (debug)
{
Console.WriteLine("Created Sphere");
}
break;
}
case PropertySet.PropertySetCollider.PropertySetCollisionType.MESH:
{
//Create a mesh shape
if (node.CollisionMeshID == -1)
{
break;
}
PropertySet.MeshCollider colliderInfo = (PropertySet.MeshCollider)current.Collider;
Vector3[] vertices = MeshUtilities.DataToVector(mesh.colliders[node.CollisionMeshID].verts);
if (colliderInfo.Convex)
{
subShape = new ConvexHullShape(vertices);
if (debug)
{
Console.WriteLine("Created Convex Mesh");
}
}
else
{
StridingMeshInterface sMesh = MeshUtilities.BulletShapeFromSubMesh(mesh.colliders[node.CollisionMeshID]);
subShape = new ConvexTriangleMeshShape(sMesh, true); //still not really concave
if (debug)
{
Console.WriteLine("Created Concave Mesh");
}
}
break;
}
}
if (null == subShape)
{
return;
}
//set sub shape local position/rotation and add it to the compound shape
Vector3 Translation = node.Position.Convert();
Quaternion rotation = node.Rotation.Convert();
DefaultMotionState motion = new DefaultMotionState(Matrix4.CreateFromQuaternion(rotation) * Matrix4.CreateTranslation(Translation));
motion.CenterOfMassOffset = Matrix4.CreateTranslation(mesh.physics.centerOfMass.Convert());
RigidBodyConstructionInfo info = new RigidBodyConstructionInfo(current.Mass, motion, subShape, subShape.CalculateLocalInertia(current.Mass));
info.Friction = current.Friction;
Bodies.Add(new RigidBody(info));
VisualMeshes.Add(new Mesh(mesh.meshes[node.SubMeshID], Translation));
if (debug)
{
Console.WriteLine("Created " + node.PropertySetID);
}
}
}
public static void CreateSphere(SphereShape shape, Mesh mesh)
{
mesh.Clear();
float radius = shape.Radius;
// Longitude |||
int nbLong = 24;
// Latitude ---
int nbLat = 16;
#region Vertices
UnityEngine.Vector3[] vertices = new UnityEngine.Vector3[(nbLong + 1) * nbLat + 2];
float _pi = Mathf.PI;
float _2pi = _pi * 2f;
vertices[0] = UnityEngine.Vector3.up * radius;
for (int lat = 0; lat < nbLat; lat++)
{
float a1 = _pi * (lat + 1) / (nbLat + 1);
float sin1 = Mathf.Sin(a1);
float cos1 = Mathf.Cos(a1);
for (int lon = 0; lon <= nbLong; lon++)
{
float a2 = _2pi * (lon == nbLong ? 0 : lon) / nbLong;
float sin2 = Mathf.Sin(a2);
float cos2 = Mathf.Cos(a2);
vertices[lon + lat * (nbLong + 1) + 1] = new UnityEngine.Vector3(sin1 * cos2, cos1, sin1 * sin2) * radius;
}
}
vertices[vertices.Length - 1] = UnityEngine.Vector3.up * -radius;
#endregion
#region Normales
UnityEngine.Vector3[] normales = new UnityEngine.Vector3[vertices.Length];
for (int n = 0; n < vertices.Length; n++)
{
normales[n] = vertices[n].normalized;
}
#endregion
#region UVs
Vector2[] uvs = new Vector2[vertices.Length];
uvs[0] = Vector2.up;
uvs[uvs.Length - 1] = Vector2.zero;
for (int lat = 0; lat < nbLat; lat++)
{
for (int lon = 0; lon <= nbLong; lon++)
{
uvs[lon + lat * (nbLong + 1) + 1] = new Vector2((float)lon / nbLong, 1f - (float)(lat + 1) / (nbLat + 1));
}
}
#endregion
#region Triangles
int nbFaces = vertices.Length;
int nbTriangles = nbFaces * 2;
int nbIndexes = nbTriangles * 3;
int[] triangles = new int[nbIndexes];
//Top Cap
int i = 0;
for (int lon = 0; lon < nbLong; lon++)
{
triangles[i++] = lon + 2;
triangles[i++] = lon + 1;
triangles[i++] = 0;
}
//Middle
for (int lat = 0; lat < nbLat - 1; lat++)
{
for (int lon = 0; lon < nbLong; lon++)
{
int current = lon + lat * (nbLong + 1) + 1;
int next = current + nbLong + 1;
triangles[i++] = current;
triangles[i++] = current + 1;
triangles[i++] = next + 1;
triangles[i++] = current;
triangles[i++] = next + 1;
triangles[i++] = next;
}
}
//Bottom Cap
for (int lon = 0; lon < nbLong; lon++)
{
triangles[i++] = vertices.Length - 1;
triangles[i++] = vertices.Length - (lon + 2) - 1;
triangles[i++] = vertices.Length - (lon + 1) - 1;
}
#endregion
mesh.vertices = vertices;
mesh.normals = normales;
mesh.uv = uvs;
mesh.triangles = triangles;
mesh.RecalculateBounds();
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
Vector3 localClosestPoint = new Vector3();
#else
Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
Vector3 offset;
Vector3.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return(false);
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.Z;
}
contact.PenetrationDepth += sphere.collisionMargin;
Quaternion.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return(true);
}
/*
* Mesh CreateMultiSphereShape(MultiSphereShape shape)
* {
* Mesh mesh = null;
*
* int i;
* for (i = 0; i < shape.SphereCount; i++)
* {
* Vector3 position = shape.GetSpherePosition(i);
*
* Mesh sphereMesh = Mesh.CreateSphere(device, shape.GetSphereRadius(i), 12, 12);
* if (i == 0)
* {
* Matrix[] transform = new Matrix[] { Matrix.Translation(position) };
* mesh = Mesh.Concatenate(device, new Mesh[] { sphereMesh }, MeshFlags.Managed, transform, null);
* }
* else
* {
* Mesh multiSphereMeshNew;
* Matrix[] transform = new Matrix[] { Matrix.Identity, Matrix.Translation(position) };
* multiSphereMeshNew = Mesh.Concatenate(device, new Mesh[] { mesh, sphereMesh }, MeshFlags.Managed, transform, null);
* mesh.Dispose();
* mesh = multiSphereMeshNew;
* }
* sphereMesh.Dispose();
* }
*
* complexShapes.Add(shape, mesh);
* return mesh;
* }
*/
ShapeData CreateSphereShape(SphereShape shape)
{
float radius = shape.Radius;
int slices = (int)(radius * 10.0f);
int stacks = (int)(radius * 10.0f);
slices = (slices > 16) ? 16 : (slices < 3) ? 3 : slices;
stacks = (stacks > 16) ? 16 : (stacks < 2) ? 2 : stacks;
float hAngleStep = (float)Math.PI * 2 / slices;
float vAngleStep = (float)Math.PI / stacks;
ShapeData shapeData = new ShapeData();
shapeData.VertexCount = 2 + slices * (stacks - 1);
shapeData.IndexCount = 6 * slices * (stacks - 1);
Vector3[] vertices = new Vector3[shapeData.VertexCount * 2];
ushort[] indices = new ushort[shapeData.IndexCount];
int i = 0, v = 0;
// Vertices
// Top and bottom
vertices[v++] = new Vector3(0, -radius, 0);
vertices[v++] = -Vector3.UnitY;
vertices[v++] = new Vector3(0, radius, 0);
vertices[v++] = Vector3.UnitY;
// Stacks
int j, k;
float angle = 0;
float vAngle = -(float)Math.PI / 2;
Vector3 vTemp;
for (j = 0; j < stacks - 1; j++)
{
vAngle += vAngleStep;
for (k = 0; k < slices; k++)
{
angle += hAngleStep;
vTemp = new Vector3((float)Math.Cos(vAngle) * (float)Math.Sin(angle), (float)Math.Sin(vAngle), (float)Math.Cos(vAngle) * (float)Math.Cos(angle));
vertices[v++] = vTemp * radius;
vertices[v++] = Vector3.Normalize(vTemp);
}
}
// Indices
// Top cap
ushort index = 2;
for (k = 0; k < slices; k++)
{
indices[i++] = index++;
indices[i++] = 0;
indices[i++] = index;
}
indices[i - 1] = 2;
// Stacks
//for (j = 0; j < 1; j++)
int sliceDiff = slices * 3;
for (j = 0; j < stacks - 2; j++)
{
for (k = 0; k < slices; k++)
{
indices[i] = indices[i - sliceDiff + 2];
indices[i + 1] = index++;
indices[i + 2] = indices[i - sliceDiff];
i += 3;
}
for (k = 0; k < slices; k++)
{
indices[i] = indices[i - sliceDiff + 1];
indices[i + 1] = indices[i - sliceDiff];
indices[i + 2] = indices[i - sliceDiff + 4];
i += 3;
}
indices[i - 1] = indices[i - sliceDiff];
}
// Bottom cap
index--;
for (k = 0; k < slices; k++)
{
indices[i++] = index--;
indices[i++] = 1;
indices[i++] = index;
}
indices[i - 1] = indices[i - sliceDiff];
shapeData.SetVertexBuffer(device, vertices);
shapeData.SetIndexBuffer(device, indices);
return(shapeData);
}
public AdvancedAvatarRagdollSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
SampleFramework.IsMouseVisible = false;
// 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 Vector3(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(Vector3.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++)
{
Vector3 position = RandomHelper.Random.NextVector3(-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++)
{
Vector3 position = RandomHelper.Random.NextVector3(-10, 10);
position.Y = 1;
Simulation.RigidBodies.Add(new RigidBody(boxShape)
{
Pose = new Pose(position)
});
}
}
private static void Main(string[] args)
{
var collisionConfig = new DefaultCollisionConfiguration();
var dispatcher = new CollisionDispatcher(collisionConfig);
var pairCache = new DbvtBroadphase();
var solver = new SequentialImpulseConstraintSolver();
var dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, pairCache, solver, collisionConfig);
dynamicsWorld.Gravity = new Vector3(0, -10, 0);
var groundShape = new BoxShape(new Vector3(50, 50, 50));
var groundTransform = Matrix.CreateTranslation(0, -56, 0);
{
float mass = 0;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = groundShape.CalculateLocalInertia(mass);
var motionState = new DefaultMotionState(groundTransform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, groundShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
{
var collisionShape = new SphereShape(1);
var transform = Matrix.Identity;
float mass = 1;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = collisionShape.CalculateLocalInertia(mass);
transform.Translation = new Vector3(2, 10, 0);
var motionState = new DefaultMotionState(transform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
Console.WriteLine("Starting simulation");
for (int i = 0; i < 100; i++)
{
dynamicsWorld.StepSimulation(1f / 60f, 10);
for (int j = dynamicsWorld.GetNumCollisionObjects() - 1; j >= 0; j--)
{
var obj = dynamicsWorld.GetCollisionObjectArray()[j];
var body = (RigidBody)obj;
if (body != null && body.GetMotionState() != null)
{
Matrix transform = Matrix.Identity;
body.GetMotionState().GetWorldTransform(ref transform);
Console.WriteLine("Object@{0} Position={1}", body.GetHashCode(), transform.Translation);
}
}
}
Console.Read();
}
public BaseDisposable SharedComponentCreate(string id, string name, int classId)
{
SceneController.i.OnMessageDecodeStart?.Invoke("ComponentCreated");
sharedComponentCreatedMessage.id = id;
sharedComponentCreatedMessage.name = name;
sharedComponentCreatedMessage.classId = classId;
SceneController.i.OnMessageDecodeEnds?.Invoke("ComponentCreated");
BaseDisposable disposableComponent;
if (disposableComponents.TryGetValue(sharedComponentCreatedMessage.id, out disposableComponent))
{
return(disposableComponent);
}
BaseDisposable newComponent = null;
switch ((CLASS_ID)sharedComponentCreatedMessage.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 (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;
}
default:
Debug.LogError($"Unknown classId");
break;
}
if (newComponent != null)
{
newComponent.id = sharedComponentCreatedMessage.id;
disposableComponents.Add(sharedComponentCreatedMessage.id, newComponent);
if (state != State.READY)
{
disposableNotReady.Add(id);
}
}
return(newComponent);
}
public void Volume()
{
var s = new SphereShape(17);
Assert.AreEqual(4f/3f * ConstantsF.Pi * 17 * 17 * 17, s.GetVolume(0.1f, 1));
}
protected virtual void IntegrateTransforms(float timeStep)
{
//BT_PROFILE("integrateTransforms");
Matrix predictedTrans = Matrix.Identity;
int length = m_nonStaticRigidBodies.Count;
for (int i = 0; i < length; ++i)
{
RigidBody body = m_nonStaticRigidBodies[i];
if (body != null)
{
body.SetHitFraction(1f);
if (body.IsActive() && (!body.IsStaticOrKinematicObject()))
{
body.PredictIntegratedTransform(timeStep, ref predictedTrans);
float squareMotion = (predictedTrans.Translation - body.GetWorldTransform().Translation).LengthSquared();
if (body.GetCcdSquareMotionThreshold() != 0 && body.GetCcdSquareMotionThreshold() < squareMotion)
{
//BT_PROFILE("CCD motion clamping");
if (body.GetCollisionShape().IsConvex())
{
gNumClampedCcdMotions++;
ClosestNotMeConvexResultCallback sweepResults = new ClosestNotMeConvexResultCallback(body, body.GetWorldTransform().Translation, predictedTrans.Translation, GetBroadphase().GetOverlappingPairCache(), GetDispatcher());
//ConvexShape convexShape = (ConvexShape)(body.getCollisionShape());
SphereShape tmpSphere = new SphereShape(body.GetCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body.getCollisionShape());
sweepResults.m_collisionFilterGroup = body.GetBroadphaseProxy().m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body.GetBroadphaseProxy().m_collisionFilterMask;
ConvexSweepTest(tmpSphere, body.GetWorldTransform(), predictedTrans, sweepResults, 0f);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1f))
{
body.SetHitFraction(sweepResults.m_closestHitFraction);
body.PredictIntegratedTransform(timeStep * body.GetHitFraction(), ref predictedTrans);
body.SetHitFraction(0f);
// printf("clamped integration to hit fraction = %f\n",fraction);
}
}
}
body.ProceedToTransform(ref predictedTrans);
}
}
}
}
public void SerializationXml()
{
var a = new SphereShape(11);
// 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 Object:\n" + xml);
// Deserialize object.
stream.Position = 0;
var deserializer = new XmlSerializer(typeof(Shape));
var b = (SphereShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Radius, b.Radius);
}
public void SerializationBinary()
{
var a = new SphereShape(11);
// Serialize object.
var stream = new MemoryStream();
var formatter = new BinaryFormatter();
formatter.Serialize(stream, a);
// Deserialize object.
stream.Position = 0;
var deserializer = new BinaryFormatter();
var b = (SphereShape)deserializer.Deserialize(stream);
Assert.AreEqual(a.Radius, b.Radius);
}
public void RadiusException()
{
SphereShape s = new SphereShape();
s.Radius = -1;
}
public void Radius()
{
SphereShape s = new SphereShape();
Assert.AreEqual(0, s.Radius);
s.Radius = 3;
Assert.AreEqual(3, s.Radius);
}
public virtual bool CalcTimeOfImpact(ref Matrix fromA, ref Matrix toA, ref Matrix fromB, ref Matrix toB, CastResult result)
{
m_simplexSolver.Reset();
/// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = Vector3.Zero, angVelA = Vector3.Zero, linVelB = Vector3.Zero, angVelB = Vector3.Zero;
TransformUtil.CalculateVelocity(ref fromA,ref toA,1f,ref linVelA,ref angVelA);
TransformUtil.CalculateVelocity(ref fromB,ref toB,1f,ref linVelB,ref angVelB);
float boundingRadiusA = m_convexA.GetAngularMotionDisc();
float boundingRadiusB = m_convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA + angVelB.Length() * boundingRadiusB;
Vector3 relLinVel = (linVelB-linVelA);
float relLinVelocLength = relLinVel.Length();
if (MathUtil.FuzzyZero(relLinVelocLength + maxAngularProjectedVelocity))
{
return false;
}
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1,0,0);
int maxIter = MAX_ITERATIONS;
Vector3 n = Vector3.Zero;
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//btScalar epsilon = btScalar(0.001);
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin = 0f;
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
{
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
//we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.m_transformA = fromA;
input.m_transformB = fromB;
gjk.GetClosestPoints(input,pointCollector1,null,false);
hasResult = pointCollector1.m_hasResult;
c = pointCollector1.m_pointInWorld;
}
if (hasResult)
{
float dist = pointCollector1.m_distance;
n = pointCollector1.m_normalOnBInWorld;
float projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//not close enough
while (dist > radius)
{
if (result.m_debugDrawer != null)
{
Vector3 colour = new Vector3(1, 1, 1);
result.m_debugDrawer.DrawSphere(ref c, 0.2f, ref colour);
}
numIter++;
if (numIter > maxIter)
{
return false; //todo: report a failure
}
float dLambda = 0f;
projectedLinearVelocity = Vector3.Dot(relLinVel,n);
//calculate safe moving fraction from distance / (linear+rotational velocity)
//btScalar clippedDist = GEN_min(angularConservativeRadius,dist);
//btScalar clippedDist = dist;
//don't report time of impact for motion away from the contact normal (or causes minor penetration)
if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=MathUtil.SIMD_EPSILON)
{
return false;
}
dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f || lambda < 0f)
{
return false;
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
return false;
//n.setValue(0,0,0);
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = Matrix.Identity, interpolatedTransB = Matrix.Identity, relativeTrans = Matrix.Identity;
TransformUtil.IntegrateTransform(ref fromA,ref linVelA,ref angVelA,lambda,ref interpolatedTransA);
TransformUtil.IntegrateTransform(ref fromB,ref linVelB,ref angVelB,lambda,ref interpolatedTransB);
//relativeTrans = interpolatedTransB.inverseTimes(interpolatedTransA);
relativeTrans = MathUtil.InverseTimes(ref interpolatedTransB, ref interpolatedTransA);
if (result.m_debugDrawer != null)
{
result.m_debugDrawer.DrawSphere(interpolatedTransA.Translation, 0.2f, new Vector3(1, 0, 0));
}
result.DebugDraw( lambda );
PointCollector pointCollector = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver);
ClosestPointInput input = new ClosestPointInput();
input.m_transformA = interpolatedTransA;
input.m_transformB = interpolatedTransB;
gjk.GetClosestPoints(input,pointCollector,null,false);
if (pointCollector.m_hasResult)
{
if (pointCollector.m_distance < 0f)
{
//degenerate ?!
result.m_fraction = lastLambda;
n = pointCollector.m_normalOnBInWorld;
result.m_normal=n;//.setValue(1,1,1);// = n;
result.m_hitPoint = pointCollector.m_pointInWorld;
return true;
}
c = pointCollector.m_pointInWorld;
n = pointCollector.m_normalOnBInWorld;
dist = pointCollector.m_distance;
} else
{
//??
return false;
}
}
if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= result.m_allowedPenetration)//SIMD_EPSILON)
{
return false;
}
result.m_fraction = lambda;
result.m_normal = n;
result.m_hitPoint = c;
return true;
}
return false;
/*
//todo:
//if movement away from normal, discard result
btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin();
if (result.m_fraction < btScalar(1.))
{
if (move.dot(result.m_normal) <= btScalar(0.))
{
}
}
*/
}
//
public float SignedDistance(ref Vector3 position, float margin, ConvexShape shape0, ref Matrix wtrs0, GjkEpaSolver2Results results)
{
GjkEpaSolver2MinkowskiDiff shape = new GjkEpaSolver2MinkowskiDiff();
SphereShape shape1 = new SphereShape(margin);
Matrix wtrs1 = Matrix.CreateFromQuaternion(Quaternion.Identity);
wtrs0.Translation = position;
Initialize(shape0,ref wtrs0,shape1,ref wtrs1,results,shape,false);
GJK gjk = new GJK();
Vector3 guess = new Vector3(1,1,1);
GJKStatus gjk_status=gjk.Evaluate(shape,ref guess);
if(gjk_status==GJKStatus.Valid)
{
Vector3 w0=Vector3.Zero;
Vector3 w1=Vector3.Zero;
for(int i=0;i<gjk.m_simplex.rank;++i)
{
float p=gjk.m_simplex.p[i];
w0+=shape.Support( ref gjk.m_simplex.c[i].d,0)*p;
Vector3 temp = -gjk.m_simplex.c[i].d;
w1+=shape.Support(ref temp,1)*p;
}
results.witnesses0 = Vector3.Transform(w0,wtrs0);
results.witnesses1 = Vector3.Transform(w1,wtrs0);
Vector3 delta= results.witnesses1-results.witnesses0;
float margin2 = shape0.GetMarginNonVirtual()+shape1.GetMarginNonVirtual();
float length = delta.Length();
results.normal = delta/length;
results.witnesses0 += results.normal*margin2;
return(length-margin2);
}
else
{
if(gjk_status==GJKStatus.Inside)
{
if(Penetration(shape0,ref wtrs0,shape1,ref wtrs1,ref gjk.m_ray,results))
{
Vector3 delta= results.witnesses0-results.witnesses1;
float length= delta.Length();
if (length >= MathUtil.SIMD_EPSILON)
results.normal = delta/length;
return(-length);
}
}
}
return(MathUtil.SIMD_INFINITY);
}
private void ConstructCasterWheelShape()
{
// add caster wheel as a basic sphere shape
CasterWheelShape = new SphereShape(
new SphereShapeProperties("rear wheel", 0.001f,
new Pose(CASTER_WHEEL_POSITION), CASTER_WHEEL_RADIUS));
CasterWheelShape.State.Name = "Caster wheel";
// a fixed caster wheel has high friction when moving
laterely, but low friction when it moves along the
// body axis its aligned with. We use anisotropic
friction to model this
CasterWheelShape.State.Material = new
MaterialProperties("small friction with anisotropy", 0.5f, 0.5f, 1);
CasterWheelShape.State.Material.Advanced = new
MaterialAdvancedProperties();
CasterWheelShape.State.Material.Advanced.AnisotropicDynamicFriction =
0.3f;
CasterWheelShape.State.Material.Advanced.AnisotropicStaticFriction =
0.4f;
CasterWheelShape.State.Material.Advanced.AnisotropyDirection = new
Vector3(0, 0, 1);
}
private void CloneSphere(SphereShape sh)
{
this.fp1 = sh.radius;
}
public SphereTriangleDetector(SphereShape sphere,TriangleShape triangle, float contactBreakingThreshold)
{
m_sphere = sphere;
m_triangle = triangle;
m_contactBreakingThreshold = contactBreakingThreshold;
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
protected override void OnLoad()
{
// Add rigid bodies to simulation.
var simulation = _services.GetInstance <Simulation>();
// We use a random number generator with a custom seed.
RandomHelper.Random = new Random(123);
// ----- Add a ground plane.
AddBody(simulation, "GroundPlane", Pose.Identity, new PlaneShape(Vector3F.UnitY, 0), MotionType.Static);
// ----- Create a small flying sphere.
AddBody(simulation, "Sphere", new Pose(new Vector3F(0, 1f, 0)), new SphereShape(0.2f), MotionType.Static);
// ----- Create small walls at the level boundary.
AddBody(simulation, "WallLeft", new Pose(new Vector3F(-30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
AddBody(simulation, "WallRight", new Pose(new Vector3F(30, 1, 0)), new BoxShape(0.3f, 2, 60), MotionType.Static);
AddBody(simulation, "WallFront", new Pose(new Vector3F(0, 1, -30)), new BoxShape(60, 2, 0.3f), MotionType.Static);
AddBody(simulation, "WallBack", new Pose(new Vector3F(0, 1, 30)), new BoxShape(60, 2, 0.3f), MotionType.Static);
// ----- 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.
AddBody(simulation, "House0", new Pose(new Vector3F(10, 1, -10)), new BoxShape(8, 2, 8f), MotionType.Static);
AddBody(simulation, "House1", new Pose(new Vector3F(13, 1, -4)), new BoxShape(2, 2, 4), MotionType.Static);
AddBody(simulation, "House2", new Pose(new Vector3F(10, 2, -15), Matrix33F.CreateRotationY(-0.3f)), new BoxShape(8, 4, 2), MotionType.Static);
// ----- 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;
Vector3F position = new Vector3F(0, startHeight + stepHeight / 2, -2 - i * stepDepth);
AddBody(simulation, "Step" + i, new Pose(position), new BoxShape(2, stepHeight, stepDepth), MotionType.Static);
startHeight += stepHeight;
}
// ----- V obstacle to test if we get stuck.
AddBody(simulation, "V0", new Pose(new Vector3F(-5.5f, 0, 10), QuaternionF.CreateRotationZ(0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);
AddBody(simulation, "V1", new Pose(new Vector3F(-4, 0, 10), QuaternionF.CreateRotationZ(-0.2f)), new BoxShape(1f, 2f, 2), MotionType.Static);
// ----- 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);
AddBody(simulation, "HeightField", new Pose(new Vector3F(10, 0, 10)), heightField, MotionType.Static);
// ----- 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++)
{
Vector3F position = new Vector3F(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20));
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
Vector3F size = RandomHelper.Random.NextVector3F(0.05f, 0.8f);
AddBody(simulation, "Stone" + i, new Pose(position, orientation), new BoxShape(size), MotionType.Static);
}
// ----- 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.
AddBody(simulation, "SlopeGround", new Pose(new Vector3F(-2, 1.8f, -12), QuaternionF.CreateRotationX(0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);
AddBody(simulation, "SlopeRoof", new Pose(new Vector3F(-2, 5.6f, -12), QuaternionF.CreateRotationX(-0.4f)), new BoxShape(2, 0.5f, 10), MotionType.Static);
// Create slopes with increasing tilt angles.
// The character controller has a slope limit. Only flat slopes should be climbable.
// Movement between slopes should be smooth.
Vector3F slopePosition = new Vector3F(-17, -0.25f, 6);
BoxShape slopeShape = new BoxShape(8, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX((i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
AddBody(simulation, "Slope" + i, new Pose(slopePosition, rotation), slopeShape, MotionType.Static);
}
// Create slopes with decreasing tilt angles.
slopePosition = new Vector3F(-8, -2, 5);
slopeShape = new BoxShape(8f, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - (i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
Vector3F position = slopePosition - rotation * new Vector3F(0, slopeShape.WidthY / 2, 0);
AddBody(simulation, "Slope2" + i, new Pose(position, rotation), slopeShape, MotionType.Static);
}
// ----- 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.)
AddBody(simulation, "LongSlope", new Pose(new Vector3F(-24, 3, -10), Matrix33F.CreateRotationX(0.4f)), new BoxShape(4, 5f, 30), MotionType.Static);
AddBody(simulation, "LongSlopeWall", new Pose(new Vector3F(-26, 5, -10)), new BoxShape(0.5f, 10f, 25), MotionType.Static);
// ----- Create a trigger object that represents a ladder.
var ladder = AddBody(simulation, "Ladder", new Pose(new Vector3F(-25.7f, 5, 0)), new BoxShape(0.5f, 10f, 1), MotionType.Static);
ladder.CollisionObject.Type = CollisionObjectType.Trigger;
// ----- 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 Vector3F(10, 1, 10))));
compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3F(15, 0, 15))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 5))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(4, 4, 3), new Pose(new Vector3F(15, 1.5f, 0))));
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,
};
// Contact welding creates smoother contact normals - but it costs a bit of performance.
meshShape.EnableContactWelding = true;
AddBody(simulation, "Mesh", new Pose(new Vector3F(-30, 0, 10)), meshShape, MotionType.Static);
// ----- Create a seesaw.
var seesawBase = AddBody(simulation, "SeesawBase", new Pose(new Vector3F(5, 0.5f, 0)), new BoxShape(0.2f, 1, 1), MotionType.Static);
var seesaw = AddBody(simulation, "Seesaw", new Pose(new Vector3F(5, 1.05f, 0)), new BoxShape(5, 0.1f, 1), MotionType.Dynamic);
// Attach the seesaw to the base using a hinge joint.
simulation.Constraints.Add(new HingeJoint
{
BodyA = seesaw,
BodyB = seesawBase,
AnchorPoseALocal = new Pose(new Vector3F(0, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.5f, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
CollisionEnabled = false,
});
// ----- A platform that is moving up/down.
_elevator = AddBody(simulation, "Elevator", new Pose(new Vector3F(5, -1f, 5)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
_elevator.LinearVelocity = new Vector3F(2, 2, 0);
// ----- A platform that is moving sideways.
_pusher = AddBody(simulation, "Pusher", new Pose(new Vector3F(15, 0.5f, 0)), new BoxShape(3, 1f, 3), MotionType.Kinematic);
_pusher.LinearVelocity = new Vector3F(0, 0, 2);
// ----- Create conveyor belt with two static boxes on the sides.
AddBody(simulation, "ConveyorSide0", new Pose(new Vector3F(19, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);
AddBody(simulation, "ConveyorSide1", new Pose(new Vector3F(21, 0.25f, 0)), new BoxShape(0.8f, 0.5f, 8f), MotionType.Static);
// The conveyor belt is a simple box with a special material.
var conveyorBelt = AddBody(simulation, "ConveyorBelt", new Pose(new Vector3F(20, 0.25f, 0)), new BoxShape(1f, 0.51f, 8f), MotionType.Static);
UniformMaterial materialWithSurfaceMotion = new UniformMaterial("ConveyorBelt", true) // Important: The second parameter enables the surface
{ // motion. It has to be set to true in the constructor!
SurfaceMotion = new Vector3F(0, 0, 1), // The surface motion relative to the object.
};
conveyorBelt.Material = materialWithSurfaceMotion;
// ----- Distribute a few dynamic spheres and boxes across the landscape.
SphereShape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
position.Y = 20;
AddBody(simulation, "Sphere" + i, new Pose(position), sphereShape, MotionType.Dynamic);
}
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 10; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-15, 15);
position.Y = 20;
AddBody(simulation, "Box" + i, new Pose(position), boxShape, MotionType.Dynamic);
}
}
unsafe void CreateSphereShape( SphereShape shape, float totalVolume )
{
Vec3 position = shape.Position;
Quat rotation = shape.Rotation;
float mass = GetShapeMass( shape, totalVolume );
IntPtr material = CreateMaterial( shape );
PhysXNativeBody.CreateSphereShape( nativeBody, ref position, ref rotation, shape.Radius, 1, (IntPtr*)&material,
mass, shape.ContactGroup );
}
/// <summary>
/// Builds the simulated robotic entity using local fields for positionm size, orientation
/// </summary>
/// <param name="device"></param>
/// <param name="physicsEngine"></param>
public void ProgrammaticallyBuildModel(xnagrfx.GraphicsDevice device, PhysicsEngine physicsEngine)
{
if (_casterWheelShape == null)
{
// add caster wheel as a basic sphere shape
CasterWheelShape = new SphereShape(
new SphereShapeProperties("rear wheel", 0.001f,
new Pose(CASTER_WHEEL_POSITION), CASTER_WHEEL_RADIUS));
CasterWheelShape.State.Name = "Caster wheel";
// a fixed caster wheel has high friction when moving laterely, but low friction when it moves along the
// body axis its aligned with. We use anisotropic friction to model this
CasterWheelShape.State.Material = new MaterialProperties("small friction with anisotropy", 0.5f, 0.5f, 1);
CasterWheelShape.State.Material.Advanced = new MaterialAdvancedProperties();
CasterWheelShape.State.Material.Advanced.AnisotropicDynamicFriction = 0.3f;
CasterWheelShape.State.Material.Advanced.AnisotropicStaticFriction = 0.4f;
CasterWheelShape.State.Material.Advanced.AnisotropyDirection = new Vector3(0, 0, 1);
}
base.State.PhysicsPrimitives.Add(_casterWheelShape);
if (_chassisShape != null)
base.State.PhysicsPrimitives.Add(_chassisShape);
base.CreateAndInsertPhysicsEntity(physicsEngine);
// increase physics fidelity
base.PhysicsEntity.SolverIterationCount = 64;
// if we were created from xml the wheel entities would already be instantiated
if (_leftWheel != null && _rightWheel != null)
return;
// front left wheel
WheelShapeProperties w = new WheelShapeProperties("front left wheel", FRONT_WHEEL_MASS, FRONT_WHEEL_RADIUS);
// Set this flag on both wheels if you want to use axle speed instead of torque
w.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
w.InnerRadius = 0.7f * w.Radius;
w.LocalPose = new Pose(LEFT_FRONT_WHEEL_POSITION);
_leftWheel = new WheelEntity(w);
_leftWheel.State.Name = State.Name + ":" + "Left wheel";
_leftWheel.State.Assets.Mesh = _wheelMesh;
_leftWheel.Parent = this;
//_leftWheel.WheelShape.WheelState.Material = new MaterialProperties("wheel", 0.5f, 0f, 1f);
// wheels must have zero friction material.The wheel model will do friction differently
// front right wheel
w = new WheelShapeProperties("front right wheel", FRONT_WHEEL_MASS, FRONT_WHEEL_RADIUS);
w.Flags |= WheelShapeBehavior.OverrideAxleSpeed;
w.InnerRadius = 0.7f * w.Radius;
w.LocalPose = new Pose(RIGHT_FRONT_WHEEL_POSITION);
_rightWheel = new WheelEntity(w);
_rightWheel.State.Name = State.Name + ":" + "Right wheel";
_rightWheel.State.Assets.Mesh = _wheelMesh;
_rightWheel.Parent = this;
//_rightWheel.WheelShape.WheelState.Material = _leftWheel.WheelShape.WheelState.Material;
}
public virtual void RayTestSingle(ref Matrix rayFromTrans,ref Matrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
ref Matrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = new SphereShape(0.0f);
pointShape.Margin = 0f;
ConvexShape castShape = pointShape;
if (collisionShape.IsConvex())
{
// BT_PROFILE("rayTestConvex");
CastResult castResult = new CastResult();
castResult.m_fraction = resultCallback.m_closestHitFraction;
ConvexShape convexShape = (ConvexShape)collisionShape;
VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
// FIXME - MAN - convexcat here seems to make big difference to forklift.
SubSimplexConvexCast convexCaster = new SubSimplexConvexCast(castShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster = new GjkConvexCast(castShape, convexShape, simplexSolver);
//#else
//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
if (convexCaster.CalcTimeOfImpact(ref rayFromTrans,ref rayToTrans,ref colObjWorldTransform,ref colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
//if (resultCallback.m_closestHitFraction != 1f)
//{
// int ibreak = 0;
// convexCaster.calcTimeOfImpact(ref rayFromTrans, ref rayToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult);
//}
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = Vector3.TransformNormal(castResult.m_normal,rayFromTrans);
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.Normalize();
LocalRayResult localRayResult = new LocalRayResult(
collisionObject,
null,
ref castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(localRayResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
if (collisionShape.IsConcave())
{
// BT_PROFILE("rayTestConcave");
if (collisionShape.ShapeType==BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
{
///optimized version for btBvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
Matrix transform = Matrix.Identity;
BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(ref rayFromLocal,ref rayToLocal, resultCallback,collisionObject,triangleMesh,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.PerformRaycast(rcb,ref rayFromLocal,ref rayToLocal);
rcb.Cleanup();
}
else
{
//generic (slower) case
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
Matrix worldTocollisionObject = Matrix.Invert(colObjWorldTransform);
Vector3 rayFromLocal = Vector3.Transform(rayFromTrans.Translation,worldTocollisionObject);
Vector3 rayToLocal = Vector3.Transform(rayToTrans.Translation,worldTocollisionObject);
//ConvexCast::CastResult
Matrix transform = Matrix.Identity;
BridgeTriangleConcaveRaycastCallback rcb = new BridgeTriangleConcaveRaycastCallback(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape,ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
Vector3 rayAabbMinLocal = rayFromLocal;
MathUtil.VectorMin(ref rayToLocal,ref rayAabbMinLocal);
Vector3 rayAabbMaxLocal = rayFromLocal;
MathUtil.VectorMax(ref rayToLocal,ref rayAabbMaxLocal);
concaveShape.ProcessAllTriangles(rcb,ref rayAabbMinLocal,ref rayAabbMaxLocal);
rcb.Cleanup();
}
}
else
{
// BT_PROFILE("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape.IsCompound())
{
CompoundShape compoundShape = (CompoundShape)(collisionShape);
int i=0;
for (i=0;i<compoundShape.GetNumChildShapes();i++)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
Matrix childWorldTrans = MathUtil.BulletMatrixMultiply(colObjWorldTransform,childTrans) ;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape((CollisionShape)childCollisionShape);
LocalInfoAdder2 my_cb = new LocalInfoAdder2(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
RayTestSingle(ref rayFromTrans,ref rayToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
my_cb.cleanup();
}
}
}
}
}
public TriangleMeshSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// The triangle mesh could be loaded from a file, such as an XNA Model.
// In this example will use the same height field as in Sample11 and convert
// the shape into a triangle mesh.
var numberOfSamplesX = 20;
var numberOfSamplesZ = 20;
var heights = new float[numberOfSamplesX * numberOfSamplesZ];
for (int z = 0; z < numberOfSamplesZ; z++)
{
for (int x = 0; x < numberOfSamplesX; x++)
{
heights[z * numberOfSamplesX + x] = (float)(Math.Cos(z / 2f) * Math.Sin(x / 2f) * 5f + 5f);
}
}
HeightField heightField = new HeightField(0, 0, 100, 100, heights, numberOfSamplesX, numberOfSamplesZ);
// Convert the height field to a triangle mesh.
ITriangleMesh mesh = heightField.GetMesh(0.01f, 3);
// Create a shape for the triangle mesh.
TriangleMeshShape triangleMeshShape = new TriangleMeshShape(mesh);
// Optional: We can enable "contact welding". When this flag is enabled, the triangle shape
// precomputes additional internal information for the mesh. The collision detection will
// be able to compute better contacts (e.g. better normal vectors at triangle edges).
// Pro: Collision detection can compute better contact information.
// Con: Contact welding information needs a bit of memory. And the collision detection is
// a bit slower.
triangleMeshShape.EnableContactWelding = true;
// Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
// adds an additional memory overhead, but it improves collision detection speed tremendously!)
triangleMeshShape.Partition = new CompressedAabbTree
{
// 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,
};
// Optional: The partition will be automatically built when needed. For static meshes it is
// built only once when it is needed for the first time. Building the AABB tree can take a
// few seconds for very large meshes.
// By calling Update() manually we can force the partition to be built right now:
//triangleMeshShape.Partition.Update(false);
// We could also call this method in a background thread while the level is loading. Or,
// we can build the triangle mesh and the AABB tree in the XNA content pipeline and avoid the
// building of the tree at runtime (see Sample 33).
// Create a static rigid body using the shape and add it to the simulation.
// We explicitly specify a mass frame. We can use any mass frame for static bodies (because
// static bodies are effectively treated as if they have infinite mass). If we do not specify
// a mass frame in the rigid body constructor, the constructor will automatically compute an
// approximate mass frame (which can take some time for large meshes).
var ground = new RigidBody(triangleMeshShape, new MassFrame(), null)
{
Pose = new Pose(new Vector3F(-50, 0, -50f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ground);
// Distribute a few spheres and boxes across the landscape.
SphereShape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 30; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-30, 30);
position.Y = 20;
RigidBody body = new RigidBody(sphereShape)
{
Pose = new Pose(position),
};
Simulation.RigidBodies.Add(body);
}
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 30; i++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-30, 30);
position.Y = 20;
RigidBody body = new RigidBody(boxShape)
{
Pose = new Pose(position),
};
Simulation.RigidBodies.Add(body);
}
}
//SphereShape
public ISphereShapeImp AddSphereShape(float radius)
{
var btSphereShape = new SphereShape(radius);
BtCollisionShapes.Add(btSphereShape);
var retval = new SphereShapeImp();
retval.BtSphereShape = btSphereShape;
btSphereShape.UserObject = retval;
return retval;
}
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);
}
/// <summary>
/// Cleans up the pair tester.
/// </summary>
public override void CleanUp()
{
sphere = null;
Updated = false;
}
public BuoyancySample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// ----- Buoyancy Force Effect
// Buoyancy is a force effect that lets bodies swim in water. The water area is
// defined by two properties:
// - Buoyancy.AreaOfEffect defines which objects are affected.
// - Buoyancy.Surface defines the water level within this area.
// The area of effect can be defined in different ways. In this sample we will use
// a geometric object ("trigger volume").
// First, define the shape of the water area. We will create simple pool.
Shape poolShape = new BoxShape(16, 10, 16);
Vector3 poolCenter = new Vector3(0, -5, 0);
// Then create a geometric object for the water area. (A GeometricObject is required
// to position the shape in the world. A GeometricObject stores shape, scale, position,
// orientation, ...)
GeometricObject waterGeometry = new GeometricObject(poolShape, new Pose(poolCenter));
// Then create a collision object for the geometric object. (A CollisionObject required
// because the geometry should be used for collision detection with other objects.)
_waterCollisionObject = new CollisionObject(waterGeometry)
{
// Assign the object to a different collision group:
// The Grab component (see Grab.cs) uses a ray to perform hit tests. We don't want the ray
// to collide with the water. Therefore, we need to assign the water collision object to a
// different collision group. The general geometry is in collision group 0. The rays are in
// collision group 2. Add the water to collision group 1. Collision between 0 and 2 are
// enabled. Collision between 1 and 2 need to be disabled - this collision filter was set
// in PhysicsGame.cs.
CollisionGroup = 1,
// Set the type to "Trigger". This improves the performance because the collision
// detection does not need to compute detailed contact information. The collision
// detection only returns whether an objects has contact with the water.
Type = CollisionObjectType.Trigger,
};
// The collision object needs to be added into the collision domain of the simulation.
Simulation.CollisionDomain.CollisionObjects.Add(_waterCollisionObject);
// Now we can add the buoyancy effect.
Buoyancy buoyancy = new Buoyancy
{
AreaOfEffect = new GeometricAreaOfEffect(_waterCollisionObject),
Surface = new Plane(Vector3.Up, 0),
Density = 1000f, // The density of water (1000 kg/m³).
AngularDrag = 0.4f,
LinearDrag = 4f,
// Optional: Let the objects drift in the water by setting a flow velocity.
//Velocity = new Vector3(-0.5f, 0, 0.5f),
};
Simulation.ForceEffects.Add(buoyancy);
// Add static area around the pool.
RigidBody bottom = new RigidBody(new BoxShape(36, 2, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3(0, -11, 0)),
};
Simulation.RigidBodies.Add(bottom);
RigidBody left = new RigidBody(new BoxShape(10, 10, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3(-13, -5, 0)),
};
Simulation.RigidBodies.Add(left);
RigidBody right = new RigidBody(new BoxShape(10, 10, 36))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3(13, -5, 0)),
};
Simulation.RigidBodies.Add(right);
RigidBody front = new RigidBody(new BoxShape(16, 10, 10))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3(0, -5, 13)),
};
Simulation.RigidBodies.Add(front);
RigidBody back = new RigidBody(new BoxShape(16, 10, 10))
{
MotionType = MotionType.Static,
Pose = new Pose(new Vector3(0, -5, -13)),
};
Simulation.RigidBodies.Add(back);
// ----- Add some random objects to test the effect.
// Note: Objects swim if their density is less than the density of water. They sink
// if the density is greater than the density of water.
// We can define the density of objects by explicitly setting the mass.
// Add a swimming board.
BoxShape raftShape = new BoxShape(4, 0.3f, 4);
MassFrame raftMass = MassFrame.FromShapeAndDensity(raftShape, Vector3.One, 700, 0.01f, 3);
RigidBody raft = new RigidBody(raftShape, raftMass, null)
{
Pose = new Pose(new Vector3(0, 4, 0)),
};
Simulation.RigidBodies.Add(raft);
// Add some boxes on top of the swimming board.
BoxShape boxShape = new BoxShape(1, 1, 1);
MassFrame boxMass = MassFrame.FromShapeAndDensity(boxShape, Vector3.One, 700, 0.01f, 3);
for (int i = 0; i < 5; i++)
{
RigidBody box = new RigidBody(boxShape, boxMass, null)
{
Pose = new Pose(new Vector3(0, 5 + i * 1.1f, 0)),
};
Simulation.RigidBodies.Add(box);
}
// Add some "heavy stones" represented as spheres.
SphereShape stoneShape = new SphereShape(0.5f);
MassFrame stoneMass = MassFrame.FromShapeAndDensity(stoneShape, Vector3.One, 2500, 0.01f, 3);
for (int i = 0; i < 10; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-9, 9);
position.Y = 5;
RigidBody stone = new RigidBody(stoneShape, stoneMass, null)
{
Pose = new Pose(position),
};
Simulation.RigidBodies.Add(stone);
}
// Add some very light objects.
CylinderShape cylinderShape = new CylinderShape(0.3f, 1);
MassFrame cylinderMass = MassFrame.FromShapeAndDensity(cylinderShape, Vector3.One, 500, 0.01f, 3);
for (int i = 0; i < 10; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-9, 9);
position.Y = 5;
Quaternion orientation = RandomHelper.Random.NextQuaternion();
RigidBody cylinder = new RigidBody(cylinderShape, cylinderMass, null)
{
Pose = new Pose(position, orientation),
};
Simulation.RigidBodies.Add(cylinder);
}
}
public override void Build()
{
this.Demo.World.Solver = Jitter.World.SolverType.Sequential;
AddGround();
RigidBody boxb = new RigidBody(new BoxShape(7, 1, 2));
boxb.Position = new JVector(3.0f, 12, 0);
this.Demo.World.AddBody(boxb);
boxb.Tag = BodyTag.DontDrawMe;
boxb.IsStatic = true;
this.Demo.World.Solver = Jitter.World.SolverType.Sequential;
//this.Demo.World.SetDampingFactors(1.0f, 1.0f);
SphereShape shape = new SphereShape(0.501f);
for (int i = 0; i < 7; i++)
{
RigidBody body = new RigidBody(shape);
body.Position = new JVector(i, 6, 0);
DistanceConstraint dc1 = new DistanceConstraint(boxb, body, body.Position + JVector.Up * 6 + JVector.Backward * 5 + JVector.Down * 0.5f, body.Position);
dc1.Softness = 1.0f;
DistanceConstraint dc2 = new DistanceConstraint(boxb, body, body.Position + JVector.Up * 6 + JVector.Forward * 5 + JVector.Down * 0.5f, body.Position);
dc2.Softness = 1.0f;
dc1.BiasFactor = dc2.BiasFactor = 0.8f;
dc1.IsMaxDistance = dc2.IsMaxDistance = false;
this.Demo.World.AddBody(body);
this.Demo.World.AddConstraint(dc1);
this.Demo.World.AddConstraint(dc2);
body.Restitution = 1.0f;
body.StaticFriction = 1.0f;
// this.Demo.World.SetDampingFactors(1.0f, 1.0f);
}
//for (int i = 0; i < 5; i++)
//{
// RigidBody sBody = new RigidBody(new SphereShape(0.5f));
// sBody.Position = new JVector(0, 0.5f, i);
// this.Demo.World.AddBody(sBody);
// sBody.Restitution = 1.0f;
// sBody.Friction = 0.0f;
//}
//for (int i = 0; i < 3; i++)
//{
// RigidBody sBody = new RigidBody(new SphereShape(0.5f));
// sBody.Position = new JVector(0, 0.5f, 10 + i);
// this.Demo.World.AddBody(sBody);
// sBody.LinearVelocity = JVector.Forward * 3;
// sBody.Restitution = 1.0f;
// sBody.Friction = 0.0f;
//}
//this.Demo.World.SetDampingFactors(1, 1);
}
private void AddCubeOfShperes()
{
List<IRayTraceable> scanData1 = new List<IRayTraceable>();
Random rand = new Random(0);
double dist = 2;
for (int i = 0; i < 4000; i++)
{
BaseShape littleShpere = new SphereShape(new Vector3(rand.NextDouble() * dist - dist / 2, rand.NextDouble() * dist - dist / 2, rand.NextDouble() * dist - dist / 2),
rand.NextDouble() * .1 + .01,
new SolidMaterial(new RGBA_Floats(rand.NextDouble() * .5 + .5, rand.NextDouble() * .5 + .5, rand.NextDouble() * .5 + .5), 0, 0.0, 2.0));
scanData1.Add(littleShpere);
}
renderCollection.Add(BoundingVolumeHierarchy.CreateNewHierachy(scanData1));
}
private RigidBody CreateRigidBody(MMDRigid rigid, MMDModel Model,out short group, out MMDMotionState motionStateStart)
{
CollisionShape collision;
RigidBody body;
//衝突スキンの作成
switch (rigid.ShapeType)
{
case 0:
collision = new SphereShape(rigid.ShapeWidth);
break;
case 1:
collision = new BoxShape(new btVector3(rigid.ShapeWidth, rigid.ShapeHeight, rigid.ShapeDepth));
break;
case 2:
collision = new CapsuleShape(rigid.ShapeWidth, rigid.ShapeHeight);
break;
default:
throw new NotImplementedException("不明な剛体タイプ");
}
motionStateStart = new MMDMotionState(rigid, Model);
btVector3 localInertia = btVector3.Zero;
//イナーシャの計算
if (rigid.Type != 0)
collision.calculateLocalInertia(rigid.Weight, out localInertia);
//剛体を作成
body = new RigidBody((rigid.Type != 0 ? rigid.Weight : 0), motionStateStart, collision, localInertia);
//ダンピング値、摩擦、Restitutionをセット
body.setDamping(rigid.LinerDamping, rigid.AngularDamping);
body.Friction = rigid.Friction;
body.Restitution = rigid.Restitution;
//ボーン追従型はKinematicにする
if (rigid.Type == 0)
{
body.ActivationState |= ActivationStateFlags.DISABLE_DEACTIVATION;
body.CollisionFlags = CollisionFlags.CF_KINEMATIC_OBJECT;
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = false;
}
else
{//物理演算型はボーンのフラグをオンにする
if (!string.IsNullOrEmpty(rigid.RelatedBoneName))
Model.BoneManager[rigid.RelatedBoneName].IsPhysics = true;
}
//グループのフラグをセット
group = (short)Math.Pow(2, rigid.GroupIndex);
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;
}
/*
* ThreadSupportInterface CreateSolverThreadSupport(int maxNumThreads)
* {
* //#define SEQUENTIAL
* /* if (SEQUENTIAL)
* {
* SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc);
* SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci);
* threadSupport->startSPU();
* }
* else * /
*
* Win32ThreadConstructionInfo threadConstructionInfo = new Win32ThreadConstructionInfo("solverThreads",
* Win32ThreadFunc.SolverThreadFunc, Win32LSMemorySetupFunc.SolverLSMemoryFunc, maxNumThreads);
* Win32ThreadSupport threadSupport = new Win32ThreadSupport(threadConstructionInfo);
* threadSupport.StartSpu();
* return threadSupport;
* }
*/
protected override void OnInitializePhysics()
{
// 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 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;
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();
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);
}
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;
}
}
///<summary>
/// Initializes the pair tester.
///</summary>
///<param name="convex">Convex shape to use.</param>
public override void Initialize(ConvexShape convex)
{
this.sphere = (SphereShape)convex;
}
public ContentPipelineMeshSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Load triangle mesh model.
var sharedModelNode = ContentManager.Load <ModelNode>("Saucer/saucer");
// Let's create a clone because we do not want to change the shared Saucer
// instance stored in the content manager.
_modelNode = sharedModelNode.Clone();
_modelNode.PoseWorld = new Pose(new Vector3(0, 2, -5), Matrix.CreateRotationY(MathHelper.ToRadians(-30)));
_modelNode.ScaleLocal = new Vector3(8);
// The UserData contains the collision shape of type TriangleMeshShape.
TriangleMeshShape triangleMesh = (TriangleMeshShape)_modelNode.UserData;
// Add model node to graphics scene.
GraphicsScreen.Scene.Children.Add(_modelNode);
// Create rigid body.
// We explicitly specify a mass frame. We can use any mass frame for static bodies (because
// static bodies are effectively treated as if they have infinite mass). If we do not specify
// a mass frame in the rigid body constructor, the constructor will automatically compute an
// approximate mass frame (which can take some time for large meshes).
_rigidBody = new RigidBody(triangleMesh, new MassFrame(), null)
{
MotionType = MotionType.Static,
Pose = _modelNode.PoseWorld,
Scale = _modelNode.ScaleWorld,
// The PhysicsSample class should not draw the height field.
UserData = "NoDraw",
};
Simulation.RigidBodies.Add(_rigidBody);
// Distribute a few spheres and boxes across the triangle mesh.
SphereShape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 40; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-15, 10);
position.Y = RandomHelper.Random.NextFloat(20, 40);
RigidBody body = new RigidBody(sphereShape)
{
Pose = new Pose(position)
};
Simulation.RigidBodies.Add(body);
}
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 40; i++)
{
Vector3 position = RandomHelper.Random.NextVector3(-15, 10);
position.Y = RandomHelper.Random.NextFloat(20, 40);
RigidBody body = new RigidBody(boxShape)
{
Pose = new Pose(position)
};
Simulation.RigidBodies.Add(body);
}
}
public RollingSphereSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// To demonstrate the problems with triangle meshes we increase the gravity and let a
// sphere roll over a curved surface.
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity {
Acceleration = new Vector3(0, -30, 0)
}); // Higher gravity to make the problem more visible.
Simulation.ForceEffects.Add(new Damping());
// Use the custom contact filter to improve sphere contacts.
_sphereContactFilter = new SphereContactFilter();
Simulation.CollisionDomain.CollisionDetection.ContactFilter = _sphereContactFilter;
// The triangle mesh could be loaded from a file, such as an XNA Model.
// In this example will create a height field and convert the height field into a triangle mesh.
var numberOfSamplesX = 60;
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.Sin(x / 6f) * 10f + 5f);
}
}
var heightField = new HeightField(0, 0, 70, 30, samples, numberOfSamplesX, numberOfSamplesZ);
// Convert the height field to a triangle mesh.
ITriangleMesh mesh = heightField.GetMesh(0.01f, 3);
// Create a shape for the triangle mesh.
_triangleMeshShape = new TriangleMeshShape(mesh);
// Enable contact welding. And set the welding limit to 1 for maximal effect.
_triangleMeshShape.EnableContactWelding = true;
_originalWeldingLimit = TriangleMeshAlgorithm.WeldingLimit;
TriangleMeshAlgorithm.WeldingLimit = 1;
// Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
// adds an additional memory overhead, but it improves collision detection speed tremendously!)
_triangleMeshShape.Partition = new CompressedAabbTree()
{
BottomUpBuildThreshold = 0
};
// Create a static rigid body using the shape and add it to the simulation.
// We explicitly specify a mass frame. We can use any mass frame for static bodies (because
// static bodies are effectively treated as if they have infinite mass). If we do not specify
// a mass frame in the rigid body constructor, the constructor will automatically compute an
// approximate mass frame (which can take some time for large meshes).
var ground = new RigidBody(_triangleMeshShape, new MassFrame(), null)
{
Pose = new Pose(new Vector3(-34, 0, -40f)),
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(ground);
SphereShape sphereShape = new SphereShape(0.5f);
_sphere = new RigidBody(sphereShape);
Simulation.RigidBodies.Add(_sphere);
_enableSmoothMovement = true;
_timeUntilReset = TimeSpan.Zero;
}
public override void SpawnBody()
{
Model smod = TheServer.Models.GetModel(model);
if (smod == null) // TODO: smod should return a cube when all else fails?
{
// TODO: Make it safe to -> TheRegion.DespawnEntity(this);
return;
}
Model3D smodel = smod.Original;
if (smodel == null) // TODO: smodel should return a cube when all else fails?
{
// TODO: Make it safe to -> TheRegion.DespawnEntity(this);
return;
}
if (mode == ModelCollisionMode.PRECISE)
{
Shape = TheServer.Models.handler.MeshToBepu(smodel, out modelVerts); // TODO: Scale!
}
if (mode == ModelCollisionMode.CONVEXHULL)
{
Shape = TheServer.Models.handler.MeshToBepuConvex(smodel, out modelVerts); // TODO: Scale!
}
else if (mode == ModelCollisionMode.AABB)
{
List <BEPUutilities.Vector3> vecs = TheServer.Models.handler.GetCollisionVertices(smodel);
Location zero = new Location(vecs[0]);
AABB abox = new AABB()
{
Min = zero, Max = zero
};
for (int v = 1; v < vecs.Count; v++)
{
abox.Include(new Location(vecs[v]));
}
Location size = abox.Max - abox.Min;
Location center = abox.Max - size / 2;
offset = -center;
Shape = new BoxShape((double)size.X * (double)scale.X, (double)size.Y * (double)scale.Y, (double)size.Z * (double)scale.Z);
}
else
{
List <BEPUutilities.Vector3> vecs = TheServer.Models.handler.GetCollisionVertices(smodel);
double distSq = 0;
for (int v = 1; v < vecs.Count; v++)
{
if (vecs[v].LengthSquared() > distSq)
{
distSq = vecs[v].LengthSquared();
}
}
double size = Math.Sqrt(distSq);
offset = Location.Zero;
Shape = new SphereShape((double)size * (double)scale.X);
}
base.SpawnBody();
if (mode == ModelCollisionMode.PRECISE)
{
offset = InternalOffset;
}
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
IGeometricObject sphereObjectA = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObjectB = contactSet.ObjectB.GeometricObject;
SphereShape sphereShapeA = sphereObjectA.Shape as SphereShape;
SphereShape sphereShapeB = sphereObjectB.Shape as SphereShape;
// Check if collision objects are spheres.
if (sphereShapeA == null || sphereShapeB == null)
{
throw new ArgumentException("The contact set must contain sphere shapes.", "contactSet");
}
Vector3F scaleA = Vector3F.Absolute(sphereObjectA.Scale);
Vector3F scaleB = Vector3F.Absolute(sphereObjectB.Scale);
// Call MPR for non-uniformly scaled spheres.
if (scaleA.X != scaleA.Y || scaleA.Y != scaleA.Z ||
scaleB.X != scaleB.Y || scaleB.Y != scaleB.Z)
{
if (_fallbackAlgorithm == null)
{
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(ConvexShape), typeof(ConvexShape)];
}
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Apply uniform scale.
float radiusA = sphereShapeA.Radius * scaleA.X;
float radiusB = sphereShapeB.Radius * scaleB.X;
// Vector from center of A to center of B.
Vector3F centerA = sphereObjectA.Pose.Position;
Vector3F centerB = sphereObjectB.Pose.Position;
Vector3F aToB = centerB - centerA;
float lengthAToB = aToB.Length;
// Check radius of spheres.
float penetrationDepth = radiusA + radiusB - lengthAToB;
contactSet.HaveContact = penetrationDepth >= 0;
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// ----- Create contact information.
Vector3F normal;
if (Numeric.IsZero(lengthAToB))
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = Vector3F.UnitY;
}
else
{
normal = aToB.Normalized;
}
// The contact point lies in the middle of the intersecting volume.
Vector3F position = centerA + normal * (radiusA - penetrationDepth / 2);
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public static void Test()
{
var f0 = BuildHull();
f0.CollisionMargin = 0;
//Generate spheres all around the central froxel in such a way that we know that they're not colliding.
var froxelSphereSurface = new BoundingBox(new Vector3(-1.51f, -1.51f, -1.51f), new Vector3(1.51f, 1.51f, 1.51f));
int testIterations = 1000;
int innerIterations = 1000;
Random random = new Random(5);
long sphereFroxelSeparatedTicks = 0;
SphereShape sphere = new SphereShape(1);
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * t, Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel separated: {(1e6 * sphereFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Do the same kind of test, but now with intersection.
froxelSphereSurface = new BoundingBox(new Vector3(-0.5f, -0.5f, -0.5f), new Vector3(0.5f, 0.5f, 0.5f));
long sphereFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelSphereSurface, random);
float t;
ray.Intersects(ref froxelSphereSurface, out t);
var sphereTransform = new RigidTransform {
Position = ray.Position + ray.Direction * (t - 0.99f), Orientation = Quaternion.Identity
};
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, sphere, ref sphereTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
sphereFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Sphere-froxel intersecting: {(1e6 * sphereFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Create a surface for the rays to hit such that every query froxel will be just outside of the central froxel.
var froxelFroxelSurface = new BoundingBox(new Vector3(-1.01f, -1.01f, -1.01f), new Vector3(1.01f, 1.01f, 1.01f));
var queryHull = BuildHull();
queryHull.CollisionMargin = 0;
long froxelFroxelSeparatedTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no intersection!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelSeparatedTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel separated: {(1e6 * froxelFroxelSeparatedTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
//Same thing as above, but now with slight intersection.
froxelFroxelSurface = new BoundingBox(new Vector3(-.99f, -.99f, -.99f), new Vector3(0.99f, 0.99f, 0.99f));
long froxelFroxelIntersectingTicks = 0;
for (int i = 0; i < testIterations; ++i)
{
var ray = GetRandomRay(ref froxelFroxelSurface, random);
float t;
ray.Intersects(ref froxelFroxelSurface, out t);
var queryTransform = new RigidTransform(ray.Position + ray.Direction * t);
var start = Stopwatch.GetTimestamp();
for (int j = 0; j < innerIterations; ++j)
{
if (!MPRToolbox.AreLocalShapesOverlapping(f0, queryHull, ref queryTransform))
{
Trace.Fail("By construction there can be no separation!");
}
}
var end = Stopwatch.GetTimestamp();
froxelFroxelIntersectingTicks += (end - start);
}
Console.WriteLine($"Froxel-froxel intersecting: {(1e6 * froxelFroxelIntersectingTicks) / (testIterations * innerIterations * Stopwatch.Frequency)}");
}
/// <summary>
/// Create new StaticCollider from existing collision data<para/>
/// Создание нового коллайдера из существующих данных
/// </summary>
/// <param name="colmesh">Collision data<para/>Данные о коллизиях</param>
public StaticCollider(CollisionFile.Group colmesh, Vector3 position, Quaternion angles, Vector3 scale)
{
// Create base transformation matrix
// Создание базовой матрицы трансформации
Matrix4 mat =
Matrix4.CreateScale(scale) *
Matrix4.CreateFromQuaternion(angles) *
Matrix4.CreateTranslation(position);
// Create bodies
// Создание тел
List <CollisionEntry> col = new List <CollisionEntry>();
// Spheres
// Сферы
if (colmesh.Spheres != null)
{
foreach (CollisionFile.Sphere s in colmesh.Spheres)
{
// Transforming positions to world coordinates
// Трансформация расположения в мировые координаты
Vector3 pos = Vector3.TransformPosition(s.Center, mat);
float radius = Vector3.TransformVector(Vector3.UnitX * s.Radius, mat).Length;
// Create primitive
// Создание примитива
EntityShape shape = new SphereShape(radius);
col.Add(new CollisionEntry()
{
Type = PrimitiveType.Sphere,
Position = pos,
Rotation = Quaternion.Identity,
Shape = shape,
Body = new Entity(shape)
});
}
}
// Cubes
// Кубы
if (colmesh.Boxes != null)
{
foreach (CollisionFile.Box b in colmesh.Boxes)
{
// Transforming positions to world coordinates
// Трансформация расположения в мировые координаты
Vector3 pos = Vector3.TransformPosition(
new Vector3(
(b.Min.X + b.Max.X) / 2f,
(b.Min.Y + b.Max.Y) / 2f,
(b.Min.Z + b.Max.Z) / 2f
)
, mat);
float factor = Vector3.TransformVector(Vector3.UnitX, mat).Length;
// Create primitive
// Создание примитива
EntityShape shape = new BoxShape(
(float)Math.Abs(b.Max.X - b.Min.X) * factor,
(float)Math.Abs(b.Max.Y - b.Min.Y) * factor,
(float)Math.Abs(b.Max.Z - b.Min.Z) * factor
);
col.Add(new CollisionEntry()
{
Type = PrimitiveType.Box,
Position = pos,
Rotation = angles,
Shape = shape,
Body = new Entity(shape)
});
}
}
// Trimeshes
// Тримеши
if (colmesh.Meshes != null)
{
// Creating vertices array
// Создание массива вершин
BEPUutilities.Vector3[] verts = new BEPUutilities.Vector3[colmesh.Vertices.Length];
for (int i = 0; i < colmesh.Vertices.Length; i++)
{
verts[i] = new BEPUutilities.Vector3(
colmesh.Vertices[i].X,
colmesh.Vertices[i].Y,
colmesh.Vertices[i].Z
);
}
foreach (CollisionFile.Trimesh m in colmesh.Meshes)
{
// Creating affine transformation
// Создание трансформации
BEPUutilities.AffineTransform transform = new BEPUutilities.AffineTransform(
new BEPUutilities.Vector3(scale.X, scale.Y, scale.Z),
new BEPUutilities.Quaternion(angles.X, angles.Y, angles.Z, angles.W),
new BEPUutilities.Vector3(position.X, position.Y, position.Z)
);
// Create primitive
// Создание примитива
col.Add(new CollisionEntry()
{
Type = PrimitiveType.Mesh,
Mesh = new StaticMesh(verts, m.Indices, transform)
});
}
}
subColliders = col.ToArray();
}
private void CreateShapesGravity()
{
const float cubeHalfExtent = 1.5f;
const float cubeWidth = cubeHalfExtent * 2;
Vector3 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();
}
private void RestoreSphere(SphereShape sh)
{
sh.radius = this.fp1;
}
public MaterialSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
// Adjust the coefficient of restitution of the ground plane.
// (By default, the material of a rigid body is of type UniformMaterial.)
((UniformMaterial)groundPlane.Material).Restitution = 1; // Max. bounciness. (The simulation actually
// accepts higher values, but these usually
// lead to unnatural bounciness.)
Simulation.RigidBodies.Add(groundPlane);
// Add a static inclined ground plane.
RigidBody inclinedPlane = new RigidBody(new PlaneShape(new Vector3F(-0.3f, 1f, 0).Normalized, 0))
{
Name = "InclinedPlane",
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(inclinedPlane);
// Create a few boxes with different coefficient of friction.
BoxShape boxShape = new BoxShape(1, 1, 1);
for (int i = 0; i < 5; i++)
{
// Each box gets a different friction value.
UniformMaterial material = new UniformMaterial
{
DynamicFriction = i * 0.2f,
StaticFriction = i * 0.2f,
};
RigidBody box = new RigidBody(boxShape, null, material) // The second argument (the mass frame) is null. The
{ // simulation will automatically compute a default mass.
Name = "Box" + i,
Pose = new Pose(new Vector3F(5, 6, -5 + i * 2)),
};
Simulation.RigidBodies.Add(box);
}
// Create a few balls with different coefficient of restitution (= bounciness).
Shape sphereShape = new SphereShape(0.5f);
for (int i = 0; i < 6; i++)
{
// Vary restitution between 0 and 1.
UniformMaterial material = new UniformMaterial
{
Restitution = i * 0.2f
};
RigidBody body = new RigidBody(sphereShape, null, material)
{
Name = "Ball" + i,
Pose = new Pose(new Vector3F(-1 - i * 2, 5, 0)),
};
Simulation.RigidBodies.Add(body);
}
}
// 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 Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize));
hull.Points.Add(new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize));
hull.Points.Add(new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize));
hull.Points.Add(new Vector3(-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 Vector3(0, 0, 0))));
composite.Children.Add(
new GeometricObject(
new BoxShape(2 * ObjectSize, ObjectSize, ObjectSize),
new Pose(new Vector3(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 Vector3(0, 0.5f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new BoxShape(0.8f, 0.5f, 0.5f), new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-MathHelper.ToRadians(15)))));
compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
compBvh.Children.Add(new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.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 Vector3(0, 0.5f * ObjectSize, 0), Quaternion.Identity)));
comp.Children.Add(new GeometricObject(new BoxShape(0.8f * ObjectSize, 0.5f * ObjectSize, 0.5f * ObjectSize), new Pose(new Vector3(0.3f * ObjectSize, 0.7f * ObjectSize, 0), Quaternion.CreateRotationZ(-MathHelper.ToRadians(45)))));
comp.Children.Add(new GeometricObject(new SphereShape(0.3f * ObjectSize), new Pose(new Vector3(0, 1.15f * ObjectSize, 0), Quaternion.Identity)));
shape = comp;
break;
case 10:
shape = new ConvexHullOfPoints(new[]
{
new Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-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 Vector3(0, 2 * ObjectSize, 0), Matrix.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 Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, -0.3f).Normalized);
//break;
case 15:
shape = new LineSegmentShape(
new Vector3(0.1f, 0.2f, 0.3f), new Vector3(0.1f, 0.2f, 0.3f) + 3 * ObjectSize * new Vector3(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 Vector3(0.2f, 0, -0.12f), new Vector3(1, 2, 3).Normalized, ObjectSize * 2);
break;
case 22:
shape = new RayShape(new Vector3(0.2f, 0, -0.12f), new Vector3(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 Vector3(0.1f, 1, -0.2f))));
break;
case 25:
shape = new TriangleShape(
new Vector3(ObjectSize, 0, 0), new Vector3(0, ObjectSize, 0), new Vector3(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 Vector3(0, 0.5f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Matrix.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Matrix.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Matrix.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 Vector3(0, 0.5f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(
// new BoxShape(0.8f, 0.5f, 0.5f),
// new Pose(new Vector3(0.5f, 0.7f, 0), Quaternion.CreateRotationZ(-(float)MathHelper.ToRadians(15)))));
// compBvh.Children.Add(new GeometricObject(new SphereShape(0.3f), new Pose(new Vector3(0, 1.15f, 0), Quaternion.Identity)));
// compBvh.Children.Add(
// new GeometricObject(new CapsuleShape(0.2f, 1), new Pose(new Vector3(0.6f, 1.15f, 0), Quaternion.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 Vector3(-1 * ObjectSize, -2 * ObjectSize, -1 * ObjectSize),
new Vector3(2 * ObjectSize, -1 * ObjectSize, -0.5f * ObjectSize),
new Vector3(1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-1 * ObjectSize, 2 * ObjectSize, 1 * ObjectSize),
new Vector3(-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.NextVector3(-BoxSize + ObjectSize * 2, BoxSize - ObjectSize * 2),
random.NextQuaternion());
var newObject = new MovingGeometricObject
{
Pose = randomPose,
Shape = shape,
LinearVelocity = random.NextQuaternion().Rotate(new Vector3(MaxLinearVelocity, 0, 0)),
AngularVelocity = random.NextQuaternion().Rotate(Vector3.Forward)
* RandomHelper.Random.NextFloat(0, MaxAngularVelocity),
};
if (RandomHelper.Random.NextBool())
newObject.LinearVelocity = Vector3.Zero;
if (RandomHelper.Random.NextBool())
newObject.AngularVelocity = Vector3.Zero;
if (shape is LineShape || shape is HeightField)
{
// Do not move lines or the height field.
newObject.LinearVelocity = Vector3.Zero;
newObject.AngularVelocity = Vector3.Zero;
}
// Create only 1 heightField!
if (shape is HeightField)
{
if (isFirstHeightField)
{
isFirstHeightField = true;
newObject.Pose = new Pose(new Vector3(-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;
}
}
public static Vector3[] CreateSphere(SphereShape shape, out uint[] indices)
{
return(CreateSphere(shape.Radius, out indices));
}
public override void ComputeCollision(ContactSet contactSet, CollisionQueryType type)
{
// Object A should be the plane.
// Object B should be the sphere.
IGeometricObject planeObject = contactSet.ObjectA.GeometricObject;
IGeometricObject sphereObject = contactSet.ObjectB.GeometricObject;
// A should be the plane, swap objects if necessary.
bool swapped = (sphereObject.Shape is PlaneShape);
if (swapped)
{
MathHelper.Swap(ref planeObject, ref sphereObject);
}
PlaneShape planeShape = planeObject.Shape as PlaneShape;
SphereShape sphereShape = sphereObject.Shape as SphereShape;
// Check if collision object shapes are correct.
if (planeShape == null || sphereShape == null)
{
throw new ArgumentException("The contact set must contain a plane and a sphere.", "contactSet");
}
// Get scalings.
Vector3F planeScale = planeObject.Scale;
Vector3F sphereScale = Vector3F.Absolute(sphereObject.Scale);
// Call other algorithm for non-uniformly scaled spheres.
if (sphereScale.X != sphereScale.Y || sphereScale.Y != sphereScale.Z)
{
if (_fallbackAlgorithm == null)
{
_fallbackAlgorithm = CollisionDetection.AlgorithmMatrix[typeof(PlaneShape), typeof(ConvexShape)];
}
_fallbackAlgorithm.ComputeCollision(contactSet, type);
return;
}
// Get poses.
Pose planePose = planeObject.Pose;
Pose spherePose = sphereObject.Pose;
// Apply scaling to plane and transform plane to world space.
Plane plane = new Plane(planeShape);
plane.Scale(ref planeScale); // Scale plane.
plane.ToWorld(ref planePose); // Transform plane to world space.
// Calculate distance from plane to sphere surface.
float sphereRadius = sphereShape.Radius * sphereScale.X;
Vector3F sphereCenter = spherePose.Position;
float planeToSphereDistance = Vector3F.Dot(sphereCenter, plane.Normal) - sphereRadius - plane.DistanceFromOrigin;
float penetrationDepth = -planeToSphereDistance;
contactSet.HaveContact = (penetrationDepth >= 0);
if (type == CollisionQueryType.Boolean || (type == CollisionQueryType.Contacts && !contactSet.HaveContact))
{
// HaveContact queries can exit here.
// GetContacts queries can exit here if we don't have a contact.
return;
}
// Compute contact details.
Vector3F position = sphereCenter - plane.Normal * (sphereRadius - penetrationDepth / 2);
Vector3F normal = (swapped) ? -plane.Normal : plane.Normal;
// Update contact set.
Contact contact = ContactHelper.CreateContact(contactSet, position, normal, penetrationDepth, false);
ContactHelper.Merge(contactSet, contact, type, CollisionDetection.ContactPositionTolerance);
}
public ShapesSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
// Add basic force effects.
Simulation.ForceEffects.Add(new Gravity());
Simulation.ForceEffects.Add(new Damping());
// Add a ground plane.
RigidBody groundPlane = new RigidBody(new PlaneShape(Vector3F.UnitY, 0))
{
Name = "GroundPlane", // Names are not required but helpful for debugging.
MotionType = MotionType.Static,
};
Simulation.RigidBodies.Add(groundPlane);
// ----- Add a sphere.
Shape sphere = new SphereShape(0.5f);
Simulation.RigidBodies.Add(new RigidBody(sphere));
// ----- Add a box.
BoxShape box = new BoxShape(0.5f, 0.9f, 0.7f);
Simulation.RigidBodies.Add(new RigidBody(box));
// ----- Add a capsule.
CapsuleShape capsule = new CapsuleShape(0.4f, 1.2f);
Simulation.RigidBodies.Add(new RigidBody(capsule));
// ----- Add a cone.
ConeShape cone = new ConeShape(0.5f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cone));
// ----- Add a cylinder.
CylinderShape cylinder = new CylinderShape(0.3f, 1f);
Simulation.RigidBodies.Add(new RigidBody(cylinder));
// ----- Add a convex hull of random points.
ConvexHullOfPoints convexHullOfPoints = new ConvexHullOfPoints();
for (int i = 0; i < 20; i++)
{
convexHullOfPoints.Points.Add(RandomHelper.Random.NextVector3F(-0.5f, 0.5f));
}
Simulation.RigidBodies.Add(new RigidBody(convexHullOfPoints));
// ----- Add a convex polyhedron.
// (A ConvexPolyhedron is similar to the ConvexHullOfPoints. The difference is that
// the points in a ConvexHullOfPoints can be changed at runtime. A ConvexPolyhedron
// cannot be changed at runtime, but it is faster.)
List <Vector3F> points = new List <Vector3F>();
for (int i = 0; i < 20; i++)
{
points.Add(RandomHelper.Random.NextVector3F(-0.7f, 0.7f));
}
ConvexPolyhedron convexPolyhedron = new ConvexPolyhedron(points);
Simulation.RigidBodies.Add(new RigidBody(convexPolyhedron));
// ----- Add a composite shape (a table that consists of 5 boxes).
CompositeShape composite = new CompositeShape();
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, -0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(-0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(0.1f, 0.8f, 0.1f), new Pose(new Vector3F(0.75f, 0.4f, 0.5f))));
composite.Children.Add(new GeometricObject(new BoxShape(1.8f, 0.1f, 1.1f), new Pose(new Vector3F(0, 0.8f, 0))));
Simulation.RigidBodies.Add(new RigidBody(composite));
// ----- Add a convex hull of multiple shapes.
ConvexHullOfShapes convexHullOfShapes = new ConvexHullOfShapes();
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(-0.4f, 0, 0))));
convexHullOfShapes.Children.Add(new GeometricObject(new CylinderShape(0.2f, 0.8f), new Pose(new Vector3F(+0.4f, 0, 0))));
Simulation.RigidBodies.Add(new RigidBody(convexHullOfShapes));
// ----- Add the Minkowski sum of two shapes.
// (The Minkowski sum is a mathematical operation that combines two shapes.
// Here a circle is combined with a sphere. The result is a wheel.)
MinkowskiSumShape minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.2f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new CircleShape(0.5f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// Create another Minkowski sum. (Here a small sphere is added to a box to create a
// box with rounded corners.)
minkowskiSum = new MinkowskiSumShape();
minkowskiSum.ObjectA = new GeometricObject(new SphereShape(0.1f), Pose.Identity);
minkowskiSum.ObjectB = new GeometricObject(new BoxShape(0.2f, 0.5f, 0.8f), Pose.Identity);
Simulation.RigidBodies.Add(new RigidBody(minkowskiSum));
// ----- Add a triangle mesh.
// A triangle mesh could be loaded from a file or built from an XNA model.
// Here we first create a composite shape and convert the shape into a triangle
// mesh. (Any Shape in DigitalRune.Geometry can be converted to a triangle mesh.)
CompositeShape dumbbell = new CompositeShape();
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new SphereShape(0.4f), new Pose(new Vector3F(-0.6f, 0.0f, 0.0f))));
dumbbell.Children.Add(new GeometricObject(new CylinderShape(0.1f, 0.6f), new Pose(Matrix33F.CreateRotationZ(ConstantsF.PiOver2))));
TriangleMeshShape triangleMeshShape = new TriangleMeshShape(dumbbell.GetMesh(0.01f, 2));
// Optional: We can enable "contact welding". When this flag is enabled, the triangle shape
// precomputes additional internal information for the mesh. The collision detection will
// be able to compute better contacts (e.g. better normal vectors at triangle edges).
// Pro: Collision detection can compute better contact information.
// Con: Contact welding information needs a bit of memory. And the collision detection is
// a bit slower.
triangleMeshShape.EnableContactWelding = true;
// Optional: Assign a spatial partitioning scheme to the triangle mesh. (A spatial partition
// adds an additional memory overhead, but it improves collision detection speed tremendously!)
triangleMeshShape.Partition = new AabbTree <int>();
Simulation.RigidBodies.Add(new RigidBody(triangleMeshShape));
// ----- Set random positions/orientations.
// (Start with the second object. The first object is the ground plane which should
// not be changed.)
for (int i = 1; i < Simulation.RigidBodies.Count; i++)
{
RigidBody body = Simulation.RigidBodies[i];
Vector3F position = RandomHelper.Random.NextVector3F(-3, 3);
position.Y = 3; // Position the objects 3m above ground.
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
body.Pose = new Pose(position, orientation);
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new DbvtBroadphase();
Solver = new MultiBodyConstraintSolver();
World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf);
World.Gravity = new Vector3(0, -9.81f, 0);
const bool floating = false;
const bool gyro = false;
const int numLinks = 1;
const bool canSleep = false;
const bool selfCollide = false;
Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f);
Vector3 baseInertiaDiag = Vector3.Zero;
const float baseMass = 0;
multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
//multiBody.UseRK4Integration = true;
//multiBody.BaseWorldTransform = Matrix.Identity;
//init the links
Vector3 hingeJointAxis = new Vector3(1, 0, 0);
//y-axis assumed up
Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0);
Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;
for (int i = 0; i < numLinks; i++)
{
const float linkMass = 10;
Vector3 linkInertiaDiag = Vector3.Zero;
using (var shape = new SphereShape(radius))
{
shape.CalculateLocalInertia(linkMass, out linkInertiaDiag);
}
multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity,
hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
multiBody.FinalizeMultiDof();
(World as MultiBodyDynamicsWorld).AddMultiBody(multiBody);
multiBody.CanSleep = canSleep;
multiBody.HasSelfCollision = selfCollide;
multiBody.UseGyroTerm = gyro;
#if PENDULUM_DAMPING
multiBody.LinearDamping = 0.1f;
multiBody.AngularDamping = 0.9f;
#else
multiBody.LinearDamping = 0;
multiBody.AngularDamping = 0;
#endif
for (int i = 0; i < numLinks; i++)
{
var shape = new SphereShape(radius);
CollisionShapes.Add(shape);
var col = new MultiBodyLinkCollider(multiBody, i);
col.CollisionShape = shape;
const bool isDynamic = true;
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter;
World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);
multiBody.GetLink(i).Collider = col;
}
}
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(Vector3F.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 Vector3F(10, 0, 20)), heightField, MotionType.Static);
// ----- Create rubble on the floor (small random objects on the floor).
for (int i = 0; i < 60; i++)
{
Vector3F position = new Vector3F(RandomHelper.Random.NextFloat(-5, 5), 0, RandomHelper.Random.NextFloat(10, 20));
QuaternionF orientation = RandomHelper.Random.NextQuaternionF();
BoxShape shape = new BoxShape(RandomHelper.Random.NextVector3F(0.05f, 0.5f));
AddBody(simulation, "Stone" + i, new Pose(position, orientation), shape, MotionType.Static);
}
// ----- Slopes with different tilt angles.
// Create a loop.
Vector3F slopePosition = new Vector3F(-20, -0.25f, -5);
BoxShape slopeShape = new BoxShape(8, 0.5f, 2);
for (int i = 1; i < 33; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX((i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
AddBody(simulation, "Loop" + i, new Pose(slopePosition, rotation), slopeShape, MotionType.Static);
}
// Create an arched bridge.
slopePosition = new Vector3F(-10, -2, -15);
slopeShape = new BoxShape(8f, 0.5f, 5);
for (int i = 1; i < 8; i++)
{
Matrix33F oldRotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - (i - 1) * MathHelper.ToRadians(10));
Matrix33F rotation = Matrix33F.CreateRotationX(MathHelper.ToRadians(40) - i * MathHelper.ToRadians(10));
slopePosition += (oldRotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2
+ (rotation * new Vector3F(0, 0, -slopeShape.WidthZ)) / 2;
Vector3F position = slopePosition - rotation * new Vector3F(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 Vector3F(10, 1, 10))));
compositeShape.Children.Add(new GeometricObject(new SphereShape(3), new Pose(new Vector3F(15, 0, 15))));
compositeShape.Children.Add(new GeometricObject(new BoxShape(1, 2, 3), new Pose(new Vector3F(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 Vector3F(-120, 0, 20)), meshShape, MotionType.Static);
// ----- Create a seesaw.
var seesawBase = AddBody(simulation, "SeesawBase", new Pose(new Vector3F(15, 0.5f, 0)), new BoxShape(0.2f, 1, 6), MotionType.Static);
var seesaw = AddBody(simulation, "Seesaw", new Pose(new Vector3F(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 Vector3F(1.0f, 0, 0),
new Matrix33F(0, 0, -1,
0, 1, 0,
1, 0, 0)),
AnchorPoseBLocal = new Pose(new Vector3F(0, 0.5f, 0),
new Matrix33F(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++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-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++)
{
Vector3F position = RandomHelper.Random.NextVector3F(-60, 60);
position.Y = 1;
AddBody(simulation, "Box" + i, new Pose(position), boxShape, MotionType.Dynamic);
}
}
public override float CalculateTimeOfImpact(CollisionObject body0,CollisionObject body1,DispatcherInfo dispatchInfo,ManifoldResult resultOut)
{
///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
///body0.m_worldTransform,
float resultFraction = 1.0f;
float squareMot0 = (body0.GetInterpolationWorldTransform().Translation - body0.GetWorldTransform().Translation).LengthSquared();
float squareMot1 = (body1.GetInterpolationWorldTransform().Translation - body1.GetWorldTransform().Translation).LengthSquared();
if (squareMot0 < body0.GetCcdSquareMotionThreshold() &&
squareMot1 < body1.GetCcdSquareMotionThreshold())
{
return resultFraction;
}
//An adhoc way of testing the Continuous Collision Detection algorithms
//One object is approximated as a sphere, to simplify things
//Starting in penetration should report no time of impact
//For proper CCD, better accuracy and handling of 'allowed' penetration should be added
//also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
/// Convex0 against sphere for Convex1
{
ConvexShape convex0 = (ConvexShape)(body0.GetCollisionShape());
SphereShape sphere1 = new SphereShape(body1.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast( convex0 ,sphere1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction()> result.m_fraction)
{
body0.SetHitFraction( result.m_fraction );
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
/// Sphere (for convex0) against Convex1
{
ConvexShape convex1 = (ConvexShape)(body1.GetCollisionShape());
SphereShape sphere0 = new SphereShape(body0.GetCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
CastResult result = new CastResult();
VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
///Simplification, one object is simplified as a sphere
GjkConvexCast ccd1 = new GjkConvexCast(sphere0,convex1,voronoiSimplex);
//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
if (ccd1.CalcTimeOfImpact(body0.GetWorldTransform(),body0.GetInterpolationWorldTransform(),
body1.GetWorldTransform(),body1.GetInterpolationWorldTransform(),result))
{
//store result.m_fraction in both bodies
if (body0.GetHitFraction() > result.m_fraction)
{
body0.SetHitFraction( result.m_fraction);
}
if (body1.GetHitFraction() > result.m_fraction)
{
body1.SetHitFraction( result.m_fraction);
}
if (resultFraction > result.m_fraction)
{
resultFraction = result.m_fraction;
}
}
}
return resultFraction;
}
private void PopulateWorld()
{
AddSky();
AddGround();
// AddBlocks();
// Add an overhead camera
// AddCamera();
// Create and place the dominos
// SpawnIterator(CreateDominos);
// Create a LynxL6Arm Entity positioned at the origin
var robotArm = new SimulatedRobotArmEntity(RobotArmEntityName, new Vector3(0, 0, 0));
SimulationEngine.GlobalInstancePort.Insert(robotArm);
var targetProps = new SphereShapeProperties(0, new Pose(), 0.0025f);
var shape = new SphereShape(targetProps);
shape.State.DiffuseColor = new Vector4(0.1f, 0f, 1f, 1f);
_moveTargetEntity = new SingleShapeEntity(shape, new Vector3(0f, 0.2f, 0.1f));
_moveTargetEntity.State.Name = "Move To Target";
SimulationEngine.GlobalInstancePort.Insert(_moveTargetEntity);
}
