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