/// <summary> /// Returns a Quaternion representing a rotation. /// </summary> /// <param name="axis">The axis to rotate around.</param> /// <param name="angle">The angle to rotate by.</param> /// <returns>A Quaternion representing the rotation.</returns> public static Quaternion Rotation(Vector3 axis, double angle) { double real = Math.Cos(angle / 2.0); Vector3 imaginary; //normalize first imaginary = axis.Multiply(1.0 / axis.Length()); imaginary = imaginary.Multiply(Math.Sin(angle / 2.0)); return new Quaternion(real, imaginary); }
/// <summary> /// Adjusts the contrast of a pixel. /// </summary> /// <param name="argb">The ARGB pixel to adjust.</param> /// <param name="scale">The value to scale the contrast by.</param> /// <returns>The adjusted ARGB pixel.</returns> public static int AdjustContrast(int argb, float scale) { int a = (argb >> 24) & 0xFF; int r = (argb >> 16) & 0xFF; int g = (argb >> 8) & 0xFF; int b = (argb) & 0xFF; Vector3 res = new Vector3(r, g, b); res.Multiply(1 / 255.0f); res.Subtract(0.5f); res.Multiply(scale); res.Add(0.5f); res.Multiply(255.0f); res.Clamp(0, 255); r = (int)res.X; g = (int)res.Y; b = (int)res.Z; return (a << 24) | (r << 16) | (g << 8) | b; }
public void When_Multiplying_A_Vector_With_A_Scalar_Vector_With_Result_Is_Returned() { // Arrange Vector3 vectorOne = new Vector3(1.0, 2.0, 3.0); double scalarOne = 2; // Act Vector3 result = vectorOne.Multiply(scalarOne); // Assert Assert.AreEqual(2, result.X); Assert.AreEqual(4, result.Y); Assert.AreEqual(6, result.Z); }
/// <summary> /// Detección de colisiones recursiva /// </summary> public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List <TgcBoundingBox> obstaculos, int recursionDepth) { //Limitar recursividad if (recursionDepth > 5) { return; } //Ver si la distancia a recorrer es para tener en cuenta float distanceToTravelSq = movementVector.LengthSq(); if (distanceToTravelSq < EPSILON) { return; } //Posicion deseada Vector3 originalSphereCenter = characterSphere.Center; Vector3 nextSphereCenter = originalSphereCenter + movementVector; //Buscar el punto de colision mas cercano de todos los objetos candidatos float minCollisionDistSq = float.MaxValue; Vector3 realMovementVector = movementVector; TgcBoundingBox.Face collisionFace = null; TgcBoundingBox collisionObstacle = null; Vector3 nearestPolygonIntersectionPoint = Vector3.Empty; foreach (TgcBoundingBox obstaculoBB in obstaculos) { //Obtener los polígonos que conforman las 6 caras del BoundingBox TgcBoundingBox.Face[] bbFaces = obstaculoBB.computeFaces(); foreach (TgcBoundingBox.Face bbFace in bbFaces) { Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane); TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector); float brutePlaneDist; Vector3 brutePlaneIntersectionPoint; if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint)) { continue; } float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq(); if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq) { continue; } //Obtener punto de colisión en el plano, según la normal del plano float pDist; Vector3 planeIntersectionPoint; Vector3 sphereIntersectionPoint; TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal); bool embebbed = false; bool collisionFound = false; if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //Ver si el plano está embebido en la esfera if (pDist <= characterSphere.Radius) { embebbed = true; //TODO: REVISAR ESTO, caso embebido a analizar con más detalle sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius; } //Esta fuera de la esfera else { //Obtener punto de colisión del contorno de la esfera según la normal del plano sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius); //Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector); if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //no hay colisión continue; } } //Ver si planeIntersectionPoint pertenece al polígono Vector3 newMovementVector; float newMoveDistSq; Vector3 polygonIntersectionPoint; if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace)) { if (embebbed) { //TODO: REVISAR ESTO, nunca debería pasar //throw new Exception("El polígono está dentro de la esfera"); } polygonIntersectionPoint = planeIntersectionPoint; collisionFound = true; } else { //Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint, bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]); //Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega Vector3 reversePointSeg = polygonIntersectionPoint - movementVector; if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint)) { collisionFound = true; } } if (collisionFound) { //Nuevo vector de movimiento acotado newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint; newMoveDistSq = newMovementVector.LengthSq(); if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq) { minCollisionDistSq = newMoveDistSq; realMovementVector = newMovementVector; nearestPolygonIntersectionPoint = polygonIntersectionPoint; collisionFace = bbFace; collisionObstacle = obstaculoBB; } } } } } //Si nunca hubo colisión, avanzar todo lo requerido if (collisionFace == null) { //Avanzar hasta muy cerca float movementLength = movementVector.Length(); movementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(movementVector); return; } //Solo movernos si ya no estamos muy cerca if (minCollisionDistSq >= EPSILON) { //Mover el BoundingSphere hasta casi la nueva posición real float movementLength = realMovementVector.Length(); realMovementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(realMovementVector); } //Calcular plano de Sliding Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint; Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint; slidePlaneNormal.Normalize(); Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal); //Proyectamos el punto original de destino en el plano de sliding TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal); float slideT; Vector3 slideDestinationPoint; if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint)) { //Nuevo vector de movimiento Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint; if (slideMovementVector.LengthSq() < EPSILON) { return; } //Recursividad para aplicar sliding doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1); } }
/// <summary> /// Updates the collection of supporting contacts. /// </summary> public void UpdateSupports(ref Vector3 movementDirection) { bool hadTraction = HasTraction; //Reset traction/support. HasTraction = false; HasSupport = false; Vector3 downDirection = characterBody.orientationMatrix.Down; Vector3 bodyPosition = characterBody.position; //Compute the character's radius, minus a little margin. We want the rays to originate safely within the character's body. //Assume vertical rotational invariance. Spheres, cylinders, and capsules don't have varying horizontal radii. Vector3 extremePoint; var convexShape = characterBody.CollisionInformation.Shape as ConvexShape; Debug.Assert(convexShape != null, "Character bodies must be convex."); //Find the lowest point on the collision shape. convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.DownVector, out extremePoint); BottomDistance = -extremePoint.Y + convexShape.collisionMargin; convexShape.GetLocalExtremePointWithoutMargin(ref Toolbox.RightVector, out extremePoint); float rayCastInnerRadius = Math.Max((extremePoint.X + convexShape.collisionMargin) * 0.8f, extremePoint.X); //Vertically, the rays will start at the same height as the character's center. //While they could be started lower on a cylinder, that wouldn't always work for a sphere or capsule: the origin might end up outside of the shape! tractionContacts.Clear(); supportContacts.Clear(); sideContacts.Clear(); headContacts.Clear(); foreach (var pair in characterBody.CollisionInformation.Pairs) { //Don't stand on things that aren't really colliding fully. if (pair.CollisionRule != CollisionRule.Normal) { continue; } ContactCategorizer.CategorizeContacts(pair, characterBody.CollisionInformation, ref downDirection, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts); } HasSupport = supportContacts.Count > 0; HasTraction = tractionContacts.Count > 0; //Only perform ray casts if the character has fully left the surface, and only if the previous frame had traction. //(If ray casts are allowed when support contacts still exist, the door is opened for climbing surfaces which should not be climbable. //Consider a steep slope. If the character runs at it, the character will likely be wedged off of the ground, making it lose traction while still having a support contact with the slope. //If ray tests are allowed when support contacts exist, the character will maintain traction despite climbing the wall. //The VerticalMotionConstraint can stop the character from climbing in many cases, but it's nice not to have to rely on it. //Disallowing ray tests when supports exist does have a cost, though. For example, consider rounded steps. //If the character walks off a step such that it is still in contact with the step but is far enough down that the slope is too steep for traction, //the ray test won't recover traction. This situation just isn't very common.) if (!HasSupport && hadTraction) { float supportRayLength = maximumAssistedDownStepHeight + BottomDistance; SupportRayData = null; //If the contacts aren't available to support the character, raycast down to find the ground. if (!HasTraction) { //TODO: could also require that the character has a nonzero movement direction in order to use a ray cast. Questionable- would complicate the behavior on edges. Ray ray = new Ray(bodyPosition, downDirection); bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { SupportRayData = data; HasTraction = data.HasTraction; HasSupport = true; } } //If contacts and the center ray cast failed, try a ray offset in the movement direction. bool tryingToMove = movementDirection.LengthSquared() > 0; if (!HasTraction && tryingToMove) { Ray ray = new Ray( characterBody.Position + movementDirection * rayCastInnerRadius, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = characterBody.Position; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) { SupportRayData = data; } HasSupport = true; } } } } //If contacts, center ray, AND forward ray failed to find traction, try a side ray created from down x forward. if (!HasTraction && tryingToMove) { //Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too. Vector3 horizontalOffset; Vector3.Cross(ref movementDirection, ref downDirection, out horizontalOffset); Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset); Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = bodyPosition; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) { SupportRayData = data; } HasSupport = true; } } } } //If contacts, center ray, forward ray, AND the first side ray failed to find traction, try a side ray created from forward x down. if (!HasTraction && tryingToMove) { //Compute the horizontal offset direction. Down direction and the movement direction are normalized and perpendicular, so the result is too. Vector3 horizontalOffset; Vector3.Cross(ref downDirection, ref movementDirection, out horizontalOffset); Vector3.Multiply(ref horizontalOffset, rayCastInnerRadius, out horizontalOffset); Ray ray = new Ray(bodyPosition + horizontalOffset, downDirection); //Have to test to make sure the ray doesn't get obstructed. This could happen if the character is deeply embedded in a wall; we wouldn't want it detecting things inside the wall as a support! Ray obstructionRay; obstructionRay.Position = bodyPosition; obstructionRay.Direction = ray.Position - obstructionRay.Position; if (!QueryManager.RayCastHitAnything(obstructionRay, 1)) { //The origin isn't obstructed, so now ray cast down. bool hasTraction; SupportRayData data; if (TryDownCast(ref ray, supportRayLength, out hasTraction, out data)) { if (SupportRayData == null || data.HitData.T < SupportRayData.Value.HitData.T) { //Only replace the previous support ray if we now have traction or we didn't have a support ray at all before, //or this hit is a better (sooner) hit. if (hasTraction) { SupportRayData = data; HasTraction = true; } else if (SupportRayData == null) { SupportRayData = data; } HasSupport = true; } } } } } UpdateSupportData(ref downDirection); UpdateVerticalSupportData(ref downDirection, ref movementDirection); }
/// <summary> /// Multiplies a 3-D vector by a Single value. /// </summary> /// <param name="source">Source TGCVector3.</param> /// <param name="f">Source Single value used as a multiplier.</param> /// <returns>A Vector3 structure that is multiplied by the Single value.</returns> public static TGCVector3 Multiply(TGCVector3 source, float f) { return(new TGCVector3(Vector3.Multiply(source.ToVector3(), f))); }
/// <summary> /// Gets the intersection between the convex shape and the ray. /// </summary> /// <param name="ray">Ray to test.</param> /// <param name="transform">Transform of the convex shape.</param> /// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param> /// <param name="hit">Ray hit data, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { //Put the ray into local space. Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Ray localRay; Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); //Check for containment in the cylindrical portion of the capsule. if (localRay.Position.Y >= -halfLength && localRay.Position.Y <= halfLength && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= collisionMargin * collisionMargin) { //It's inside! hit.T = 0; hit.Location = localRay.Position; hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) { Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); } else { hit.Normal = new Vector3(); } //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //Project the ray direction onto the plane where the cylinder is a circle. //The projected ray is then tested against the circle to compute the time of impact. //That time of impact is used to compute the 3d hit location. Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z); float planeDirectionLengthSquared = planeDirection.LengthSquared(); if (planeDirectionLengthSquared < Toolbox.Epsilon) { //The ray is nearly parallel with the axis. //Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside //and won't hit the sides. if (localRay.Position.Y > halfLength) { goto upperSphereTest; } if (localRay.Position.Y < -halfLength) { goto lowerSphereTest; } hit = new RayHit(); return(false); } Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z); float dot; Vector2.Dot(ref planeDirection, ref planeOrigin, out dot); float closestToCenterT = -dot / planeDirectionLengthSquared; Vector2 closestPoint; Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint); Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint); //How close does the ray come to the circle? float squaredDistance = closestPoint.LengthSquared(); if (squaredDistance > collisionMargin * collisionMargin) { //It's too far! The ray cannot possibly hit the capsule. hit = new RayHit(); return(false); } //With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis. float backwardsDistance = collisionMargin * (float)Math.Sqrt(1 - squaredDistance / (collisionMargin * collisionMargin)); float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared); hit.T = closestToCenterT - tOffset; //Compute the impact point on the infinite cylinder in 3d local space. Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location); //Is it intersecting the cylindrical portion of the capsule? if (hit.Location.Y <= halfLength && hit.Location.Y >= -halfLength && hit.T < maximumLength) { //Yup! hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) { Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); } else { hit.Normal = new Vector3(); } //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } if (hit.Location.Y < halfLength) { goto lowerSphereTest; } upperSphereTest: //Nope! It may be intersecting the ends of the capsule though. //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. var spherePosition = new Vector3(0, halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return(false); lowerSphereTest: //Okay, what about the bottom sphere? //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. spherePosition = new Vector3(0, -halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return(true); } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return(false); }
/// <summary> /// Calculates necessary information for velocity solving. /// Called by preStep(float dt) /// </summary> /// <param name="dt">Time in seconds since the last update.</param> public override void Update(float dt) { Matrix3X3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3X3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); float errorReductionParameter; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReductionParameter, out softness); //Mass Matrix Matrix3X3 k; Matrix3X3 linearComponent; Matrix3X3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct); Matrix3X3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct); if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3X3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent); Matrix3X3 angularComponentA, angularComponentB; Matrix3X3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3X3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3X3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3X3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3X3.Subtract(ref linearComponent, ref angularComponentA, out k); Matrix3X3.Subtract(ref k, ref angularComponentB, out k); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3X3.CreateScale(connectionA.inverseMass, out linearComponent); Matrix3X3 angularComponentA; Matrix3X3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3X3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3X3.Subtract(ref linearComponent, ref angularComponentA, out k); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3X3.CreateScale(connectionB.inverseMass, out linearComponent); Matrix3X3 angularComponentB; Matrix3X3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3X3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3X3.Subtract(ref linearComponent, ref angularComponentB, out k); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } k.M11 += softness; k.M22 += softness; k.M33 += softness; Matrix3X3.Invert(ref k, out massMatrix); Vector3.Add(ref connectionB.position, ref worldOffsetB, out error); Vector3.Subtract(ref error, ref connectionA.position, out error); Vector3.Subtract(ref error, ref worldOffsetA, out error); Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity); //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { Matrix3x3.Transform(ref localPlaneNormal, ref connectionA.orientationMatrix4, out worldPlaneNormal); Matrix3x3.Transform(ref localPlaneAnchor, ref connectionA.orientationMatrix4, out worldPlaneAnchor); Vector3.Add(ref worldPlaneAnchor, ref connectionA.position, out worldPlaneAnchor); Matrix3x3.Transform(ref localPointAnchor, ref connectionB.orientationMatrix4, out rB); Vector3.Add(ref rB, ref connectionB.position, out worldPointAnchor); //Find rA and rB. //So find the closest point on the plane to worldPointAnchor. float pointDistance, planeDistance; Vector3.Dot(ref worldPointAnchor, ref worldPlaneNormal, out pointDistance); Vector3.Dot(ref worldPlaneAnchor, ref worldPlaneNormal, out planeDistance); float distanceChange = planeDistance - pointDistance; Vector3 closestPointOnPlane; Vector3.Multiply(ref worldPlaneNormal, distanceChange, out closestPointOnPlane); Vector3.Add(ref closestPointOnPlane, ref worldPointAnchor, out closestPointOnPlane); Vector3.Subtract(ref closestPointOnPlane, ref connectionA.position, out rA); Vector3.Cross(ref rA, ref worldPlaneNormal, out rAcrossN); Vector3.Cross(ref rB, ref worldPlaneNormal, out rBcrossN); Vector3.Negate(ref rBcrossN, out rBcrossN); Vector3 offset; Vector3.Subtract(ref worldPointAnchor, ref closestPointOnPlane, out offset); Vector3.Dot(ref offset, ref worldPlaneNormal, out error); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); biasVelocity = MathHelper.Clamp(-errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity); if (connectionA.IsDynamic && connectionB.IsDynamic) { Vector3 IrACrossN, IrBCrossN; Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN); Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN); float angularA, angularB; Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA); Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB); negativeEffectiveMass = connectionA.inverseMass + connectionB.inverseMass + angularA + angularB; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else if (connectionA.IsDynamic && !connectionB.IsDynamic) { Vector3 IrACrossN; Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN); float angularA; Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA); negativeEffectiveMass = connectionA.inverseMass + angularA; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else if (!connectionA.IsDynamic && connectionB.IsDynamic) { Vector3 IrBCrossN; Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN); float angularB; Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB); negativeEffectiveMass = connectionB.inverseMass + angularB; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else { negativeEffectiveMass = 0; } }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //Transform local axes into world space Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix4, out worldRestrictedAxis1); Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix4, out worldRestrictedAxis2); Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix4, out worldLineAnchor); Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor); Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix4, out worldLineDirection); //Transform local Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix4, out rB); Vector3.Add(ref rB, ref connectionB.position, out worldPoint); //Find the point on the line closest to the world point. Vector3 offset; Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset); float distanceAlongAxis; Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis); Vector3 worldNearPoint; Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset); Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint); Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA); //Error Vector3 error3D; Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D); Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X); Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness); float bias = -errorReduction; biasVelocity.X = bias * error.X; biasVelocity.Y = bias * error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared; if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } //Set up the jacobians Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1); Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1); Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2); Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2); float m11 = 0, m22 = 0, m1221 = 0; float inverseMass; Vector3 intermediate; //Compute the effective mass Matrix4. if (connectionA.isDynamic) { inverseMass = connectionA.inverseMass; Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA1, out m11); m11 += inverseMass; Vector3.Dot(ref intermediate, ref angularA2, out m1221); Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA2, out m22); m22 += inverseMass; } #region Mass Matrix4 B if (connectionB.isDynamic) { float extra; inverseMass = connectionB.inverseMass; Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB1, out extra); m11 += inverseMass + extra; Vector3.Dot(ref intermediate, ref angularB2, out extra); m1221 += extra; Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB2, out extra); m22 += inverseMass + extra; } #endregion negativeEffectiveMassMatrix4.M11 = m11 + softness; negativeEffectiveMassMatrix4.M12 = m1221; negativeEffectiveMassMatrix4.M21 = m1221; negativeEffectiveMassMatrix4.M22 = m22 + softness; Matrix2.Invert(ref negativeEffectiveMassMatrix4, out negativeEffectiveMassMatrix4); Matrix2.Negate(ref negativeEffectiveMassMatrix4, out negativeEffectiveMassMatrix4); }
public void Move(SimulateMethodArgs args) { PhysicsScene scene = demo.Engine.Factory.PhysicsSceneManager.Get(args.OwnerSceneIndex); PhysicsObject objectBase = scene.Factory.PhysicsObjectManager.Get(args.OwnerIndex); if (!objectBase.Camera.Enabled) { return; } if (!objectBase.Camera.Active) { return; } float time = (float)args.Time; Vector3 deltaRotation = vectorZero; Vector3 deltaTranslation = vectorZero; float rotationSpeed = 8.0f; float translationSpeed = 8.0f; float jumpSpeed = 8.0f; float swimUpSpeed = 0.2f; float swimUpOnSurfaceSpeed = 0.06f; float translationInFluidFactor = 0.15f; float soundPositionFactor = 0.1f; bool enableJump = false; bool enableSwimUp = false; bool enableShot = false; bool cameraBodyCollision = cameraBody.IsColliding(); bool cameraDownCollision = cameraDown.IsColliding(); DemoMouseState mouseState = demo.GetMouseState(); DemoKeyboardState keyboardState = demo.GetKeyboardState(); if (mouseState[MouseButton.Right]) { deltaRotation.Y += MathHelper.DegreesToRadians(rotationSpeed * (mouseState.X - oldMouseState.X) * time); deltaRotation.X += MathHelper.DegreesToRadians(rotationSpeed * (mouseState.Y - oldMouseState.Y) * time); } mousePosition.X = mouseState.X; mousePosition.Y = mouseState.Y; if (!objectBase.Camera.EnableControl) { if (mouseState[MouseButton.Middle] && !oldMouseState[MouseButton.Middle]) { enableShot = true; } if ((keyboardState[Key.ControlRight] && !oldKeyboardState[Key.ControlRight]) || (keyboardState[Key.ControlLeft] && !oldKeyboardState[Key.ControlLeft])) { enableShot = true; } } PhysicsObject cursorA = scene.Factory.PhysicsObjectManager.Find(cursorAName); PhysicsObject cursorB = scene.Factory.PhysicsObjectManager.Find(cursorBName); if (!demo.EnableMenu) { if (cursorA != null) { cursorA.EnableDrawing = true; } if (cursorB != null) { cursorB.EnableDrawing = true; } } else { if (cursorA != null) { cursorA.EnableDrawing = false; } if (cursorB != null) { cursorB.EnableDrawing = false; } } if (!objectBase.Camera.EnableControl) { if (keyboardState[Key.W]) { deltaTranslation.Z += translationSpeed * time; } if (keyboardState[Key.S]) { deltaTranslation.Z -= translationSpeed * time; } if (keyboardState[Key.D]) { deltaTranslation.X += translationSpeed * time; } if (keyboardState[Key.A]) { deltaTranslation.X -= translationSpeed * time; } if (keyboardState[Key.Space] && !oldKeyboardState[Key.Space]) { enableJump = true; } if (keyboardState[Key.Space]) { enableSwimUp = true; } } if (keyboardState[Key.Tab] && !oldKeyboardState[Key.Tab]) { enableDistance = !enableDistance; } oldMouseState = mouseState; oldKeyboardState = keyboardState; Vector3 gravityDirection = vectorZero; scene.GetGravityDirection(ref gravityDirection); if (!objectBase.Camera.EnableControl) { if (deltaRotation.LengthSquared != 0.0f) { Vector3 euler = vectorZero; objectBase.Camera.GetEuler(ref euler); Vector3.Add(ref euler, ref deltaRotation, out euler); objectBase.Camera.SetEuler(ref euler); Matrix4 rotationX, rotationY; Matrix4.CreateRotationX(-euler.X, out rotationX); Matrix4.CreateRotationY(-euler.Y, out rotationY); Matrix4.Mult(ref rotationY, ref rotationX, out cameraRotation); objectBase.Camera.SetRotation(ref cameraRotation); Matrix4.CreateRotationY(euler.Y, out rotation); objectBase.RigidGroupOwner.MainWorldTransform.SetRotation(ref rotation); objectBase.RigidGroupOwner.RecalculateMainTransform(); } } else { Vector3 euler = vectorZero; Matrix4 objectRotation = matrixIdentity; objectBase.Camera.GetEuler(ref euler); Vector3.Add(ref euler, ref deltaRotation, out euler); objectBase.RigidGroupOwner.MainWorldTransform.GetRotation(ref objectRotation); Matrix4 rotationX, rotationY; Matrix4.CreateRotationX(euler.X, out rotationX); Matrix4.CreateRotationY(euler.Y, out rotationY); Matrix4.Mult(ref rotationX, ref rotationY, out cameraRotation); Matrix4.Mult(ref cameraRotation, ref objectRotation, out rotation); objectBase.Camera.SetEuler(ref euler); objectBase.Camera.SetTransposeRotation(ref rotation); } if (deltaTranslation.LengthSquared != 0.0f) { if (objectBase.RigidGroupOwner.IsUnderFluidSurface) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 10.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 10.0f; if (enableSwimUp) { objectBase.InitLocalTransform.GetTransposeRotation(ref rotation); Vector3.TransformVector(ref deltaTranslation, ref rotation, out direction); objectBase.MainWorldTransform.GetRotation(ref rotation); Vector3.TransformVector(ref direction, ref rotation, out moveForce); Vector3.Multiply(ref moveForce, translationInFluidFactor * objectBase.RigidGroupOwner.Integral.Mass / (time * time), out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); } else { objectBase.Camera.GetTransposeRotation(ref rotation); Vector3.TransformVector(ref deltaTranslation, ref rotation, out moveForce); Vector3.Multiply(ref moveForce, translationInFluidFactor * objectBase.RigidGroupOwner.Integral.Mass / (time * time), out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); } } else { if (cameraDownCollision) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 100000.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 100000.0f; objectBase.InitLocalTransform.GetTransposeRotation(ref rotation); Vector3.TransformVector(ref deltaTranslation, ref rotation, out direction); objectBase.MainWorldTransform.GetRotation(ref rotation); Vector3.TransformVector(ref direction, ref rotation, out moveForce); Vector3.Multiply(ref moveForce, objectBase.RigidGroupOwner.Integral.Mass / (time * time), out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); cameraDown.UpdateFeedbackForce(ref moveForce); } else { if (objectBase.RigidGroupOwner.IsInFluid) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 10.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 10.0f; objectBase.InitLocalTransform.GetTransposeRotation(ref rotation); Vector3.TransformVector(ref deltaTranslation, ref rotation, out direction); objectBase.MainWorldTransform.GetRotation(ref rotation); Vector3.TransformVector(ref direction, ref rotation, out moveForce); Vector3.Multiply(ref moveForce, translationInFluidFactor * objectBase.RigidGroupOwner.Integral.Mass / (time * time), out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); } } } } if (enableSwimUp) { if (cameraDown.IsUnderFluidSurface && cameraBody.IsUnderFluidSurface) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 10.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 10.0f; PhysicsObject fluidPhysicsObject = objectBase.RigidGroupOwner.FluidPhysicsObject; fluidPhysicsObject.InternalControllers.FluidController.GetNormal(ref fluidNormal); Vector3.Multiply(ref fluidNormal, swimUpSpeed * objectBase.RigidGroupOwner.Integral.Mass / time, out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); } else if (!cameraDownCollision && cameraBody.IsInFluid && (deltaTranslation.LengthSquared == 0.0f)) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 10.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 10.0f; PhysicsObject fluidPhysicsObject = objectBase.RigidGroupOwner.FluidPhysicsObject; fluidPhysicsObject.InternalControllers.FluidController.GetNormal(ref fluidNormal); Vector3.Multiply(ref fluidNormal, swimUpOnSurfaceSpeed * objectBase.RigidGroupOwner.Integral.Mass / time, out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); } } if (enableJump) { if (!enableControl && !objectBase.Camera.EnableControl && cameraDownCollision && !cameraDown.IsUnderFluidSurface && !cameraBody.IsUnderFluidSurface) { objectBase.RigidGroupOwner.MaxPreUpdateLinearVelocity = 100000.0f; objectBase.RigidGroupOwner.MaxPostUpdateLinearVelocity = 100000.0f; Vector3.Multiply(ref gravityDirection, -jumpSpeed * objectBase.RigidGroupOwner.Integral.Mass / time, out moveForce); objectBase.RigidGroupOwner.WorldAccumulator.AddWorldForce(ref moveForce); cameraDown.UpdateFeedbackForce(ref moveForce); } } if (enableDistance) { if (distance > maxDistance) { distance -= 2.0f; } if (enableDistanceCollision) { float margin = 1.0f; objectBase.MainWorldTransform.GetPosition(ref startPoint); objectBase.Camera.GetTransposeRotation(ref cameraRotation); direction.X = cameraRotation.Row2.X; direction.Y = cameraRotation.Row2.Y; direction.Z = cameraRotation.Row2.Z; Vector3.Multiply(ref direction, distance, out direction); Vector3.Add(ref startPoint, ref direction, out endPoint); scene.UpdatePhysicsObjectsIntersectedBySegment(ref startPoint, ref endPoint, margin, true); float minDistance = float.MaxValue; float curDistance = 0.0f; for (int i = 0; i < scene.IntersectedPhysicsObjectsCount; i++) { PhysicsObject hitObject = scene.GetIntersectedPhysicsObject(i, ref hitPoint); if (hitObject.RigidGroupOwner == objectBase.RigidGroupOwner) { continue; } //if ((hitObject.InternalControllers.FluidController != null) && hitObject.InternalControllers.FluidController.Enabled) // continue; if (!hitObject.EnableCollisions) { continue; } Vector3.Subtract(ref startPoint, ref hitPoint, out hitDistance); curDistance = hitDistance.Length; if (curDistance < minDistance) { minDistance = curDistance; } } if (minDistance < Math.Abs(distance)) { distance = -minDistance; } } } else { if (distance < 0.0f) { distance += 2.0f; } if (distance > 0.0f) { distance = 0.0f; } } if (enableDistance) { if (distance > maxDistance) { if ((cameraUp != null) && (cameraBody != null) && (cameraDown != null)) { objectBase.RigidGroupOwner.EnableDrawing = true; cameraUp.EnableDrawing = true; cameraBody.EnableDrawing = true; cameraDown.EnableDrawing = true; } } } else { if (distance >= 0.0f) { if ((cameraUp != null) && (cameraBody != null) && (cameraDown != null)) { objectBase.RigidGroupOwner.EnableDrawing = false; cameraUp.EnableDrawing = false; cameraBody.EnableDrawing = false; cameraDown.EnableDrawing = false; } } } enableControl = objectBase.Camera.EnableControl; float gravityDistance = 0.0f; Vector3 gravityLinearVelocity = vectorZero; Vector3 tangentLinearVelocity = vectorZero; Vector3 velocity = vectorZero; objectBase.MainWorldTransform.GetLinearVelocity(ref velocity); Vector3.Dot(ref gravityDirection, ref velocity, out gravityDistance); Vector3.Multiply(ref gravityDirection, gravityDistance, out gravityLinearVelocity); Vector3.Subtract(ref velocity, ref gravityLinearVelocity, out tangentLinearVelocity); float tangentLength = tangentLinearVelocity.Length; if (tangentLength > maxTangentLength) { tangentLinearVelocity *= maxTangentLength / tangentLength; } Vector3.Add(ref gravityLinearVelocity, ref tangentLinearVelocity, out velocity); objectBase.RigidGroupOwner.MainWorldTransform.SetLinearVelocity(ref velocity); objectBase.Camera.Projection.CreatePerspectiveLH(1.0f, 11000.0f, 70.0f, demo.WindowWidth, demo.WindowHeight); objectBase.MainWorldTransform.GetPosition(ref position); objectBase.Camera.GetTransposeRotation(ref cameraRotation); objectBase.Camera.View.CreateLookAtLH(ref position, ref cameraRotation, distance); objectBase.Camera.UpdateFrustum(); objectBase.Camera.View.GetViewMatrix(ref view); objectBase.Camera.Projection.GetProjectionMatrix(ref projection); Vector3 rayPosition, rayDirection; rayPosition = rayDirection = vectorZero; objectBase.UnProjectToRay(ref mousePosition, 0, 0, demo.WindowWidth, demo.WindowHeight, 0.0f, 1.0f, ref view, ref matrixIdentity, ref projection, ref rayPosition, ref rayDirection); PhysicsObject cursor = scene.Factory.PhysicsObjectManager.Find(cursorName); if (cursor != null) { Vector3 cursorPosition = vectorZero; Matrix4 cursorLocalRotation = matrixIdentity; Matrix4 cursorWorldRotation = matrixIdentity; cursor.InitLocalTransform.GetPosition(ref cursorPosition); cursor.InitLocalTransform.GetRotation(ref cursorLocalRotation); cursor.MainWorldTransform.GetRotation(ref cursorWorldRotation); objectBase.Camera.GetTransposeRotation(ref cameraRotation); Matrix4.Mult(ref cursorLocalRotation, ref cameraRotation, out rotation); Vector3.TransformVector(ref cursorPosition, ref cursorWorldRotation, out position); cursor.MainWorldTransform.SetRotation(ref rotation); Vector3.Add(ref position, ref rayPosition, out position); Vector3.Add(ref position, ref rayDirection, out position); cursor.MainWorldTransform.SetPosition(ref position); cursor.RecalculateMainTransform(); } CursorController cursorController = objectBase.InternalControllers.CursorController; if (cursorController.IsDragging) { if (cursorController.HitPhysicsObject.Integral.IsStatic && (cursorController.HitPhysicsObject.InternalControllers.HeightmapController != null) && cursorController.HitPhysicsObject.InternalControllers.HeightmapController.Enabled) { Vector3 cursorStartPosition = vectorZero; Vector3 cursorEndPosition = vectorZero; cursorController.GetAnchor1(ref cursorStartPosition); cursorController.GetAnchor2(ref cursorEndPosition); Vector3.Subtract(ref cursorEndPosition, ref cursorStartPosition, out direction); float dir = direction.Y; if (dir != 0.0f) { Vector3 scale = vectorZero; cursorController.HitPhysicsObject.MainWorldTransform.GetScale(ref scale); float positionX = cursorStartPosition.X + 0.5f * scale.X; float positionY = cursorStartPosition.Y + 0.5f * scale.Y; float positionZ = cursorStartPosition.Z + 0.5f * scale.Z; cursorController.HitPhysicsObject.InternalControllers.HeightmapController.AddHeight(positionX, positionY, positionZ, dir / scale.Y); cursorController.HitPhysicsObject.InternalControllers.HeightmapController.UpdateBounding(); // To change the friction and restitution of the heightmap surface by the cursor, add the following lines of code //cursorController.HitPhysicsObject.InternalControllers.HeightmapController.SetFriction(positionX, positionY, positionZ, 0.0f); //cursorController.HitPhysicsObject.InternalControllers.HeightmapController.SetRestitution(positionX, positionY, positionZ, 2.0f); cursorStartPosition.Y += dir; cursorController.SetAnchor1(ref cursorStartPosition); } } if (cursorController.HitPhysicsObject.Integral.IsStatic && (cursorController.HitPhysicsObject.InternalControllers.HeightmapController != null) && cursorController.HitPhysicsObject.InternalControllers.HeightmapController.Enabled) { Vector3 cursorStartPosition = vectorZero; Vector3 cursorEndPosition = vectorZero; cursorController.GetAnchor1(ref cursorStartPosition); cursorController.GetAnchor2(ref cursorEndPosition); Vector3.Subtract(ref cursorEndPosition, ref cursorStartPosition, out direction); if (direction.LengthSquared != 0.0f) { // To move the heightmap surface by the cursor, add the following lines of code //cursorController.HitPhysicsObject.MainWorldTransform.GetPosition(ref cursorStartPosition); //Vector3.Add(ref cursorStartPosition, ref direction, out cursorStartPosition); //cursorController.HitPhysicsObject.MainWorldTransform.SetPosition(ref cursorStartPosition); //cursorController.HitPhysicsObject.RecalculateMainTransform(); } } if (cursorController.HitPhysicsObject.Integral.IsStatic && (cursorController.HitPhysicsObject.InternalControllers.FluidController != null) && cursorController.HitPhysicsObject.InternalControllers.FluidController.Enabled) { Vector3 cursorStartPosition = vectorZero; Vector3 cursorEndPosition = vectorZero; cursorController.GetAnchor1(ref cursorStartPosition); cursorController.GetAnchor2(ref cursorEndPosition); Vector3.Subtract(ref cursorEndPosition, ref cursorStartPosition, out direction); if (direction.LengthSquared != 0.0f) { // To move the fluid surface by the cursor, add the following lines of code // and set EnableCursorInteraction flag to true in the Lake class //cursorController.HitPhysicsObject.MainWorldTransform.GetPosition(ref cursorStartPosition); //Vector3.Add(ref cursorStartPosition, ref direction, out cursorStartPosition); //cursorController.HitPhysicsObject.MainWorldTransform.SetPosition(ref cursorStartPosition); //cursorController.HitPhysicsObject.RecalculateMainTransform(); } } } objectBase.MainWorldTransform.GetPosition(ref listenerPosition); objectBase.Camera.GetTransposeRotation(ref rotation); Vector3.Multiply(ref listenerPosition, soundPositionFactor, out position); listenerTopDirection.X = rotation.Row1.X; listenerTopDirection.Y = rotation.Row1.Y; listenerTopDirection.Z = rotation.Row1.Z; listenerFrontDirection.X = rotation.Row2.X; listenerFrontDirection.Y = rotation.Row2.Y; listenerFrontDirection.Z = rotation.Row2.Z; listener.Position = position; listener.TopDirection = listenerTopDirection; listener.FrontDirection = listenerFrontDirection; listenerRange = objectBase.Sound.Range; if (enableShot) { Vector3 shotScale, shotColor; shotCount = (shotCount + 1) % shotTab.Length; string shotCountName = shotCount.ToString(); PhysicsObject shot = scene.Factory.PhysicsObjectManager.FindOrCreate(shotName + shotCountName); PhysicsObject shotBase = scene.Factory.PhysicsObjectManager.FindOrCreate(shotBaseName + shotCountName); PhysicsObject shotLight = scene.Factory.PhysicsObjectManager.FindOrCreate(shotLightName + shotCountName); shot.AddChildPhysicsObject(shotBase); shot.AddChildPhysicsObject(shotLight); shotTab[shotCount] = shotBase; enableShotTab = true; shotScale = shotColor = vectorZero; shotScale.X = shotScale.Y = shotScale.Z = 0.5f; Vector3.Multiply(ref rayDirection, 300.0f, out rayDirection); shot.InitLocalTransform.SetRotation(ref matrixIdentity); shot.InitLocalTransform.SetPosition(ref rayPosition); shot.InitLocalTransform.SetLinearVelocity(ref rayDirection); shot.InitLocalTransform.SetAngularVelocity(ref vectorZero); shot.MaxSimulationFrameCount = 200; shot.EnableRemovePhysicsObjectsFromManagerAfterMaxSimulationFrameCount = false; //shot.EnableLocalGravity = true; shotBase.Shape = sphere; shotBase.UserDataStr = sphereName; shotBase.InitLocalTransform.SetScale(ref shotScale); shotBase.Integral.SetDensity(10.0f); shotBase.Material.RigidGroup = true; shotBase.EnableBreakRigidGroup = false; shotBase.EnableCollisions = true; shotBase.CreateSound(true); if ((cameraUp != null) && (cameraBody != null) && (cameraDown != null)) { shotBase.DisableCollision(cameraUp, true); shotBase.DisableCollision(cameraBody, true); shotBase.DisableCollision(cameraDown, true); shotBase.MaxDisableCollisionFrameCount = 50; } shotLight.Shape = sphere; shotLight.UserDataStr = sphereName; shotLight.CreateLight(true); shotLight.Light.Type = PhysicsLightType.Point; shotLight.Light.Range = 20.0f; shotColor.X = (float)Math.Max(random.NextDouble(), random.NextDouble()); shotColor.Y = (float)Math.Max(random.NextDouble(), random.NextDouble()); shotColor.Z = (float)Math.Max(random.NextDouble(), random.NextDouble()); shotLight.Light.SetDiffuse(ref shotColor); shotLight.InitLocalTransform.SetScale(20.0f); shotLight.Material.RigidGroup = true; shotLight.Material.UserDataStr = yellowName; shotLight.EnableBreakRigidGroup = false; shotLight.EnableCollisions = false; shotLight.EnableCursorInteraction = false; shotLight.EnableAddToCameraDrawTransparentPhysicsObjects = false; scene.UpdateFromInitLocalTransform(shot); shotSoundGroup = demo.SoundGroups[hitName]; shotSoundGroup.MaxHitRepeatTime = 0.0f; if (shotSound == null) { shotSound = shotSoundGroup.GetSound(objectBase.Sound, listener, emitter); } shotSound.Update(time); Vector3.Multiply(ref rayPosition, soundPositionFactor, out shotSound.HitPosition); shotSound.HitVolume = 1.0f; shotSound.FrontDirection.X = rotation.Row2.X; shotSound.FrontDirection.Y = rotation.Row2.Y; shotSound.FrontDirection.Z = rotation.Row2.Z; demo.SoundQueue.EnqueueSound(shotSound); } else { PhysicsObject shotBase; DemoSound shotBaseSound; if (shotSound != null) { shotSound.Update(time); if (shotSound.Stop()) { shotSound.SoundGroup.SetSound(shotSound); shotSound = null; } } if (enableShotTab) { enableShotTab = false; for (int i = 0; i < shotTab.Length; i++) { shotBase = shotTab[i]; if (shotBase != null) { if (shotBase.Sound.UserDataObj != null) { enableShotTab = true; shotBaseSound = (DemoSound)shotBase.Sound.UserDataObj; shotBaseSound.Update(time); if (shotBaseSound.Stop()) { shotBaseSound.SoundGroup.SetSound(shotBaseSound); shotBase.Sound.UserDataObj = null; shotTab[i] = null; } } } } } } objectBase.Camera.UpdatePhysicsObjects(true, true, true); objectBase.Camera.SortDrawPhysicsObjects(PhysicsCameraSortOrderType.DrawPriorityShapePrimitiveType); objectBase.Camera.SortTransparentPhysicsObjects(PhysicsCameraSortOrderType.DrawPriorityShapePrimitiveType); objectBase.Camera.SortLightPhysicsObjects(PhysicsCameraSortOrderType.DrawPriorityShapePrimitiveType); PhysicsObject currentPhysicsObject; PhysicsSound currentSound; DemoSoundGroup soundGroup; DemoSound sound; if (demo.SoundQueue.SoundCount > 0) { return; } for (int i = 0; i < scene.TotalPhysicsObjectCount; i++) { if (demo.SoundQueue.SoundCount >= demo.SoundQueue.MaxSoundCount) { break; } currentPhysicsObject = scene.GetPhysicsObject(i); currentSound = !currentPhysicsObject.EnableSoundFromRigidGroupOwner ? currentPhysicsObject.Sound : currentPhysicsObject.RigidGroupOwner.Sound; if (currentSound == null) { continue; } if (!currentSound.Enabled) { continue; } if (currentSound.UserDataStr == null) { soundGroup = demo.SoundGroups[defaultName]; } else { soundGroup = demo.SoundGroups[currentSound.UserDataStr]; } if (currentPhysicsObject.IsSleeping && (currentSound.UserDataObj == null) && !soundGroup.EnableBackground) { continue; } if (currentPhysicsObject.GetSoundData(ref listenerPosition, listenerRange, soundGroup.EnableHit, soundGroup.EnableRoll, soundGroup.EnableSlide, soundGroup.EnableBackground, currentSound.BackgroundVolumeVelocityModulation, ref hitPosition, ref rollPosition, ref slidePosition, ref backgroundPosition, ref hitVolume, ref rollVolume, ref slideVolume, ref backgroundVolume)) { if (currentSound.UserDataObj == null) { sound = soundGroup.GetSound(currentSound, listener, emitter); currentSound.UserDataObj = sound; } else { sound = (DemoSound)currentSound.UserDataObj; } sound.Update(time); currentPhysicsObject.MainWorldTransform.GetTransposeRotation(ref rotation); Vector3.Multiply(ref hitPosition, soundPositionFactor, out sound.HitPosition); Vector3.Multiply(ref rollPosition, soundPositionFactor, out sound.RollPosition); Vector3.Multiply(ref slidePosition, soundPositionFactor, out sound.SlidePosition); Vector3.Multiply(ref backgroundPosition, soundPositionFactor, out sound.BackgroundPosition); sound.HitVolume = hitVolume; sound.RollVolume = rollVolume; sound.SlideVolume = slideVolume; sound.BackgroundVolume = backgroundVolume; sound.FrontDirection.X = rotation.Row2.X; sound.FrontDirection.Y = rotation.Row2.Y; sound.FrontDirection.Z = rotation.Row2.Z; demo.SoundQueue.EnqueueSound(sound); } else { if (currentSound.UserDataObj != null) { sound = (DemoSound)currentSound.UserDataObj; sound.Update(time); if (sound.Stop()) { soundGroup.SetSound(sound); currentSound.UserDataObj = null; } } } } }
/// <summary> /// Finds a supporting entity, the contact location, and the contact normal. /// </summary> /// <param name="location">Contact point between the wheel and the support.</param> /// <param name="normal">Contact normal between the wheel and the support.</param> /// <param name="suspensionLength">Length of the suspension at the contact.</param> /// <param name="supportingCollidable">Collidable supporting the wheel, if any.</param> /// <param name="entity">Supporting object.</param> /// <param name="material">Material of the wheel.</param> /// <returns>Whether or not any support was found.</returns> protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material) { suspensionLength = float.MaxValue; location = Toolbox.NoVector; supportingCollidable = null; entity = null; normal = Toolbox.NoVector; material = null; Collidable testCollidable; RayHit rayHit; bool hit = false; Quaternion localSteeringTransform; Quaternion.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform); var startingTransform = new RigidTransform { Position = wheel.suspension.worldAttachmentPoint, Orientation = Quaternion.Concatenate(Quaternion.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : Quaternion.Identity), wheel.vehicle.Body.orientation) }; Vector3 sweep; Vector3.Multiply(ref wheel.suspension.worldDirection, wheel.suspension.restLength, out sweep); for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++) { var pair = detector.CollisionInformation.pairs[i]; testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable; if (testCollidable != null) { if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal && testCollidable.ConvexCast(shape, ref startingTransform, ref sweep, out rayHit) && rayHit.T * wheel.suspension.restLength < suspensionLength) { suspensionLength = rayHit.T * wheel.suspension.restLength; EntityCollidable entityCollidable; if ((entityCollidable = testCollidable as EntityCollidable) != null) { entity = entityCollidable.Entity; material = entityCollidable.Entity.Material; } else { entity = null; supportingCollidable = testCollidable; var materialOwner = testCollidable as IMaterialOwner; if (materialOwner != null) { material = materialOwner.Material; } } location = rayHit.Location; normal = rayHit.Normal; hit = true; } } } if (hit) { if (suspensionLength > 0) { float dot; Vector3.Dot(ref normal, ref wheel.suspension.worldDirection, out dot); if (dot > 0) { //The cylinder cast produced a normal which is opposite of what we expect. Vector3.Negate(ref normal, out normal); } normal.Normalize(); } else { Vector3.Negate(ref wheel.suspension.worldDirection, out normal); } return(true); } return(false); }
/// <summary> /// Computes the center, volume, and volume distribution of a shape represented by a mesh. /// </summary> /// <param name="vertices">Vertices of the mesh.</param> /// <param name="triangleIndices">Groups of 3 indices into the vertices array which represent the triangles of the mesh.</param> /// <param name="center">Computed center of the shape's volume.</param> /// <param name="volume">Volume of the shape.</param> /// <param name="volumeDistribution">Distribution of the volume as measured from the computed center.</param> public static void ComputeShapeDistribution(IList <Vector3> vertices, IList <int> triangleIndices, out Vector3 center, out float volume, out Matrix3x3 volumeDistribution) { //Explanation for the tetrahedral integration bits: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates //http://www.scipub.org/fulltext/jms2/jms2118-11.pdf //x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3 //Looking to find inertia tensor matrix of the form // [ a -b' -c' ] // [ -b' b -a' ] // [ -c' -a' c ] float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0; Vector3 summedCenter = new Vector3(); float scaledVolume = 0; for (int i = 0; i < triangleIndices.Count; i += 3) { Vector3 v2 = vertices[triangleIndices[i]]; Vector3 v3 = vertices[triangleIndices[i + 1]]; Vector3 v4 = vertices[triangleIndices[i + 2]]; //Determinant is 6 * volume. It's signed, though; the mesh isn't necessarily convex and the origin isn't necessarily in the mesh even if it is convex. float scaledTetrahedronVolume = v2.X * (v3.Z * v4.Y - v3.Y * v4.Z) - v3.X * (v2.Z * v4.Y - v2.Y * v4.Z) + v4.X * (v2.Z * v3.Y - v2.Y * v3.Z); scaledVolume += scaledTetrahedronVolume; Vector3 tetrahedronCentroid; Vector3.Add(ref v2, ref v3, out tetrahedronCentroid); Vector3.Add(ref tetrahedronCentroid, ref v4, out tetrahedronCentroid); Vector3.Multiply(ref tetrahedronCentroid, scaledTetrahedronVolume, out tetrahedronCentroid); Vector3.Add(ref tetrahedronCentroid, ref summedCenter, out summedCenter); a += scaledTetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); b += scaledTetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); c += scaledTetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y); ao += scaledTetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z); bo += scaledTetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z); co += scaledTetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y); } if (scaledVolume < Toolbox.Epsilon) { //This function works on the assumption that there is volume. //If there is no volume, then volume * density is 0, so the mass is considered to be zero. //If the mass is zero, then a zero matrix is the consistent result. //In other words, this function shouldn't be used with things with no volume. //A special case should be used instead. volumeDistribution = new Matrix3x3(); volume = 0; center = new Vector3(); } else { Vector3.Multiply(ref summedCenter, 0.25f / scaledVolume, out center); volume = scaledVolume / 6; float scaledDensity = 1 / volume; float diagonalFactor = scaledDensity / 60; float offFactor = -scaledDensity / 120; a *= diagonalFactor; b *= diagonalFactor; c *= diagonalFactor; ao *= offFactor; bo *= offFactor; co *= offFactor; //volumeDistribution = new Matrix3x3(a, bo, co, // bo, b, ao, // co, ao, c); volumeDistribution = new Matrix3x3(a, co, bo, co, b, ao, bo, ao, c); //The volume distribution, as computed, is currently offset from the origin. //There's a operation that moves a local inertia tensor to a displaced position. //The inverse of that operation can be computed and applied to the displaced inertia to center it on the origin. Matrix3x3 additionalInertia; GetPointContribution(1, ref Toolbox.ZeroVector, ref center, out additionalInertia); Matrix3x3.Subtract(ref volumeDistribution, ref additionalInertia, out volumeDistribution); //The derivation that shows the above point mass usage is valid goes something like this, with lots of details left out: //Consider the usual form of the tensor, created from the summation of a bunch of pointmasses representing the shape. //Each sum contribution relies on a particular offset, r. When the center of mass isn't aligned with (0,0,0), //r = c + b, where c is the center of mass and b is the offset of r from the center of mass. //So, each term of the matrix (like M11 = sum(mi * (ry*ry + rz*rz))) can be rephrased in terms of the center and the offset: //M11 = sum(mi * ((cy + by) * (cy + by) + (cz + bz) * (cz + bz))) //Expanding that gets you to: //M11 = sum(mi * (cycy + 2cyby + byby + czcz + 2czbz + bzbz)) //A couple of observations move things along. //1) Since it's constant over the summation, the c terms can be pulled out of a sum. //2) sum(mi * by) and sum(mi * bz) are zero, because 'by' and 'bz' are offsets from the center of mass. In other words, if you averaged all of the offsets, it would equal (0,0,0). //(uniform density assumed) //With a little more massaging, the constant c terms can be fully separated into an additive term on each matrix element. } }
public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin; //Scan the pairs in sequence, updating the state as we go. //Touching can be set to true by a single touching subpair. Touching = false; //Containing can be set to false by a single noncontaining or nontouching subpair. Containing = true; var meshData = mesh.Shape.TriangleMesh.Data; RigidTransform mobileTriangleTransform, detectorTriangleTransform; mobileTriangleTransform.Orientation = Quaternion.Identity; detectorTriangleTransform.Orientation = Quaternion.Identity; for (int i = 0; i < meshData.Indices.Length; i += 3) { //Grab a triangle associated with the mobile mesh. meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC); RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA); RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB); RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC); Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position); Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position); Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position); Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA); Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB); Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC); //Go through all the detector volume triangles which are near the mobile mesh triangle. bool triangleTouching, triangleContaining; BoundingBox mobileBoundingBox; mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox); DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps); for (int j = 0; j < overlaps.Count; j++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC); Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position); Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position); Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f, out detectorTriangleTransform.Position); Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA); Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB); Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform)) { triangleTouching = true; //The convex can't be fully contained if it's still touching the surface. triangleContaining = false; overlaps.Clear(); goto finishTriangleTest; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established. if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps)) { triangleTouching = true; triangleContaining = true; goto finishTriangleTest; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! triangleTouching = false; triangleContaining = false; finishTriangleTest: //Analyze the results of the triangle test. if (triangleTouching) { Touching = true; //If one child is touching, then we are touching too. } else { Containing = false; //If one child isn't touching, then we aren't containing. } if (!triangleContaining) //If one child isn't containing, then we aren't containing. { Containing = false; } if (!Containing && Touching) { //If it's touching but not containing, no further pairs will change the state. //Containment has been invalidated by something that either didn't touch or wasn't contained. //Touching has been ensured by at least one object touching. break; } } //There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume. //In this case, we should be Touching, but currently we are not. if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching) { //To determine if the detector volume is fully contained, check if one of the detector mesh's vertices //are in the mobile mesh. //This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids. Vector3 vertex; DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex); Ray ray; ray.Direction = Vector3.Up; RayHit hit; RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position); if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit)) { Touching = true; } } NotifyDetectorVolumeOfChanges(); }
private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 otherPoint, out SimpleSimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new SimpleSimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN > 0) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //CAN'T BE IN A'S REGION. //CAN'T BE IN B'S REGION. //CAN'T BE IN AB'S REGION. //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = C; point = C; return(true); } //Check if it's outside AC. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = A; simplex.B = C; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return(true); } //Check if it's outside BC. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = B; simplex.B = C; float V = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, V, out point); Vector3.Add(ref point, ref B, out point); return(true); } //On the face of the triangle. float vc = AdotAB * BdotAC - BdotAB * AdotAC; simplex.A = A; simplex.B = B; simplex.C = C; simplex.State = SimplexState.Triangle; float denom = 1f / (va + vb + vc); float w = vc * denom; float v = vb * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return(true); } return(false); }
///<summary> /// Gets the closest point on the triangle to the origin. ///</summary> ///<param name="point">Closest point.</param> public void GetPointOnTriangleClosestToOrigin(out Vector3 point) { Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //CAN'T BE IN A'S REGION. //CAN'T BE IN B'S REGION. //CAN'T BE IN AB'S REGION. //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float d5, d6; Vector3.Dot(ref ab, ref C, out d5); Vector3.Dot(ref ac, ref C, out d6); d5 = -d5; d6 = -d6; if (d6 >= 0f && d5 <= d6) { //It is C! State = SimplexState.Point; A = C; point = A; return; } //Check if it's outside AC. float d1, d2; Vector3.Dot(ref ab, ref A, out d1); Vector3.Dot(ref ac, ref A, out d2); d1 = -d1; d2 = -d2; float vb = d5 * d2 - d1 * d6; if (vb <= 0f && d2 > 0f && d6 < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { //Get rid of B. Compress C into B. State = SimplexState.Segment; B = C; float V = d2 / (d2 - d6); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check if it's outside BC. float d3, d4; Vector3.Dot(ref ab, ref B, out d3); Vector3.Dot(ref ac, ref B, out d4); d3 = -d3; d4 = -d4; float va = d3 * d6 - d5 * d4; float d3d4; float d6d5; if (va <= 0f && (d3d4 = d4 - d3) > 0f && (d6d5 = d5 - d6) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Throw away A. C->A. //TODO: Does B->A, C->B work better? State = SimplexState.Segment; A = C; float U = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, U, out point); Vector3.Add(ref point, ref B, out point); return; } //On the face of the triangle. float vc = d1 * d4 - d3 * d2; float denom = 1f / (va + vb + vc); float v = vb * denom; float w = vc * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); }
protected internal override void SolveVelocityIteration() { //Compute the 'relative' linear and angular velocities. For single bone constraints, it's based entirely on the one bone's velocities! //They have to be pulled into constraint space first to compute the necessary impulse, though. Vector3 linearContributionA; Matrix3x3.TransformTranspose(ref ConnectionA.linearVelocity, ref linearJacobianA, out linearContributionA); Vector3 angularContributionA; Matrix3x3.TransformTranspose(ref ConnectionA.angularVelocity, ref angularJacobianA, out angularContributionA); Vector3 linearContributionB; Matrix3x3.TransformTranspose(ref ConnectionB.linearVelocity, ref linearJacobianB, out linearContributionB); Vector3 angularContributionB; Matrix3x3.TransformTranspose(ref ConnectionB.angularVelocity, ref angularJacobianB, out angularContributionB); //The constraint velocity error will be the velocity we try to remove. Vector3 constraintVelocityError; Vector3.Add(ref linearContributionA, ref angularContributionA, out constraintVelocityError); Vector3.Add(ref constraintVelocityError, ref linearContributionB, out constraintVelocityError); Vector3.Add(ref constraintVelocityError, ref angularContributionB, out constraintVelocityError); //However, we need to take into account two extra sources of velocities which modify our target velocity away from zero. //First, the velocity bias from position correction: Vector3.Subtract(ref constraintVelocityError, ref velocityBias, out constraintVelocityError); //And second, the bias from softness: Vector3 softnessBias; Vector3.Multiply(ref accumulatedImpulse, -softness, out softnessBias); Vector3.Subtract(ref constraintVelocityError, ref softnessBias, out constraintVelocityError); //By now, the constraint velocity error contains all the velocity we want to get rid of. //Convert it into an impulse using the effective mass matrix. Vector3 constraintSpaceImpulse; Matrix3x3.Transform(ref constraintVelocityError, ref effectiveMass, out constraintSpaceImpulse); Vector3.Negate(ref constraintSpaceImpulse, out constraintSpaceImpulse); //Add the constraint space impulse to the accumulated impulse so that warm starting and softness work properly. Vector3 preadd = accumulatedImpulse; Vector3.Add(ref constraintSpaceImpulse, ref accumulatedImpulse, out accumulatedImpulse); //But wait! The accumulated impulse may exceed this constraint's capacity! Check to make sure! Fix64 impulseSquared = accumulatedImpulse.LengthSquared(); if (impulseSquared > maximumImpulseSquared) { //Oops! Clamp that down. Vector3.Multiply(ref accumulatedImpulse, maximumImpulse / Fix64.Sqrt(impulseSquared), out accumulatedImpulse); //Update the impulse based upon the clamped accumulated impulse and the original, pre-add accumulated impulse. Vector3.Subtract(ref accumulatedImpulse, ref preadd, out constraintSpaceImpulse); } //The constraint space impulse now represents the impulse we want to apply to the bone... but in constraint space. //Bring it out to world space using the transposed jacobian. if (!ConnectionA.Pinned)//Treat pinned elements as if they have infinite inertia. { Vector3 linearImpulseA; Matrix3x3.Transform(ref constraintSpaceImpulse, ref linearJacobianA, out linearImpulseA); Vector3 angularImpulseA; Matrix3x3.Transform(ref constraintSpaceImpulse, ref angularJacobianA, out angularImpulseA); //Apply them! ConnectionA.ApplyLinearImpulse(ref linearImpulseA); ConnectionA.ApplyAngularImpulse(ref angularImpulseA); } if (!ConnectionB.Pinned)//Treat pinned elements as if they have infinite inertia. { Vector3 linearImpulseB; Matrix3x3.Transform(ref constraintSpaceImpulse, ref linearJacobianB, out linearImpulseB); Vector3 angularImpulseB; Matrix3x3.Transform(ref constraintSpaceImpulse, ref angularJacobianB, out angularImpulseB); //Apply them! ConnectionB.ApplyLinearImpulse(ref linearImpulseB); ConnectionB.ApplyAngularImpulse(ref angularImpulseB); } }
void CorrectContacts() { //Go through the contacts associated with the character. //If the contact is at the bottom of the character, regardless of its normal, take a closer look. //If the direction from the closest point on the inner cylinder to the contact position has traction //and the contact's normal does not, then replace the contact normal with the offset direction. //This is necessary because various convex pair manifolds use persistent manifolds. //Contacts in these persistent manifolds can live too long for the character to behave perfectly //when going over (usually tiny) steps. Vector3 downDirection = Body.OrientationMatrix.Down; Vector3 position = Body.Position; float margin = Body.CollisionInformation.Shape.CollisionMargin; float minimumHeight = Body.Height * .5f - margin; float coreRadius = Body.Radius - margin; float coreRadiusSquared = coreRadius * coreRadius; foreach (var pair in Body.CollisionInformation.Pairs) { foreach (var contactData in pair.Contacts) { var contact = contactData.Contact; float dot; //Check to see if the contact position is at the bottom of the character. Vector3 offset = contact.Position - Body.Position; Vector3.Dot(ref offset, ref downDirection, out dot); if (dot > minimumHeight) { //It is a 'bottom' contact! //So, compute the offset from the inner cylinder to the contact. //To do this, compute the closest point on the inner cylinder. //Since we know it's on the bottom, all we need is to compute the horizontal offset. Vector3.Dot(ref offset, ref downDirection, out dot); Vector3 horizontalOffset; Vector3.Multiply(ref downDirection, dot, out horizontalOffset); Vector3.Subtract(ref offset, ref horizontalOffset, out horizontalOffset); float length = horizontalOffset.LengthSquared(); if (length > coreRadiusSquared) { //It's beyond the edge of the cylinder; clamp it. Vector3.Multiply(ref horizontalOffset, coreRadius / (float)Math.Sqrt(length), out horizontalOffset); } //It's on the bottom, so add the bottom height. Vector3 closestPointOnCylinder; Vector3.Multiply(ref downDirection, minimumHeight, out closestPointOnCylinder); Vector3.Add(ref closestPointOnCylinder, ref horizontalOffset, out closestPointOnCylinder); Vector3.Add(ref closestPointOnCylinder, ref position, out closestPointOnCylinder); //Compute the offset from the cylinder to the offset. Vector3 offsetDirection; Vector3.Subtract(ref contact.Position, ref closestPointOnCylinder, out offsetDirection); length = offsetDirection.LengthSquared(); if (length > Toolbox.Epsilon) { //Normalize the offset. Vector3.Divide(ref offsetDirection, (float)Math.Sqrt(length), out offsetDirection); } else { continue; //If there's no offset, it's really deep and correcting this contact might be a bad idea. } Vector3.Dot(ref offsetDirection, ref downDirection, out dot); float dotOriginal; Vector3.Dot(ref contact.Normal, ref downDirection, out dotOriginal); if (dot > Math.Abs(dotOriginal)) //if the new offsetDirection normal is less steep than the original slope... { //Then use it! Vector3.Dot(ref offsetDirection, ref contact.Normal, out dot); if (dot < 0) { //Don't flip the normal relative to the contact normal. That would be bad! Vector3.Negate(ref offsetDirection, out offsetDirection); dot = -dot; } //Update the contact data using the corrected information. //The penetration depth is conservatively updated; it will be less than or equal to the 'true' depth in this direction. contact.PenetrationDepth *= dot; contact.Normal = offsetDirection; } } } } }
public void ExecuteTest(Operations operation, int innerIterations, Vector3 v1, Vector3 v2) { Vector3 res; switch (operation) { case Operations.Add_Operator: for (int i = 0; i < innerIterations; i++) { res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; res = v1 + v2; } break; case Operations.Add_Function: for (int i = 0; i < innerIterations; i++) { Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); Vector3.Add(v1, v2); } break; case Operations.Sub_Operator: for (int i = 0; i < innerIterations; i++) { res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; res = v1 - v2; } break; case Operations.Sub_Function: for (int i = 0; i < innerIterations; i++) { Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); Vector3.Subtract(v1, v2); } break; case Operations.Mul_Operator: for (int i = 0; i < innerIterations; i++) { res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; res = v1 * v2; } break; case Operations.Mul_Function: for (int i = 0; i < innerIterations; i++) { Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); Vector3.Multiply(v1, v2); } break; case Operations.Dot: for (int i = 0; i < innerIterations; i++) { Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); Vector3.Dot(v1, v2); } break; case Operations.SquareRoot: for (int i = 0; i < innerIterations; i++) { Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); Vector3.SquareRoot(v1); } break; case Operations.Length_Squared: for (int i = 0; i < innerIterations; i++) { v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); v1.LengthSquared(); } break; case Operations.Normalize: for (int i = 0; i < innerIterations; i++) { Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); Vector3.Normalize(v1); } break; case Operations.Distance_Squared: for (int i = 0; i < innerIterations; i++) { Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); Vector3.DistanceSquared(v1, v2); } break; case Operations.Cross: for (int i = 0; i < innerIterations; i++) { Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); Vector3.Cross(v1, v2); } break; } }
///<summary> /// Performs the frame's configuration step. ///</summary> ///<param name="dt">Timestep duration.</param> public override void Update(float dt) { //Transform the axes into world space. basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); Matrix3x3.Transform(ref localTwistAxisB, ref connectionB.orientationMatrix, out worldTwistAxisB); //Compute the individual swing angles. Quaternion relativeRotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisB, ref basis.primaryAxis, out relativeRotation); Vector3 axis; float angle; Toolbox.GetAxisAngleFromQuaternion(ref relativeRotation, out axis, out angle); #if !WINDOWS Vector3 axisAngle = new Vector3(); #else Vector3 axisAngle; #endif //This combined axis-angle representation is similar to angular velocity in describing a rotation. //Just like you can dot an axis with angular velocity to get a velocity around that axis, //dotting an axis with the axis-angle representation gets the angle of rotation around that axis. //(As far as the constraint is concerned, anyway.) axisAngle.X = axis.X * angle; axisAngle.Y = axis.Y * angle; axisAngle.Z = axis.Z * angle; float angleX; Vector3.Dot(ref axisAngle, ref basis.xAxis, out angleX); float angleY; Vector3.Dot(ref axisAngle, ref basis.yAxis, out angleY); //The position constraint states that the angles must be within an ellipse. The following is just a reorganization of the x^2 / a^2 + y^2 / b^2 <= 1 definition of an ellipse's area. float maxAngleXSquared = maximumAngleX * maximumAngleX; float maxAngleYSquared = maximumAngleY * maximumAngleY; error = angleX * angleX * maxAngleYSquared + angleY * angleY * maxAngleXSquared - maxAngleXSquared * maxAngleYSquared; if (error < 0) { isActiveInSolver = false; error = 0; accumulatedImpulse = 0; isLimitActive = false; return; } isLimitActive = true; //Derive the position constraint with respect to time to get the velocity constraint. //d/dt(x^2 / a^2 + y^2 / b^2) <= d/dt(1) //(2x / a^2) * d/dt(x) + (2y / b^2) * d/dt(y) <= 0 //d/dt(x) is dot(angularVelocity, xAxis). //d/dt(y) is dot(angularVelocity, yAxis). //By the scalar multiplication properties of dot products, this can be written as: //dot((2x / a^2) * xAxis, angularVelocity) + dot((2y / b^2) * yAxis, angularVelocity) <= 0 //And by the distribute property, rewrite it as: //dot((2x / a^2) * xAxis + (2y / b^2) * yAxis, angularVelocity) <= 0 //So, by inspection, the jacobian is: //(2x / a^2) * xAxis + (2y / b^2) * yAxis //[some handwaving in the above: 'angularVelocity' is actually the angular velocities of the involved entities combined. //Splitting it out fully would reveal two dot products with equivalent but negated jacobians.] //The jacobian is implemented by first considering the local values (2x / a^2) and (2y / b^2). #if !WINDOWS Vector2 tangent = new Vector2(); #else Vector2 tangent; #endif tangent.X = 2 * angleX / maxAngleXSquared; tangent.Y = 2 * angleY / maxAngleYSquared; //The tangent is then taken into world space using the basis. //Create a rotation which swings our basis 'out' to b's world orientation. Quaternion.Conjugate(ref relativeRotation, out relativeRotation); Vector3 sphereTangentX, sphereTangentY; Vector3.Transform(ref basis.xAxis, ref relativeRotation, out sphereTangentX); Vector3.Transform(ref basis.yAxis, ref relativeRotation, out sphereTangentY); Vector3.Multiply(ref sphereTangentX, tangent.X, out jacobianA); //not actually jA, just storing it there. Vector3.Multiply(ref sphereTangentY, tangent.Y, out jacobianB); //not actually jB, just storing it there. Vector3.Add(ref jacobianA, ref jacobianB, out jacobianA); jacobianB.X = -jacobianA.X; jacobianB.Y = -jacobianA.Y; jacobianB.Z = -jacobianA.Z; float errorReduction; float inverseDt = 1 / dt; springSettings.ComputeErrorReductionAndSoftness(dt, inverseDt, out errorReduction, out softness); //Compute the error correcting velocity error = error - margin; biasVelocity = MathHelper.Min(Math.Max(error, 0) * errorReduction, maxCorrectiveVelocity); if (bounciness > 0) { float relativeVelocity; float dot; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot); relativeVelocity += dot; biasVelocity = MathHelper.Max(biasVelocity, ComputeBounceVelocity(relativeVelocity)); } //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else { entryA = 0; } //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else { entryB = 0; } //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); }
///<summary> /// Updates the time of impact for the pair. ///</summary> ///<param name="requester">Collidable requesting the update.</param> ///<param name="dt">Timestep duration.</param> public override void UpdateTimeOfImpact(Collidable requester, float dt) { var overlap = BroadPhaseOverlap; var meshMode = mobileMesh.entity == null ? PositionUpdateMode.Discrete : mobileMesh.entity.PositionUpdateMode; var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode; if ( (mobileMesh.IsActive || convex.IsActive) && //At least one has to be active. ( ( convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A. meshMode == PositionUpdateMode.Continuous && overlap.entryA == requester ) || ( convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it. meshMode == PositionUpdateMode.Continuous ) ) ) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; if (convexMode == PositionUpdateMode.Discrete) { //Convex is static for the purposes of CCD. Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity); } else if (meshMode == PositionUpdateMode.Discrete) { //Mesh is static for the purposes of CCD. velocity = convex.entity.linearVelocity; } else { //Both objects can move. Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity); } Vector3.Multiply(ref velocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { TriangleSidedness sidedness = mobileMesh.Shape.Sidedness; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation); var triangle = PhysicsResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data; int triangleIndex = MeshManifold.overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC); Matrix3x3.Transform(ref triangle.vA, ref orientation, out triangle.vA); Matrix3x3.Transform(ref triangle.vB, ref orientation, out triangle.vB); Matrix3x3.Transform(ref triangle.vC, ref orientation, out triangle.vC); Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA); Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB); Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 || sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsResources.GiveBack(triangle); } } }
/// <summary> /// Calculates the force necessary to rotate the grid. Two degrees of freedom are used to rotate forward toward Direction; the remaining degree is used to face upward towards UpDirect. /// </summary> /// <param name="localMatrix">The matrix to rotate to face the direction, use a block's local matrix or result of GetMatrix()</param> /// <param name="Direction">The direction to face the localMatrix in.</param> private void in_CalcRotate(Matrix localMatrix, RelativeDirection3F Direction, RelativeDirection3F UpDirect, IMyEntity targetEntity) { Log.DebugLog("Direction == null", Logger.severity.ERROR, condition: Direction == null); m_gyro.Update(); float minimumMoment = Math.Min(m_gyro.InvertedInertiaMoment.Min(), MaxInverseTensor); if (minimumMoment <= 0f) { // == 0f, not calculated yet. < 0f, we have math failure StopRotate(); Log.DebugLog("minimumMoment < 0f", Logger.severity.FATAL, condition: minimumMoment < 0f); return; } localMatrix.M41 = 0; localMatrix.M42 = 0; localMatrix.M43 = 0; localMatrix.M44 = 1; Matrix inverted; Matrix.Invert(ref localMatrix, out inverted); localMatrix = localMatrix.GetOrientation(); inverted = inverted.GetOrientation(); Vector3 localDirect = Direction.ToLocalNormalized(); Vector3 rotBlockDirect; Vector3.Transform(ref localDirect, ref inverted, out rotBlockDirect); float azimuth, elevation; Vector3.GetAzimuthAndElevation(rotBlockDirect, out azimuth, out elevation); Vector3 rotaRight = localMatrix.Right; Vector3 rotaUp = localMatrix.Up; Vector3 NFR_right = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaRight)); Vector3 NFR_up = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaUp)); Vector3 displacement = -elevation * NFR_right - azimuth * NFR_up; if (UpDirect != null) { Vector3 upLocal = UpDirect.ToLocalNormalized(); Vector3 upRotBlock; Vector3.Transform(ref upLocal, ref inverted, out upRotBlock); upRotBlock.Z = 0f; upRotBlock.Normalize(); float roll = Math.Sign(upRotBlock.X) * (float)Math.Acos(MathHelper.Clamp(upRotBlock.Y, -1f, 1f)); Vector3 rotaBackward = localMatrix.Backward; Vector3 NFR_backward = Base6Directions.GetVector(Block.CubeBlock.LocalMatrix.GetClosestDirection(ref rotaBackward)); //Log.DebugLog("upLocal: " + upLocal + ", upRotBlock: " + upRotBlock + ", roll: " + roll + ", displacement: " + displacement + ", NFR_backward: " + NFR_backward + ", change: " + roll * NFR_backward, "in_CalcRotate()"); displacement += roll * NFR_backward; } m_lastMoveAttempt = Globals.UpdateCount; RotateCheck.TestRotate(displacement); float distanceAngle = displacement.Length(); //if (distanceAngle < m_bestAngle || float.IsNaN(NavSet.Settings_Current.DistanceAngle)) //{ // m_bestAngle = distanceAngle; // if (RotateCheck.ObstructingEntity == null) // m_lastAccel = Globals.UpdateCount; //} NavSet.Settings_Task_NavWay.DistanceAngle = distanceAngle; //Log.DebugLog("localDirect: " + localDirect + ", rotBlockDirect: " + rotBlockDirect + ", elevation: " + elevation + ", NFR_right: " + NFR_right + ", azimuth: " + azimuth + ", NFR_up: " + NFR_up + ", disp: " + displacement, "in_CalcRotate()"); m_rotateTargetVelocity = MaxAngleVelocity(displacement, minimumMoment, targetEntity != null); // adjustment to face a moving entity if (targetEntity != null) { Vector3 relativeLinearVelocity = (targetEntity.GetLinearVelocity() - LinearVelocity) * 1.1f; float distance = Vector3.Distance(targetEntity.GetCentre(), Block.CubeBlock.GetPosition()); //Log.DebugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", tangentialVelocity: " + tangentialVelocity + ", localTangVel: " + localTangVel, "in_CalcRotate()"); float RLV_pitch = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Down); float RLV_yaw = Vector3.Dot(relativeLinearVelocity, Block.CubeBlock.WorldMatrix.Right); float angl_pitch = (float)Math.Atan2(RLV_pitch, distance); float angl_yaw = (float)Math.Atan2(RLV_yaw, distance); Log.DebugLog("relativeLinearVelocity: " + relativeLinearVelocity + ", RLV_yaw: " + RLV_yaw + ", RLV_pitch: " + RLV_pitch + ", angl_yaw: " + angl_yaw + ", angl_pitch: " + angl_pitch + ", total adjustment: " + (NFR_right * angl_pitch + NFR_up * angl_yaw)); m_rotateTargetVelocity += NFR_right * angl_pitch + NFR_up * angl_yaw; } //Log.DebugLog("targetVelocity: " + m_rotateTargetVelocity, "in_CalcRotate()"); if (RotateCheck.ObstructingEntity != null) { float maxVel = (float)Math.Atan2(1d, Block.CubeGrid.LocalVolume.Radius); float lenSq = m_rotateTargetVelocity.LengthSquared(); if (lenSq > maxVel) { Log.DebugLog("Reducing target velocity from " + Math.Sqrt(lenSq) + " to " + maxVel); Vector3 normVel; Vector3.Divide(ref m_rotateTargetVelocity, (float)Math.Sqrt(lenSq), out normVel); Vector3.Multiply(ref normVel, maxVel, out m_rotateTargetVelocity); } } // angular velocity is reversed Vector3 angularVelocity = AngularVelocity.ToBlock(Block.CubeBlock); // ((DirectionWorld)(-Block.Physics.AngularVelocity)).ToBlock(Block.CubeBlock); m_rotateForceRatio = (m_rotateTargetVelocity + angularVelocity) / (minimumMoment * m_gyro.GyroForce); //Log.DebugLog("targetVelocity: " + m_rotateTargetVelocity + ", angularVelocity: " + angularVelocity + ", accel: " + (m_rotateTargetVelocity + angularVelocity)); //Log.DebugLog("minimumMoment: " + minimumMoment + ", force: " + m_gyro.GyroForce + ", rotateForceRatio: " + m_rotateForceRatio); // dampeners for (int index = 0; index < 3; index++) { // if targetVelocity is close to 0, use dampeners float target = m_rotateTargetVelocity.GetDim(index); if (target > -0.01f && target < 0.01f) { //Log.DebugLog("target near 0 for " + i + ", " + target, "in_CalcRotate()"); m_rotateTargetVelocity.SetDim(index, 0f); m_rotateForceRatio.SetDim(index, 0f); continue; } } }
/// <summary> /// Computes a point along a ray given the length along the ray from the ray position. /// </summary> /// <param name="t">Length along the ray from the ray position in terms of the ray's direction.</param> /// <param name="v">Point along the ray at the given location.</param> public void GetPointOnRay(float t, out Vector3 v) { Vector3.Multiply(ref Direction, t, out v); Vector3.Add(ref v, ref Position, out v); }
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1, //ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB, //out JVector p1, out JVector p2, out JVector normal) // { // VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); // simplexSolver.Reset(); // float lambda = 0.0f; // p1 = p2 = JVector.Zero; // JVector x1 = position1; // JVector x2 = position2; // JVector r = sweptA - sweptB; // JVector w, v; // JVector supVertexA; // JVector rn = JVector.Negate(r); // SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA); // JVector supVertexB; // SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB); // v = supVertexA - supVertexB; // bool hasResult = false; // normal = JVector.Zero; // float lastLambda = lambda; // int maxIter = MaxIterations; // float distSq = v.LengthSquared(); // float epsilon = 0.00001f; // float VdotR; // while ((distSq > epsilon) && (maxIter-- != 0)) // { // JVector vn = JVector.Negate(v); // SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA); // SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB); // w = supVertexA - supVertexB; // float VdotW = JVector.Dot(ref v, ref w); // if (VdotW > 0.0f) // { // VdotR = JVector.Dot(ref v, ref r); // if (VdotR >= -JMath.Epsilon) // { // simplexSolverPool.GiveBack(simplexSolver); // return false; // } // else // { // lambda = lambda - VdotW / VdotR; // x1 = position1 + lambda * sweptA; // x2 = position2 + lambda * sweptB; // w = supVertexA - supVertexB; // normal = v; // hasResult = true; // } // } // if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB); // if (simplexSolver.Closest(out v)) // { // distSq = v.LengthSquared(); // normal = v; // hasResult = true; // } // else distSq = 0.0f; // } // simplexSolver.ComputePoints(out p1, out p2); // if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon) // normal.Normalize(); // p1 = p1 - lambda * sweptA; // p2 = p2 - lambda * sweptB; // simplexSolverPool.GiveBack(simplexSolver); // return true; // } #endregion // see: btSubSimplexConvexCast.cpp /// <summary> /// Checks if a ray definied through it's origin and direction collides /// with a shape. /// </summary> /// <param name="support">The supportmap implementation representing the shape.</param> /// <param name="orientation">The orientation of the shape.</param> /// <param name="invOrientation">The inverse orientation of the shape.</param> /// <param name="position">The position of the shape.</param> /// <param name="origin">The origin of the ray.</param> /// <param name="direction">The direction of the ray.</param> /// <param name="fraction">The fraction which gives information where at the /// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param> /// <param name="normal">The normal from the ray collision.</param> /// <returns>Returns true if the ray hit the shape, false otherwise.</returns> public static bool Raycast(ISupportMappable support, ref Matrix3x3 orientation, ref Matrix3x3 invOrientation, ref Vector3 position, ref Vector3 origin, ref Vector3 direction, out float fraction, out Vector3 normal) { VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew(); simplexSolver.Reset(); normal = Vector3.zero; fraction = float.MaxValue; float lambda = 0.0f; Vector3 r = direction; Vector3 x = origin; Vector3 w, p, v; Vector3 arbitraryPoint; SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint); Vector3.Subtract(ref x, ref arbitraryPoint, out v); int maxIter = MaxIterations; float distSq = v.LengthSquared(); float epsilon = 0.000001f; float VdotR; while ((distSq > epsilon) && (maxIter-- != 0)) { SupportMapTransformed(support, ref orientation, ref position, ref v, out p); Vector3.Subtract(ref x, ref p, out w); float VdotW = Vector3.Dot(ref v, ref w); if (VdotW > 0.0f) { VdotR = Vector3.Dot(ref v, ref r); if (VdotR >= -Mathf.Epsilon) { simplexSolverPool.GiveBack(simplexSolver); return(false); } else { lambda = lambda - VdotW / VdotR; Vector3.Multiply(ref r, lambda, out x); Vector3.Add(ref origin, ref x, out x); Vector3.Subtract(ref x, ref p, out w); normal = v; } } if (!simplexSolver.InSimplex(w)) { simplexSolver.AddVertex(w, x, p); } if (simplexSolver.Closest(out v)) { distSq = v.LengthSquared(); } else { distSq = 0.0f; } } #region Retrieving hitPoint // Giving back the fraction like this *should* work // but is inaccurate against large objects: // fraction = lambda; Vector3 p1, p2; simplexSolver.ComputePoints(out p1, out p2); p2 = p2 - origin; fraction = p2.magnitude / direction.magnitude; #endregion if (normal.LengthSquared() > Mathf.Epsilon * Mathf.Epsilon) { normal.Normalize(); } simplexSolverPool.GiveBack(simplexSolver); return(true); }
///<summary> /// Updates the time of impact for the pair. ///</summary> ///<param name="requester">Collidable requesting the update.</param> ///<param name="dt">Timestep duration.</param> public override void UpdateTimeOfImpact(Collidable requester, Fix64 dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity); Fix64 velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = F64.C1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsThreadResources.GetTriangle(); triangle.collisionMargin = F64.C0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { MeshBoundingBoxTreeData data = instancedMesh.Shape.TriangleMesh.Data; int triangleIndex = MeshManifold.overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC); AffineTransform.Transform(ref triangle.vA, ref instancedMesh.worldTransform, out triangle.vA); AffineTransform.Transform(ref triangle.vB, ref instancedMesh.worldTransform, out triangle.vB); AffineTransform.Transform(ref triangle.vC, ref instancedMesh.worldTransform, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (instancedMesh.sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); Fix64 dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (instancedMesh.sidedness == TriangleSidedness.Counterclockwise && dot < F64.C0 || instancedMesh.sidedness == TriangleSidedness.Clockwise && dot > F64.C0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsThreadResources.GiveBack(triangle); } } }
bool TryToStepUsingContact(ref ContactData contact, out Vector3 newPosition) { Vector3 down = character.Body.OrientationMatrix.Down; Vector3 position = character.Body.Position; //The normal of the contact may not be facing perfectly out to the side. //The detection process allows a bit of slop. //Correct it by removing any component of the normal along the local up vector. Vector3 normal = contact.Normal; float dot; Vector3.Dot(ref normal, ref down, out dot); Vector3 error; Vector3.Multiply(ref down, dot, out error); Vector3.Subtract(ref normal, ref error, out normal); normal.Normalize(); //Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions. //Compute the ray origin location. Fire it out of the top of the character; if we're stepping, this must be a valid location. //Putting it as high as possible helps to reject more invalid step geometry. Ray ray; float downRayLength = character.Body.Height;// MaximumStepHeight + upStepMargin; Vector3.Multiply(ref down, character.Body.Height * .5f - downRayLength, out ray.Position); Vector3.Add(ref ray.Position, ref position, out ray.Position); ray.Direction = normal; //Include a little margin in the length. //Technically, the character only needs to teleport horizontally by the complicated commented expression. //That puts it just far enough to have traction on the new surface. //In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit, //which can cause side effects for the character. float horizontalOffsetAmount = character.Body.CollisionInformation.Shape.CollisionMargin; // (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0); float length = character.Body.Radius + horizontalOffsetAmount; // -contact.PenetrationDepth; if (character.QueryManager.RayCastHitAnything(ray, length)) { //The step is obstructed! newPosition = new Vector3(); return(false); } //The down-cast ray origin has been verified by the previous ray cast. //Let's look for a support! Vector3 horizontalOffset; Vector3.Multiply(ref normal, length, out horizontalOffset); Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position); ray.Direction = down; //Find the earliest hit, if any. RayHit earliestHit = new RayHit(); if (!character.QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit. earliestHit.T <= 0 || //Can't do anything if the hit was invalid. earliestHit.T - downRayLength > -minimumUpStepHeight || //Don't bother doing anything if the step is too small. earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall. { //No valid hit was detected. newPosition = new Vector3(); return(false); } //Ensure the candidate surface supports traction. Vector3 supportNormal; Vector3.Normalize(ref earliestHit.Normal, out supportNormal); //Calibrate the normal to face in the same direction as the down vector for consistency. Vector3.Dot(ref supportNormal, ref down, out dot); if (dot < 0) { Vector3.Negate(ref supportNormal, out supportNormal); dot = -dot; } //If the new surface does not have traction, do not attempt to step up. if (dot < character.SupportFinder.cosMaximumSlope) { newPosition = new Vector3(); return(false); } //Since contact queries are frequently expensive compared to ray cast tests, //do one more ray cast test. This time, starting from the same position, cast upwards. //In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray. Vector3.Negate(ref down, out ray.Direction); //Find the earliest hit, if any. //RayHit earliestHitUp = new RayHit(); //earliestHitUp.T = float.MaxValue; float upLength = character.Body.Height - earliestHit.T; //If the sum of the up and down distances is less than the height, the character can't fit. if (character.QueryManager.RayCastHitAnything(ray, upLength)) { newPosition = new Vector3(); return(false); } //By now, a valid ray hit has been found. Now we need to validate it using contact queries. //This process is very similar in concept to the down step verification, but it has some extra //requirements. //Predict a hit location based on the time of impact and the normal at the intersection. //Take into account the radius of the character (don't forget the collision margin!) RigidTransform transform = character.Body.CollisionInformation.WorldTransform; //The transform must be modified to position the query body at the right location. //The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support. Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset); Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position); Vector3 verticalOffset; Vector3.Multiply(ref down, -downRayLength, out verticalOffset); Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position); //We know that the closest point to the plane will be the extreme point in the plane's direction. //Use it as the ray origin. Ray downRay; character.Body.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position); downRay.Direction = down; //Intersect the ray against the plane defined by the support hit. Vector3 intersection; Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot); Plane plane = new Plane(supportNormal, dot); Vector3 candidatePosition; //Define the interval bounds to be used later. //The words 'highest' and 'lowest' here refer to the position relative to the character's body. //The ray cast points downward relative to the character's body. float highestBound = -maximumStepHeight; float lowestBound = character.Body.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T; float currentOffset = lowestBound; float hintOffset; //This guess may either win immediately, or at least give us a better idea of where to search. float hitT; if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection)) { hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration; if (hitT < highestBound) { //Don't try a location known to be too high. hitT = highestBound; } currentOffset = hitT; if (currentOffset > lowestBound) { lowestBound = currentOffset; } candidatePosition = character.Body.Position + down * currentOffset + horizontalOffset; switch (TryUpStepPosition(ref normal, ref candidatePosition, out hintOffset)) { case PositionState.Accepted: currentOffset += hintOffset; //Only use the new position location if the movement distance was the right size. if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration) { //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration. //Just clamp the overall motion and let it penetrate a bit. newPosition = character.Body.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset; return(true); } else { newPosition = new Vector3(); return(false); } case PositionState.Rejected: newPosition = new Vector3(); return(false); case PositionState.NoHit: highestBound = currentOffset + hintOffset; currentOffset = (lowestBound + currentOffset) * .5f; break; case PositionState.Obstructed: lowestBound = currentOffset; currentOffset = (highestBound + currentOffset) * .5f; break; case PositionState.HeadObstructed: highestBound = currentOffset + hintOffset; currentOffset = (lowestBound + currentOffset) * .5f; break; case PositionState.TooDeep: currentOffset += hintOffset; lowestBound = currentOffset; break; } }//TODO: If the ray cast doesn't hit, that could be used to early out... Then again, it pretty much can't happen. //Our guesses failed. //Begin the regular process. Start at the time of impact of the ray itself. //How about trying the time of impact of the ray itself? //Since we wouldn't be here unless there were no contacts at the body's current position, //testing the ray cast location gives us the second bound we need to do an informed binary search. int attempts = 0; //Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing. //The bound size check prevents the system from continuing to search a meaninglessly tiny interval. while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon) { candidatePosition = character.Body.Position + currentOffset * down + horizontalOffset; switch (TryUpStepPosition(ref normal, ref candidatePosition, out hintOffset)) { case PositionState.Accepted: currentOffset += hintOffset; //Only use the new position location if the movement distance was the right size. if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration) { //It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration. //Just clamp the overall motion and let it penetrate a bit. newPosition = character.Body.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset; return(true); } else { newPosition = new Vector3(); return(false); } case PositionState.Rejected: newPosition = new Vector3(); return(false); case PositionState.NoHit: highestBound = currentOffset + hintOffset; currentOffset = (lowestBound + highestBound) * .5f; break; case PositionState.Obstructed: lowestBound = currentOffset; currentOffset = (highestBound + lowestBound) * .5f; break; case PositionState.HeadObstructed: highestBound = currentOffset + hintOffset; currentOffset = (lowestBound + currentOffset) * .5f; break; case PositionState.TooDeep: currentOffset += hintOffset; lowestBound = currentOffset; break; } } //Couldn't find a candidate. newPosition = new Vector3(); return(false); }
/// <summary> /// Tests a ray against the entry. /// </summary> /// <param name="ray">Ray to test.</param> /// <param name="maximumLength">Maximum length, in units of the ray's direction's length, to test.</param> /// <param name="rayHit">Hit location of the ray on the entry, if any.</param> /// <returns>Whether or not the ray hit the entry.</returns> public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { //Put the ray into local space. Ray localRay; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation); Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction); Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position); Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position); if (Shape.solidity == MobileMeshSolidity.Solid) { //Find all hits. Use the count to determine the ray started inside or outside. //If it starts inside and we're in 'solid' mode, then return the ray start. //The raycast must be of infinite length at first. This allows it to determine //if it is inside or outside. if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit)) { //It was inside! rayHit = new RayHit() { Location = ray.Position, Normal = Vector3.Zero, T = 0 }; return(true); } else { if (rayHit.T < maximumLength) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal); } else { //The hit was too far away, or there was no hit (in which case T would be float.MaxValue). return(false); } return(true); } } else { //Just do a normal raycast since the object isn't solid. TriangleSidedness sidedness; switch (Shape.solidity) { case MobileMeshSolidity.Clockwise: sidedness = TriangleSidedness.Clockwise; break; case MobileMeshSolidity.Counterclockwise: sidedness = TriangleSidedness.Counterclockwise; break; default: sidedness = TriangleSidedness.DoubleSided; break; } if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit)) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal); return(true); } } rayHit = new RayHit(); return(false); }
///<summary> /// Tests a ray against the terrain shape. ///</summary> ///<param name="ray">Ray to test against the shape.</param> ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param> ///<param name="transform">Transform to apply to the terrain shape during the test.</param> ///<param name="sidedness">Sidedness of the triangles to use when raycasting.</param> ///<param name="hit">Hit data of the ray cast, if any.</param> ///<returns>Whether or not the ray hit the transformed terrain shape.</returns> public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit) { hit = new RayHit(); //Put the ray into local space. Ray localRay; AffineTransform inverse; AffineTransform.Invert(ref transform, out inverse); Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction); AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position); //Use rasterizey traversal. //The origin is at 0,0,0 and the map goes +X, +Y, +Z. //if it's before the origin and facing away, or outside the max and facing out, early out. float maxX = heights.GetLength(0) - 1; float maxZ = heights.GetLength(1) - 1; Vector3 progressingOrigin = localRay.Position; float distance = 0; //Check the outside cases first. if (progressingOrigin.X < 0) { if (localRay.Direction.X > 0) { //Off the left side. float timeToMinX = -progressingOrigin.X / localRay.Direction.X; distance += timeToMinX; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { return(false); //Outside and pointing away from the terrain. } } else if (progressingOrigin.X > maxX) { if (localRay.Direction.X < 0) { //Off the left side. float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X; distance += timeToMinX; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { return(false); //Outside and pointing away from the terrain. } } if (progressingOrigin.Z < 0) { if (localRay.Direction.Z > 0) { float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z; distance += timeToMinZ; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { return(false); } } else if (progressingOrigin.Z > maxZ) { if (localRay.Direction.Z < 0) { float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z; distance += timeToMinZ; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { return(false); } } if (distance > maximumLength) { return(false); } //By now, we should be entering the main body of the terrain. int xCell = (int)progressingOrigin.X; int zCell = (int)progressingOrigin.Z; //If it's hitting the border and going in, then correct the index //so that it will initially target a valid quad. //Without this, a quad beyond the border would be tried and failed. if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0) { xCell = heights.GetLength(0) - 2; } if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0) { zCell = heights.GetLength(1) - 2; } while (true) { //Check for a miss. if (xCell < 0 || zCell < 0 || xCell >= heights.GetLength(0) - 1 || zCell >= heights.GetLength(1) - 1) { return(false); } //Test the triangles of this cell. Vector3 v1, v2, v3, v4; // v3 v4 // v1 v2 GetLocalPosition(xCell, zCell, out v1); GetLocalPosition(xCell + 1, zCell, out v2); GetLocalPosition(xCell, zCell + 1, out v3); GetLocalPosition(xCell + 1, zCell + 1, out v4); RayHit hit1, hit2; bool didHit1; bool didHit2; //Don't bother doing ray intersection tests if the ray can't intersect it. float highest = v1.Y; float lowest = v1.Y; if (v2.Y > highest) { highest = v2.Y; } else if (v2.Y < lowest) { lowest = v2.Y; } if (v3.Y > highest) { highest = v3.Y; } else if (v3.Y < lowest) { lowest = v3.Y; } if (v4.Y > highest) { highest = v4.Y; } else if (v4.Y < lowest) { lowest = v4.Y; } if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 || progressingOrigin.Y < lowest && localRay.Direction.Y < 0)) { if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight) { //Always perform the raycast as if Y+ in local space is the way the triangles are facing. didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1); didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2); } else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft) { didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1); didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2); } if (didHit1 && didHit2) { if (hit1.T < hit2.T) { Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit1.T; return(true); } Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit2.T; return(true); } else if (didHit1) { Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit1.T; return(true); } else if (didHit2) { Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit2.T; return(true); } } //Move to the next cell. float timeToX; if (localRay.Direction.X < 0) { timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X; } else if (localRay.Direction.X > 0) { timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X; } else { timeToX = float.MaxValue; } float timeToZ; if (localRay.Direction.Z < 0) { timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z; } else if (localRay.Direction.Z > 0) { timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z; } else { timeToZ = float.MaxValue; } //Move to the next cell. if (timeToX < timeToZ) { if (localRay.Direction.X < 0) { xCell--; } else { xCell++; } distance += timeToX; if (distance > maximumLength) { return(false); } Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { if (localRay.Direction.Z < 0) { zCell--; } else { zCell++; } distance += timeToZ; if (distance > maximumLength) { return(false); } Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } } }
/// <summary> /// Detección de colisiones recursiva /// </summary> public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List<IColisionable> obstaculos, int recursionDepth, ColisionInfo colisionInfo) { //Limitar recursividad if (recursionDepth > 5) { return; } //Ver si la distancia a recorrer es para tener en cuenta float distanceToTravelSq = movementVector.LengthSq(); if (distanceToTravelSq < EPSILON) { return; } //Posicion deseada Vector3 originalSphereCenter = characterSphere.Center; Vector3 nextSphereCenter = originalSphereCenter + movementVector; //Buscar el punto de colision mas cercano de todos los objetos candidatos float minCollisionDistSq = float.MaxValue; Vector3 realMovementVector = movementVector; TgcBoundingAxisAlignBox.Face collisionFace = null; IColisionable collisionObstacle = null; Vector3 nearestPolygonIntersectionPoint = Vector3.Empty; foreach (IColisionable obstaculoBB in obstaculos) { //Obtener los polígonos que conforman las 6 caras del BoundingBox TgcBoundingAxisAlignBox.Face[] bbFaces = obstaculoBB.GetTgcBoundingBox().computeFaces(); foreach (TgcBoundingAxisAlignBox.Face bbFace in bbFaces) { Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane); TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector); float brutePlaneDist; Vector3 brutePlaneIntersectionPoint; if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint)) { continue; } float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq(); if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq) { continue; } //Obtener punto de colisión en el plano, según la normal del plano float pDist; Vector3 planeIntersectionPoint; Vector3 sphereIntersectionPoint; TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal); bool embebbed = false; bool collisionFound = false; if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //Ver si el plano está embebido en la esfera if (pDist <= characterSphere.Radius) { embebbed = true; //TODO: REVISAR ESTO, caso embebido a analizar con más detalle sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius; } //Esta fuera de la esfera else { //Obtener punto de colisión del contorno de la esfera según la normal del plano sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius); //Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector); if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint)) { //no hay colisión continue; } } //Ver si planeIntersectionPoint pertenece al polígono Vector3 newMovementVector; float newMoveDistSq; Vector3 polygonIntersectionPoint; if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace)) { if (embebbed) { //TODO: REVISAR ESTO, nunca debería pasar //throw new Exception("El polígono está dentro de la esfera"); } polygonIntersectionPoint = planeIntersectionPoint; collisionFound = true; } else { //Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint, bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]); //Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega Vector3 reversePointSeg = polygonIntersectionPoint - movementVector; if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint)) { collisionFound = true; } } if (collisionFound) { //Nuevo vector de movimiento acotado newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint; newMoveDistSq = newMovementVector.LengthSq(); //se colisiono con algo, lo agrego a la lista colisionInfo.Add(obstaculoBB); if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq) { minCollisionDistSq = newMoveDistSq; realMovementVector = newMovementVector; nearestPolygonIntersectionPoint = polygonIntersectionPoint; collisionFace = bbFace; collisionObstacle = obstaculoBB; } } } } } //Si nunca hubo colisión, avanzar todo lo requerido if (collisionFace == null) { //Avanzar hasta muy cerca float movementLength = movementVector.Length(); movementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(movementVector); return; } //Solo movernos si ya no estamos muy cerca if (minCollisionDistSq >= EPSILON) { //Mover el BoundingSphere hasta casi la nueva posición real float movementLength = realMovementVector.Length(); realMovementVector.Multiply((movementLength - EPSILON) / movementLength); characterSphere.moveCenter(realMovementVector); } //Calcular plano de Sliding Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint; Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint; slidePlaneNormal.Normalize(); Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal); //Proyectamos el punto original de destino en el plano de sliding TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal); float slideT; Vector3 slideDestinationPoint; if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint)) { //Nuevo vector de movimiento Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint; if (slideMovementVector.LengthSq() < EPSILON) { return; } //Recursividad para aplicar sliding doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1, colisionInfo); } return; }
/// <summary> /// Computes representative support information based on the character's current traction contacts, support contacts, and ray contacts. /// </summary> /// <param name="down">Down direction of the character.</param> private void UpdateSupportData(ref Vector3 down) { //Choose which set of contacts to use. RawList <CharacterContact> contacts; if (tractionContacts.Count > 0) { contacts = tractionContacts; } else if (supportContacts.Count > 0) { contacts = supportContacts; } else { //No contacts provide support! //Fall back to the ray cast result. if (SupportRayData != null) { supportData = new SupportData { Position = SupportRayData.Value.HitData.Location, Normal = SupportRayData.Value.HitData.Normal, Depth = Vector3.Dot(down, SupportRayData.Value.HitData.Normal) * (BottomDistance - SupportRayData.Value.HitData.T), SupportObject = SupportRayData.Value.HitObject }; } else { supportData = new SupportData(); } return; } //Compute a representative support from the set of contacts. supportData.Position = contacts.Elements[0].Contact.Position; supportData.Normal = contacts.Elements[0].Contact.Normal; for (int i = 1; i < contacts.Count; i++) { Vector3.Add(ref supportData.Position, ref contacts.Elements[i].Contact.Position, out supportData.Position); Vector3.Add(ref supportData.Normal, ref contacts.Elements[i].Contact.Normal, out supportData.Normal); } if (contacts.Count > 1) { Vector3.Divide(ref supportData.Position, contacts.Count, out supportData.Position); float length = supportData.Normal.LengthSquared(); if (length < Toolbox.Epsilon) { //It's possible that the normals have cancelled each other out- that would be bad! //Just use an arbitrary support's normal in that case. supportData.Normal = contacts.Elements[0].Contact.Normal; } else { Vector3.Multiply(ref supportData.Normal, 1 / (float)Math.Sqrt(length), out supportData.Normal); } } //Now that we have the normal, cycle through all the contacts again and find the deepest projected depth. //Use that object as our support too. float depth = -float.MaxValue; Collidable supportObject = null; for (int i = 0; i < contacts.Count; i++) { float dot; Vector3.Dot(ref contacts.Elements[i].Contact.Normal, ref supportData.Normal, out dot); dot = dot * contacts.Elements[i].Contact.PenetrationDepth; if (dot > depth) { depth = dot; supportObject = contacts.Elements[i].Collidable; } } supportData.Depth = depth; supportData.SupportObject = supportObject; }
public void When_Vector_Multiplied_With_A_Matrix_Vector_With_Result_Is_Returned() { //Arrange Vector3 vectorOne = new Vector3(3.0, 2.0, 1.0); Matrix4 matrixOne = new Matrix4(1.0, 2.0, 3.0, 4.0, 2.0, 3.0, 4.0, 1.0, 3.0, 4.0, 1.0, 2.0, 4.0, 1.0, 2.0, 3.0); //Act Vector3 result = vectorOne.Multiply(matrixOne); //Assert Assert.AreEqual(result.X, 14.0); Assert.AreEqual(result.Y, 17.0); Assert.AreEqual(result.Z, 20.0); }
/// <summary> /// Gets the intersection between the capsule and the ray. /// </summary> /// <param name="ray">Ray to test against the capsule.</param> /// <param name="transform">Transform of the shape.</param> /// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param> /// <param name="hit">Hit data for the raycast, if any.</param> /// <returns>Whether or not the ray hit the target.</returns> public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { hit = new RayHit(); Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3 localOrigin; Vector3.Subtract(ref ray.Position, ref transform.Position, out localOrigin); Quaternion.Transform(ref localOrigin, ref conjugate, out localOrigin); Vector3 localDirection; Quaternion.Transform(ref ray.Direction, ref conjugate, out localDirection); Vector3 normal = Toolbox.ZeroVector; float temp, tmin = 0, tmax = maximumLength; if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth)) { return(false); } float inverseDirection = 1 / localDirection.X; float t1 = (-halfWidth - localOrigin.X) * inverseDirection; float t2 = (halfWidth - localOrigin.X) * inverseDirection; var tempNormal = new Vector3(-1, 0, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) { normal = tempNormal; } tmax = Math.Min(tmax, t2); if (tmin > tmax) { return(false); } if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight)) { return(false); } inverseDirection = 1 / localDirection.Y; t1 = (-halfHeight - localOrigin.Y) * inverseDirection; t2 = (halfHeight - localOrigin.Y) * inverseDirection; tempNormal = new Vector3(0, -1, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) { normal = tempNormal; } tmax = Math.Min(tmax, t2); if (tmin > tmax) { return(false); } if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength)) { return(false); } inverseDirection = 1 / localDirection.Z; t1 = (-halfLength - localOrigin.Z) * inverseDirection; t2 = (halfLength - localOrigin.Z) * inverseDirection; tempNormal = new Vector3(0, 0, -1); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) { normal = tempNormal; } tmax = Math.Min(tmax, t2); if (tmin > tmax) { return(false); } hit.T = tmin; Vector3.Multiply(ref ray.Direction, tmin, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Quaternion.Transform(ref normal, ref transform.Orientation, out normal); hit.Normal = normal; return(true); }
public static void GenerateNSBMD(string obj_path, float scale, bool has_texture, out byte[] nsbmd, out byte[] nsbtx) { var oBJ = FixNitroUV(new OBJ(obj_path)); if (oBJ == null) { throw new Exception(); } MLT mLT = new MLT(oBJ.MLTName); List <string> list = new List <string>(); Vector3 right = new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); Vector3 left = new Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity); int i; for (i = 0; i < oBJ.Vertices.Count; i++) { oBJ.Vertices[i] = Vector3.Multiply(oBJ.Vertices[i], scale); if (oBJ.Vertices[i].X < right.X) { right.X = oBJ.Vertices[i].X; } if (oBJ.Vertices[i].X > left.X) { left.X = oBJ.Vertices[i].X; } if (oBJ.Vertices[i].Y < right.Y) { right.Y = oBJ.Vertices[i].Y; } if (oBJ.Vertices[i].Y > left.Y) { left.Y = oBJ.Vertices[i].Y; } if (oBJ.Vertices[i].Z < right.Z) { right.Z = oBJ.Vertices[i].Z; } if (oBJ.Vertices[i].Z > left.Z) { left.Z = oBJ.Vertices[i].Z; } } Vector3 vector = left - right; float num = HelpersMax(vector.X, vector.Y, vector.Z); float num2 = 1f; int num3 = 0; while (num > 7.999756f) { num3++; num2 /= 2f; num /= 2f; } for (i = 0; i < oBJ.Vertices.Count; i++) { oBJ.Vertices[i] = Vector3.Multiply(oBJ.Vertices[i], num2); } NSBMD nSBMD = new NSBMD(!has_texture); nSBMD.modelSet = new NSBMD.ModelSet(); string text = Path.GetFileNameWithoutExtension(obj_path); if (text.Length > 16) { text = text.Remove(16); } nSBMD.modelSet.dict = new Dictionary <NSBMD.ModelSet.MDL0Data>(); nSBMD.modelSet.dict.Add(text, new NSBMD.ModelSet.MDL0Data()); nSBMD.modelSet.models = new NSBMD.ModelSet.Model[1]; nSBMD.modelSet.models[0] = new NSBMD.ModelSet.Model(); nSBMD.modelSet.models[0].info = new NSBMD.ModelSet.Model.ModelInfo(); nSBMD.modelSet.models[0].info.numNode = 1; foreach (OBJ.Face face in oBJ.Faces) { if (!list.Contains(face.MaterialName)) { list.Add(face.MaterialName); } nSBMD.modelSet.models[0].info.numTriangle++; } nSBMD.modelSet.models[0].info.numMat = (byte)list.Count; nSBMD.modelSet.models[0].info.numShp = (byte)list.Count; nSBMD.modelSet.models[0].info.firstUnusedMtxStackID = 1; nSBMD.modelSet.models[0].info.posScale = 1 << num3; nSBMD.modelSet.models[0].info.invPosScale = 1f / nSBMD.modelSet.models[0].info.posScale; nSBMD.modelSet.models[0].info.numVertex = (byte)oBJ.Vertices.Count; nSBMD.modelSet.models[0].info.numPolygon = (byte)oBJ.Faces.Count; nSBMD.modelSet.models[0].info.boxX = right.X * num2; nSBMD.modelSet.models[0].info.boxY = right.Y * num2; nSBMD.modelSet.models[0].info.boxZ = right.Z * num2; nSBMD.modelSet.models[0].info.boxW = vector.X * num2; nSBMD.modelSet.models[0].info.boxH = vector.Y * num2; nSBMD.modelSet.models[0].info.boxD = vector.Z * num2; nSBMD.modelSet.models[0].info.boxPosScale = 1 << num3; nSBMD.modelSet.models[0].info.boxInvPosScale = 1f / nSBMD.modelSet.models[0].info.boxPosScale; nSBMD.modelSet.models[0].nodes = new NSBMD.ModelSet.Model.NodeSet(); nSBMD.modelSet.models[0].nodes.dict = new Dictionary <NSBMD.ModelSet.Model.NodeSet.NodeSetData>(); nSBMD.modelSet.models[0].nodes.dict.Add("world_root", new NSBMD.ModelSet.Model.NodeSet.NodeSetData()); nSBMD.modelSet.models[0].nodes.data = new NSBMD.ModelSet.Model.NodeSet.NodeData[1]; nSBMD.modelSet.models[0].nodes.data[0] = new NSBMD.ModelSet.Model.NodeSet.NodeData(); nSBMD.modelSet.models[0].nodes.data[0].flag = 7; nSBMD.modelSet.models[0].materials = new NSBMD.ModelSet.Model.MaterialSet(); nSBMD.modelSet.models[0].materials.dictTexToMatList = new Dictionary <NSBMD.ModelSet.Model.MaterialSet.TexToMatData>(); nSBMD.modelSet.models[0].materials.dictPlttToMatList = new Dictionary <NSBMD.ModelSet.Model.MaterialSet.PlttToMatData>(); nSBMD.modelSet.models[0].materials.dict = new Dictionary <NSBMD.ModelSet.Model.MaterialSet.MaterialSetData>(); nSBMD.modelSet.models[0].materials.materials = new NSBMD.ModelSet.Model.MaterialSet.Material[list.Count]; i = 0; int num4 = 0; foreach (string item in list) { MLT.Material materialByName = mLT.GetMaterialByName(item); if (materialByName.DiffuseMap != null) { nSBMD.modelSet.models[0].materials.dictTexToMatList.Add(i.ToString() + "_t", new NSBMD.ModelSet.Model.MaterialSet.TexToMatData()); nSBMD.modelSet.models[0].materials.dictPlttToMatList.Add(i.ToString() + "_p", new NSBMD.ModelSet.Model.MaterialSet.PlttToMatData()); nSBMD.modelSet.models[0].materials.dictTexToMatList[num4].Value.NrMat = 1; nSBMD.modelSet.models[0].materials.dictTexToMatList[num4].Value.Materials = new int[1] { i }; nSBMD.modelSet.models[0].materials.dictPlttToMatList[num4].Value.NrMat = 1; nSBMD.modelSet.models[0].materials.dictPlttToMatList[num4].Value.Materials = new int[1] { i }; num4++; } nSBMD.modelSet.models[0].materials.dict.Add(i.ToString() + "_m", new NSBMD.ModelSet.Model.MaterialSet.MaterialSetData()); nSBMD.modelSet.models[0].materials.materials[i] = new NSBMD.ModelSet.Model.MaterialSet.Material(); nSBMD.modelSet.models[0].materials.materials[i].diffAmb = (uint)(((Graphic.encodeColor(Color.Black.ToArgb()) & 0x7FFF) << 16) | 0x8000 | (Graphic.encodeColor((materialByName.DiffuseMap != null) ? Color.FromArgb(200, 200, 200).ToArgb() : materialByName.DiffuseColor.ToArgb()) & 0x7FFF)); nSBMD.modelSet.models[0].materials.materials[i].specEmi = (uint)(((Graphic.encodeColor(Color.Black.ToArgb()) & 0x7FFF) << 16) | (Graphic.encodeColor(Color.Black.ToArgb()) & 0x7FFF)); uint num5 = (uint)(materialByName.Alpha * 31f); nSBMD.modelSet.models[0].materials.materials[i].polyAttr = 0u; nSBMD.modelSet.models[0].materials.materials[i].polyAttr |= 192u; nSBMD.modelSet.models[0].materials.materials[i].polyAttr |= num5 << 16; nSBMD.modelSet.models[0].materials.materials[i].polyAttrMask = 1059059967u; nSBMD.modelSet.models[0].materials.materials[i].texImageParam = 196608u; nSBMD.modelSet.models[0].materials.materials[i].texImageParamMask = uint.MaxValue; nSBMD.modelSet.models[0].materials.materials[i].texPlttBase = 0; nSBMD.modelSet.models[0].materials.materials[i].flag = (NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_TEXMTX_SCALEONE | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_TEXMTX_ROTZERO | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_TEXMTX_TRANSZERO | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_DIFFUSE | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_AMBIENT | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_VTXCOLOR | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_SPECULAR | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_EMISSION | NSBMD.ModelSet.Model.MaterialSet.Material.NNS_G3D_MATFLAG.NNS_G3D_MATFLAG_SHININESS); if (materialByName.DiffuseMap != null) { nSBMD.modelSet.models[0].materials.materials[i].origWidth = (ushort)materialByName.DiffuseMap.Width; nSBMD.modelSet.models[0].materials.materials[i].origHeight = (ushort)materialByName.DiffuseMap.Height; } nSBMD.modelSet.models[0].materials.materials[i].magW = 1f; nSBMD.modelSet.models[0].materials.materials[i].magH = 1f; i++; } nSBMD.modelSet.models[0].shapes = new NSBMD.ModelSet.Model.ShapeSet(); nSBMD.modelSet.models[0].shapes.dict = new Dictionary <NSBMD.ModelSet.Model.ShapeSet.ShapeSetData>(); nSBMD.modelSet.models[0].shapes.shape = new NSBMD.ModelSet.Model.ShapeSet.Shape[list.Count]; i = 0; foreach (string item2 in list) { int num6 = 1; int num7 = 1; MLT.Material materialByName = mLT.GetMaterialByName(item2); List <Color> list2 = null; if (materialByName.DiffuseMap != null) { num6 = materialByName.DiffuseMap.Width; num7 = -materialByName.DiffuseMap.Height; } else { list2 = new List <Color>(); } nSBMD.modelSet.models[0].shapes.dict.Add(i.ToString() + "_py", new NSBMD.ModelSet.Model.ShapeSet.ShapeSetData()); List <Vector3> list3 = new List <Vector3>(); List <Vector2> list4 = new List <Vector2>(); List <Vector3> list5 = new List <Vector3>(); foreach (OBJ.Face face2 in oBJ.Faces) { if (face2.MaterialName == item2) { list3.AddRange(new Vector3[3] { oBJ.Vertices[face2.VertexIndieces[0]], oBJ.Vertices[face2.VertexIndieces[1]], oBJ.Vertices[face2.VertexIndieces[2]] }); Vector2[] array = new Vector2[3] { oBJ.TexCoords[face2.TexCoordIndieces[0]], oBJ.TexCoords[face2.TexCoordIndieces[1]], oBJ.TexCoords[face2.TexCoordIndieces[2]] }; array[0].X *= num6; array[0].Y *= num7; array[1].X *= num6; array[1].Y *= num7; array[2].X *= num6; array[2].Y *= num7; list4.AddRange(array); if (face2.NormalIndieces.Count != 0) { list5.AddRange(new Vector3[3] { oBJ.Normals[face2.NormalIndieces[0]], oBJ.Normals[face2.NormalIndieces[1]], oBJ.Normals[face2.NormalIndieces[2]] }); } list2?.AddRange(new Color[3] { materialByName.DiffuseColor, materialByName.DiffuseColor, materialByName.DiffuseColor }); } } nSBMD.modelSet.models[0].shapes.shape[i] = new NSBMD.ModelSet.Model.ShapeSet.Shape(); nSBMD.modelSet.models[0].shapes.shape[i].DL = GlNitro.GenerateDl(list3.ToArray(), list4.ToArray(), list5.ToArray(), list2?.ToArray()); nSBMD.modelSet.models[0].shapes.shape[i].sizeDL = (uint)nSBMD.modelSet.models[0].shapes.shape[i].DL.Length; nSBMD.modelSet.models[0].shapes.shape[i].flag = (NSBMD.ModelSet.Model.ShapeSet.Shape.NNS_G3D_SHPFLAG.NNS_G3D_SHPFLAG_USE_NORMAL | NSBMD.ModelSet.Model.ShapeSet.Shape.NNS_G3D_SHPFLAG.NNS_G3D_SHPFLAG_USE_COLOR | NSBMD.ModelSet.Model.ShapeSet.Shape.NNS_G3D_SHPFLAG.NNS_G3D_SHPFLAG_USE_TEXCOORD); i++; } var sBCWriter = new Obj2Nsbmd.SBCWriter(); sBCWriter.NODEDESC(0, 0, S: false, P: false, 0); sBCWriter.NODE(0, V: true); sBCWriter.POSSCALE(OPT: true); for (i = 0; i < list.Count; i++) { sBCWriter.MAT((byte)i); sBCWriter.SHP((byte)i); } sBCWriter.POSSCALE(OPT: false); sBCWriter.RET(); sBCWriter.NOP(); nSBMD.modelSet.models[0].sbc = sBCWriter.GetData(); NSBTX.TexplttSet texplttSet = new NSBTX.TexplttSet(); texplttSet.TexInfo = new NSBTX.TexplttSet.texInfo(); texplttSet.Tex4x4Info = new NSBTX.TexplttSet.tex4x4Info(); texplttSet.PlttInfo = new NSBTX.TexplttSet.plttInfo(); texplttSet.dictTex = new Dictionary <NSBTX.TexplttSet.DictTexData>(); texplttSet.dictPltt = new Dictionary <NSBTX.TexplttSet.DictPlttData>(); i = 0; num4 = 0; foreach (string item3 in list) { MLT.Material materialByName = mLT.GetMaterialByName(item3); if (materialByName.DiffuseMap != null) { BitmapData bitmapData = materialByName.DiffuseMap.LockBits(new Rectangle(0, 0, materialByName.DiffuseMap.Width, materialByName.DiffuseMap.Height), ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb); List <Color> list6 = new List <Color>(); bool flag = false; for (int j = 0; j < materialByName.DiffuseMap.Width * materialByName.DiffuseMap.Height; j++) { list6.Add(Color.FromArgb(Marshal.ReadInt32(bitmapData.Scan0, j * 4))); if (list6.Last().A != 0 && list6.Last().A != byte.MaxValue && !flag) { flag = true; } } materialByName.DiffuseMap.UnlockBits(bitmapData); list6 = list6.Distinct().ToList(); texplttSet.dictTex.Add(i.ToString() + "_t", new NSBTX.TexplttSet.DictTexData()); texplttSet.dictPltt.Add(i.ToString() + "_p", new NSBTX.TexplttSet.DictPlttData()); texplttSet.dictTex[num4].Value.S = (ushort)materialByName.DiffuseMap.Width; texplttSet.dictTex[num4].Value.T = (ushort)materialByName.DiffuseMap.Height; if (flag && list6.Count <= 8) { texplttSet.dictTex[num4].Value.Fmt = Graphic.GXTexFmt.GX_TEXFMT_A5I3; } else if (flag) { texplttSet.dictTex[num4].Value.Fmt = Graphic.GXTexFmt.GX_TEXFMT_A3I5; } else if (list6.Count <= 16) { texplttSet.dictTex[num4].Value.Fmt = Graphic.GXTexFmt.GX_TEXFMT_PLTT16; } else { texplttSet.dictTex[num4].Value.Fmt = Graphic.GXTexFmt.GX_TEXFMT_COMP4x4; } Graphic.ConvertBitmap(materialByName.DiffuseMap, out texplttSet.dictTex[num4].Value.Data, out texplttSet.dictPltt[num4].Value.Data, out texplttSet.dictTex[num4].Value.Data4x4, texplttSet.dictTex[num4].Value.Fmt, Graphic.NNSG2dCharacterFmt.NNS_G2D_CHARACTER_FMT_BMP, out texplttSet.dictTex[num4].Value.TransparentColor); num4++; } i++; } nsbtx = null; if (!has_texture) { nSBMD.TexPlttSet = texplttSet; } else { var nSBTX = new NSBTX { TexPlttSet = texplttSet }; nsbtx = nSBTX.Write(); } nsbmd = nSBMD.Write(); }
public float DistanceSquareToSegment(Vector3 v0,Vector3 v1, ref Vector3 optionalPointOnRay, ref Vector3 optionalPointOnSegment) { // from http://www.geometrictools.com/LibMathematics/Distance/Wm5DistRay3Segment3.cpp // It returns the min distance between the ray and the segment // defined by v0 and v1 // It can also set two optional targets : // - The closest point on the ray // - The closest point on the segment var segCenter = v0; segCenter.Add( v1 ); segCenter.Multiply( 0.5f ); var segDir = v1; segDir.Subtract(v0); segDir.Normalize(); var segExtent = v0.DistanceTo( v1 ) / 2; var diff = origin; diff.Subtract(segCenter ); var a01 = - direction.Dot( segDir ); var b0 = diff.Dot( direction ); var b1 = - diff.Dot( segDir ); var c = diff.LengthSquared(); var det = Mathf.Abs( 1 - a01 * a01 ); var sqrDist = 0f; var s0 = 0f; var s1 = 0f; if ( det >= 0 ) { // The ray and segment are not parallel. s0 = a01 * b1 - b0; s1 = a01 * b0 - b1; var extDet = segExtent * det; if ( s0 >= 0 ) { if ( s1 >= - extDet ) { if ( s1 <= extDet ) { // region 0 // Minimum at interior points of ray and segment. var invDet = 1 / det; s0 *= invDet; s1 *= invDet; sqrDist = s0 * ( s0 + a01 * s1 + 2 * b0 ) + s1 * ( a01 * s0 + s1 + 2 * b1 ) + c; } else { // region 1 s1 = segExtent; s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) ); sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c; } } else { // region 5 s1 = - segExtent; s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) ); sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c; } } else { if ( s1 <= - extDet ) { // region 4 s0 = Mathf.Max( 0, - ( - a01 * segExtent + b0 ) ); s1 = ( s0 > 0 ) ? - segExtent : Mathf.Min(Mathf.Max( - segExtent, - b1 ), segExtent ); sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c; } else if ( s1 <= extDet ) { // region 3 s0 = 0; s1 = Mathf.Min(Mathf.Max( - segExtent, - b1 ), segExtent ); sqrDist = s1 * ( s1 + 2 * b1 ) + c; } else { // region 2 s0 = Mathf.Max( 0, - ( a01 * segExtent + b0 ) ); s1 = ( s0 > 0 ) ? segExtent : Mathf.Min( Mathf.Max( - segExtent, - b1 ), segExtent ); sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c; } } } else { // Ray and segment are parallel. s1 = ( a01 > 0 ) ? - segExtent : segExtent; s0 = Mathf.Max( 0, - ( a01 * s1 + b0 ) ); sqrDist = - s0 * s0 + s1 * ( s1 + 2 * b1 ) + c; } optionalPointOnRay = direction; optionalPointOnRay.Multiply(s0); optionalPointOnRay.Add(origin); optionalPointOnSegment = segDir; optionalPointOnSegment.Multiply( s1 ); optionalPointOnSegment.Add( segCenter ); return sqrDist; }
///// <summary> ///// Defines two planes that bound the mesh shape in local space. ///// </summary> //struct Extent //{ // internal Vector3 Direction; // internal float Minimum; // internal float Maximum; // internal void Clamp(ref Vector3 v) // { // float dot; // Vector3.Dot(ref v, ref Direction, out dot); // float difference; // if (dot < Minimum) // { // difference = dot - Minimum; // } // else if (dot > Maximum) // { // difference = dot - Maximum; // } // else return; // //Subtract the component of v which is parallel to the normal. // v.X -= difference * Direction.X; // v.Y -= difference * Direction.Y; // v.Z -= difference * Direction.Z; // } //} //RawList<Extent> extents = new RawList<Extent>(); //void ComputeBoundingHull() //{ // //TODO: // //While we have computed a convex hull of the shape already, we don't really // //need the full tightness of the convex hull. // extents.Add(new Extent() { Direction = new Vector3(1, 0, 0) }); // extents.Add(new Extent() { Direction = new Vector3(0, 1, 0) }); // extents.Add(new Extent() { Direction = new Vector3(0, 0, 1) }); // //extents.Add(new Extent() { Direction = new Vector3(1, 1, 0) }); // //extents.Add(new Extent() { Direction = new Vector3(-1, 1, 0) }); // //extents.Add(new Extent() { Direction = new Vector3(0, 1, 1) }); // //extents.Add(new Extent() { Direction = new Vector3(0, 1, -1) }); // extents.Add(new Extent() { Direction = Vector3.Normalize(new Vector3(1, 0, 1)) }); // extents.Add(new Extent() { Direction = Vector3.Normalize(new Vector3(1, 0, -1)) }); // //Add more extents for a tighter volume // //Initialize the max and mins. // for (int i = 0; i < extents.count; i++) // { // extents.Elements[i].Minimum = float.MaxValue; // extents.Elements[i].Maximum = -float.MaxValue; // } // for (int i = 0; i < triangleMesh.Data.vertices.Length; i++) // { // Vector3 v; // triangleMesh.Data.GetVertexPosition(i, out v); // for (int j = 0; j < extents.count; j++) // { // float dot; // Vector3.Dot(ref v, ref extents.Elements[j].Direction, out dot); // if (dot < extents.Elements[j].Minimum) // extents.Elements[j].Minimum = dot; // if (dot > extents.Elements[j].Maximum) // extents.Elements[j].Maximum = dot; // } // } //} private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif //Sample the local directions from the matrix, implicitly transposed. var rightDirection = new Vector3(o.M11, o.M21, o.M31); var upDirection = new Vector3(o.M12, o.M22, o.M32); var backDirection = new Vector3(o.M13, o.M23, o.M33); int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0; float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue; for (int i = 0; i < surfaceVertices.Count; i++) { float dotX, dotY, dotZ; Vector3.Dot(ref rightDirection, ref surfaceVertices.Elements[i], out dotX); Vector3.Dot(ref upDirection, ref surfaceVertices.Elements[i], out dotY); Vector3.Dot(ref backDirection, ref surfaceVertices.Elements[i], out dotZ); if (dotX < minX) { minX = dotX; left = i; } if (dotX > maxX) { maxX = dotX; right = i; } if (dotY < minY) { minY = dotY; down = i; } if (dotY > maxY) { maxY = dotY; up = i; } if (dotZ < minZ) { minZ = dotZ; forward = i; } if (dotZ > maxZ) { maxZ = dotZ; backward = i; } } //Incorporate the collision margin. Vector3.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection); Vector3.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection); Vector3.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection); var rightElement = surfaceVertices.Elements[right]; var leftElement = surfaceVertices.Elements[left]; var upElement = surfaceVertices.Elements[up]; var downElement = surfaceVertices.Elements[down]; var backwardElement = surfaceVertices.Elements[backward]; var forwardElement = surfaceVertices.Elements[forward]; Vector3.Add(ref rightElement, ref rightDirection, out rightElement); Vector3.Subtract(ref leftElement, ref rightDirection, out leftElement); Vector3.Add(ref upElement, ref upDirection, out upElement); Vector3.Subtract(ref downElement, ref upDirection, out downElement); Vector3.Add(ref backwardElement, ref backDirection, out backwardElement); Vector3.Subtract(ref forwardElement, ref backDirection, out forwardElement); //TODO: This could be optimized. Unnecessary transformation information gets computed. Vector3 vMinX, vMaxX, vMinY, vMaxY, vMinZ, vMaxZ; Matrix3x3.Transform(ref rightElement, ref o, out vMaxX); Matrix3x3.Transform(ref leftElement, ref o, out vMinX); Matrix3x3.Transform(ref upElement, ref o, out vMaxY); Matrix3x3.Transform(ref downElement, ref o, out vMinY); Matrix3x3.Transform(ref backwardElement, ref o, out vMaxZ); Matrix3x3.Transform(ref forwardElement, ref o, out vMinZ); boundingBox.Max.X = vMaxX.X; boundingBox.Max.Y = vMaxY.Y; boundingBox.Max.Z = vMaxZ.Z; boundingBox.Min.X = vMinX.X; boundingBox.Min.Y = vMinY.Y; boundingBox.Min.Z = vMinZ.Z; }
public void Transform(ref Vector3 pos, ref Vector3 scale, ref Vector3 euler) { pos += translation; scale.Multiply(this.scale); euler += this.euler; }
public override void Update(float dt) { base.Update(dt); ////Rotate the camera of the character based on the support velocity, if a support with velocity exists. ////This can be very disorienting in some cases; that's why it is off by default! //if (Character.SupportFinder.HasSupport) //{ // SupportData? data; // if (Character.SupportFinder.HasTraction) // data = Character.SupportFinder.TractionData; // else // data = Character.SupportFinder.SupportData; // var support = data.Value.SupportObject as EntityCollidable; // if (support != null && !support.Entity.IsDynamic) //Having the view turned by dynamic entities is extremely confusing for the most part. // { // float dot = Vector3.Dot(support.Entity.AngularVelocity, Character.Body.OrientationMatrix.Up); // Camera.Yaw(dot * dt); // } //} if (UseCameraSmoothing) { //First, find where the camera is expected to be based on the last position and the current velocity. //Note: if the character were a free-floating 6DOF character, this would need to include an angular velocity contribution. //And of course, the camera orientation would be based on the character's orientation. //We use the space's time step since, in the demos, the simulation moves forward one time step per frame. //The passed-in dt, in contrast, does not necessarily correspond to a simulated state and tends to make the camera jittery. var spaceDt = Character.Space != null ? Character.Space.TimeStepSettings.TimeStepDuration : dt; Camera.Position = Camera.Position + Character.Body.LinearVelocity * spaceDt; //Now compute where it should be according the physical body of the character. Vector3 up = Character.Body.OrientationMatrix.Up; Vector3 bodyPosition = Character.Body.BufferedStates.InterpolatedStates.Position; Vector3 goalPosition = bodyPosition + up * (Character.StanceManager.CurrentStance == Stance.Standing ? StandingCameraOffset : CrouchingCameraOffset); //Usually, the camera position and the goal will be very close, if not matching completely. //However, if the character steps or has its position otherwise modified, then they will not match. //In this case, we need to correct the camera position. //To do this, first note that we can't correct infinite errors. We need to define a bounding region that is relative to the character //in which the camera can interpolate around. The most common discontinuous motions are those of upstepping and downstepping. //In downstepping, the character can teleport up to the character's MaximumStepHeight downwards. //In upstepping, the character can teleport up to the character's MaximumStepHeight upwards, and the body's CollisionMargin horizontally. //Picking those as bounds creates a constraining cylinder. Vector3 error = goalPosition - Camera.Position; float verticalError = Vector3.Dot(error, up); Vector3 horizontalError = error - verticalError * up; //Clamp the vertical component of the camera position within the bounding cylinder. if (verticalError > Character.StepManager.MaximumStepHeight) { Camera.Position -= up * (Character.StepManager.MaximumStepHeight - verticalError); verticalError = Character.StepManager.MaximumStepHeight; } else if (verticalError < -Character.StepManager.MaximumStepHeight) { Camera.Position -= up * (-Character.StepManager.MaximumStepHeight - verticalError); verticalError = -Character.StepManager.MaximumStepHeight; } //Clamp the horizontal distance too. float horizontalErrorLength = horizontalError.LengthSquared(); float margin = Character.Body.CollisionInformation.Shape.CollisionMargin; if (horizontalErrorLength > margin * margin) { Vector3 previousHorizontalError = horizontalError; Vector3.Multiply(ref horizontalError, margin / (float)Math.Sqrt(horizontalErrorLength), out horizontalError); Camera.Position -= horizontalError - previousHorizontalError; } //Now that the error/camera position is known to lie within the constraining cylinder, we can perform a smooth correction. //This removes a portion of the error each frame. //Note that this is not framerate independent. If fixed time step is not enabled, //a different smoothing method should be applied to the final error values. //float errorCorrectionFactor = .3f; //This version is framerate independent, although it is more expensive. float errorCorrectionFactor = (float)(1 - Math.Pow(.000000001, dt)); Camera.Position += up * (verticalError * errorCorrectionFactor); Camera.Position += horizontalError * errorCorrectionFactor; } else { Camera.Position = Character.Body.Position + (Character.StanceManager.CurrentStance == Stance.Standing ? StandingCameraOffset : CrouchingCameraOffset) * Character.Body.OrientationMatrix.Up; } }