// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. // This allows more customization. public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Matrix colObjWorldTransform, RayResultCallback resultCallback) { SphereShape pointShape=new SphereShape(0.0f); if (collisionShape.IsConvex) { CastResult castResult = new CastResult(); castResult.Fraction = 1f;//?? ConvexShape convexShape = collisionShape as ConvexShape; VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.Normal.LengthSquared() > 0.0001f) { castResult.Normal.Normalize(); if (castResult.Fraction < resultCallback.ClosestHitFraction) { CollisionWorld.LocalRayResult localRayResult = new LocalRayResult ( collisionObject, new LocalShapeInfo(), castResult.Normal, castResult.Fraction ); resultCallback.AddSingleResult(localRayResult); } } } else { if (collisionShape.IsConcave) { TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); rcb.HitFraction = resultCallback.ClosestHitFraction; Vector3 rayAabbMinLocal = rayFromLocal; MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); Vector3 rayAabbMaxLocal = rayFromLocal; MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape.IsCompound) { CompoundShape compoundShape = collisionShape as CompoundShape; for (int i = 0; i < compoundShape.ChildShapeCount; i++) { Matrix childTrans = compoundShape.GetChildTransform(i); CollisionShape childCollisionShape = compoundShape.GetChildShape(i); Matrix childWorldTrans = colObjWorldTransform * childTrans; RayTestSingle(rayFromTrans, rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback); } } } } } }
/// <summary> /// Creates new rigid body and adds it to the world. /// </summary> /// <param name="mass">Body mass, if 0 body is static.</param> /// <param name="startTransform">Starting body transform.</param> /// <param name="shape">Body shape.</param> /// <returns>Created rigid body.</returns> public RigidBody CreateRigidBody(float mass, Matrix startTransform, CollisionShape shape) { //rigidbody is dynamic if and only if mass is non zero, otherwise static bool isDynamic = (mass != 0.0f); Vector3 localInertia = new Vector3(); if (isDynamic) shape.CalculateLocalInertia(mass, out localInertia); //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects DefaultMotionState myMotionState = new DefaultMotionState(startTransform, Matrix.Identity); RigidBody body = new RigidBody(mass, myMotionState, shape, localInertia, 0.0f, 0.0f, 0.5f, 0.0f); _world.AddRigidBody(body); return body; }
public void AddChildShape(Matrix localTransform, CollisionShape shape) { _childTransforms.Add(localTransform); _childShapes.Add(shape); //extend the local aabbMin/aabbMax Vector3 localAabbMin, localAabbMax; shape.GetAabb(localTransform, out localAabbMin, out localAabbMax); if (_localAabbMin.X > localAabbMin.X) { _localAabbMin.X = localAabbMin.X; } if (_localAabbMax.X < localAabbMax.X) { _localAabbMax.X = localAabbMax.X; } if (_localAabbMin.Y > localAabbMin.Y) { _localAabbMin.Y = localAabbMin.Y; } if (_localAabbMax.Y < localAabbMax.Y) { _localAabbMax.Y = localAabbMax.Y; } if (_localAabbMin.Z > localAabbMin.Z) { _localAabbMin.Z = localAabbMin.Z; } if (_localAabbMax.Z < localAabbMax.Z) { _localAabbMax.Z = localAabbMax.Z; } }
// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape. // This allows more customization. public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans, CollisionObject collisionObject, CollisionShape collisionShape, Matrix colObjWorldTransform, RayResultCallback resultCallback) { SphereShape pointShape = new SphereShape(0.0f); if (collisionShape.IsConvex) { CastResult castResult = new CastResult(); castResult.Fraction = 1f; //?? ConvexShape convexShape = collisionShape as ConvexShape; VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver); //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { //add hit if (castResult.Normal.LengthSquared() > 0.0001f) { castResult.Normal.Normalize(); if (castResult.Fraction < resultCallback.ClosestHitFraction) { CollisionWorld.LocalRayResult localRayResult = new LocalRayResult ( collisionObject, new LocalShapeInfo(), castResult.Normal, castResult.Fraction ); resultCallback.AddSingleResult(localRayResult); } } } else { if (collisionShape.IsConcave) { TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape; Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform); Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject); Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject); BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); rcb.HitFraction = resultCallback.ClosestHitFraction; Vector3 rayAabbMinLocal = rayFromLocal; MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal); Vector3 rayAabbMaxLocal = rayFromLocal; MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal); triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); } else { //todo: use AABB tree or other BVH acceleration structure! if (collisionShape.IsCompound) { CompoundShape compoundShape = collisionShape as CompoundShape; for (int i = 0; i < compoundShape.ChildShapeCount; i++) { Matrix childTrans = compoundShape.GetChildTransform(i); CollisionShape childCollisionShape = compoundShape.GetChildShape(i); Matrix childWorldTrans = colObjWorldTransform * childTrans; RayTestSingle(rayFromTrans, rayToTrans, collisionObject, childCollisionShape, childWorldTrans, resultCallback); } } } } } }