public static bool calcRayCollision(SpatialVectorDouble rayOrigin, SpatialVectorDouble rayDirection, SpatialVectorDouble[] convexHullPlanes, out SpatialVectorDouble?normal, out double tResult, out int?faceNumber)
{
int?fnormNum, bnormNum;
RayPolyhedron.EnumReturnCode returnCode = RayPolyhedron.rayCvxPolyhedronInt(rayOrigin, rayDirection, double.PositiveInfinity, convexHullPlanes, out tResult, out normal, out fnormNum, out bnormNum);
// function returns 4 component normal but we need only 3 components
if (normal != null)
{
normal = SpatialVectorUtilities.toVector3(normal.Value);
}
if (returnCode == RayPolyhedron.EnumReturnCode.FRONTFACE)
{
faceNumber = fnormNum;
return(true);
}
else if (returnCode == RayPolyhedron.EnumReturnCode.BACKFACE)
{
faceNumber = bnormNum;
return(true);
}
else
{
faceNumber = null;
return(false);
}
}
public static FrustumPlane makeFrom4Component(SpatialVectorDouble plane)
{
FrustumPlane made = new FrustumPlane();
made.normal = SpatialVectorUtilities.toVector3(plane);
made.distance = plane.w;
return(made);
}
void calcCollisionsOfParticlesAndObjectsAndResponse()
{
// parallel computation is difficult, because objects velocity and angular velocity are manipulated
foreach (PhysicsComponent iParticle in particles)
{
bool hit;
RayHitDescriptor?rayHitDescriptor = traceRayInternal(iParticle.lastPosition, iParticle.position - iParticle.lastPosition, out hit, 1.0 /* maxT */);
if (!hit)
{
continue;
}
Debug.Assert(rayHitDescriptor != null);
CollisionInformation collisionInformation;
collisionInformation.a = iParticle;
collisionInformation.aIsParticle = true;
collisionInformation.b = rayHitDescriptor.Value.hitPhysicsComponentAndCollider.physicsComponent;
collisionInformation.globalPosition = rayHitDescriptor.Value.hitPosition;
collisionInformation.globalNormal = rayHitDescriptor.Value.hitNormal;
EnumParticleCollisionResponse particalCollisionResponse;
collisionCallbacks.instantaniousParticleContact(ref collisionInformation, out particalCollisionResponse);
PhysicsComponent hitObjectPhysicsComponent = rayHitDescriptor.Value.hitPhysicsComponentAndCollider.physicsComponent;
if (particalCollisionResponse == EnumParticleCollisionResponse.REFLECT)
{
// TODO
}
else if (particalCollisionResponse == EnumParticleCollisionResponse.ABSORB)
{
particlesToRemove.Add(iParticle); // remove the particle
// impulse depends on the relative velocity between the particle and the object and not just the velocity of the object
SpatialVectorDouble globalImpulseFromParticleRelativeToObject = calcImpulse(iParticle.velocity - hitObjectPhysicsComponent.velocity, iParticle.mass); // PHYSICS CORRECTNESS NOTE< we work with the impulse >
SpatialVectorDouble globalImpulsePosition = rayHitDescriptor.Value.hitPosition;
// transform impulse and position into object local space from global space
SpatialVectorDouble localImpulseFromParticle = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(hitObjectPhysicsComponent.calcGlobalToLocalRotationMatrix() * SpatialVectorUtilities.toVector4(globalImpulseFromParticleRelativeToObject).asMatrix));
Matrix globalToLocalRotationAndTranslationMatrix = hitObjectPhysicsComponent.calcGlobalToLocalRotationAndTranslationMatrix();
SpatialVectorDouble localImpulsePosition = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(globalToLocalRotationAndTranslationMatrix * SpatialVectorUtilities.toVector4(globalImpulsePosition).asMatrix));
// PHYSICS CORRECTNESS NOTE< we handle the impulse as a force, no idea if this is 100% right >
applyForceToLinearAndAngularVelocity(hitObjectPhysicsComponent, localImpulseFromParticle, localImpulsePosition);
}
else if (particalCollisionResponse == EnumParticleCollisionResponse.SCATTER)
{
// nothing to do
}
else if (particalCollisionResponse == EnumParticleCollisionResponse.DELETEA)
{
particlesToRemove.Add(iParticle);
}
}
}
public void transformByMatrix(Matrix matrix, MeshWithExplicitFaces mesh)
{
var positionAccessor = mesh.verticesWithAttributes.double4PositionAccessor;
bool isInvalidated = privateTransformedVertices == null || privateTransformedVertices.Length != mesh.verticesWithAttributes.numberOfVertices;
if (isInvalidated)
{
privateTransformedVertices = new SpatialVectorDouble[mesh.verticesWithAttributes.numberOfVertices];
}
for (int i = 0; i < mesh.verticesWithAttributes.numberOfVertices; i++)
{
privateTransformedVertices[i] = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(matrix * new Matrix(positionAccessor[i], 1)));
}
valid = true;
}
IList <FracturedParticle> fractureSolidWithPositionAndRotation(SpatialVectorDouble objectGlobalPosition, Quaternion objectRotation, SolidCluster.SolidWithPositionAndRotation solidWithPositionAndRotation)
{
uint roughtlyNumberOfFracturedElements = 64;
IList <FracturedParticle> fracturedParticles = SimpleFracturing.fractureSolid(solidWithPositionAndRotation.solid, roughtlyNumberOfFracturedElements);
// transform positions from local to global
foreach (FracturedParticle iFracturedParticle in fracturedParticles)
{
Matrix localToGlobalTranslation = MatrixUtilities.calcLocalToGlobalTranslationMatrix(objectGlobalPosition);
Matrix localToGlobalRotation = QuaternionUtilities.convToRotationMatrix4(objectRotation);
Matrix localToGlobal =
(localToGlobalTranslation * localToGlobalRotation) *
MatrixUtilities.calcLocalToGlobalRotationAndTranslationMatrix(solidWithPositionAndRotation.localPosition, solidWithPositionAndRotation.localRotation);
iFracturedParticle.relativePosition = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(localToGlobal * SpatialVectorUtilities.toVector4(iFracturedParticle.relativePosition).asMatrix));
}
return(fracturedParticles);
}
static void applyForceToLinearAndAngularVelocity(PhysicsComponent physicsComponent, SpatialVectorDouble localForce, SpatialVectorDouble objectLocalPositionOfForce)
{
{ // linear part
// to calculate the linear component we use the dot product
double scaleOfLinearForce = 0.0;
if (localForce.length > double.Epsilon)
{
double dotOfForceAndLocalPosition = SpatialVectorDouble.dot(localForce.normalized(), objectLocalPositionOfForce.normalized());
scaleOfLinearForce = System.Math.Abs(dotOfForceAndLocalPosition);
}
// the linear force (and resulting acceleration) is the force scaled by the dot product
Matrix rotationMatrix = physicsComponent.calcLocalToGlobalRotationMatrix();
Matrix globalForceAsMatrix = rotationMatrix * SpatialVectorUtilities.toVector4(localForce).asMatrix;
SpatialVectorDouble globalForce = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(globalForceAsMatrix));
physicsComponent.linearAcceleration += globalForce.scale(scaleOfLinearForce * physicsComponent.invMass);
}
{ // angular part
physicsComponent.eulerAngularAcceleration += physicsComponent.calcAngularAccelerationOfRigidBodyForAppliedForce(objectLocalPositionOfForce, localForce);
}
}
