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); } }