public void PointToVector2()
{
var pt = new Point(-3, 5);
var actual = pt.ToVector2();
var expected = new System.Numerics.Vector2(-3, 5);
Assert.AreEqual(expected, actual);
}
static int Main(string[] args)
{
Point a = new Point(0, 0), b = new Point(1, 0);
Point c = a + b;
Point d = c - b;
Point e = d - a;
if (CheckNEQ(e.X, 0) || CheckNEQ(e.Y, 0))
{
return 0;
}
e += e;
if (CheckNEQ(e.X, 0) || CheckNEQ(e.Y, 0))
{
return 0;
}
a += new Point(5, 2);
e += a + b;
if (CheckNEQ(e.X, 6) || CheckNEQ(e.Y, 2))
{
return 0;
}
e *= 10;
if (CheckNEQ(e.X, 60) || CheckNEQ(e.Y, 20))
{
return 0;
}
if (Adds() != 100)
{
return 0;
}
return 100;
}
static int Main(string[] args)
{
Point a = new Point(10, 50);
Point b = new Point(10, 10);
Point c = a * b;
if (((int)c.X) == 100)
{
return 100;
}
return 0;
}
static int Adds()
{
Point a = new Point(0, 0);
Point b = new Point(1, 2);
a += b;
a += b;
a += b;
a += b;
a += b;
a += b;
a += b;
a += b;
a += b;
a += b;
if (CheckNEQ(a.X, 10) || CheckNEQ(a.Y, 20))
{
return 0;
}
return 100;
}
///<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 localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);
//Compute the plane normals.
System.Numerics.Vector3 minPlaneNormal, maxPlaneNormal;
//Rotate basisA y axis around the basisA primary axis.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref basis.xAxis, ref rotation, out minPlaneNormal);
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref basis.xAxis, ref rotation, out maxPlaneNormal);
//Compute the errors along the two normals.
float planePositionMin, planePositionMax;
Vector3Ex.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
Vector3Ex.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);
float span = GetDistanceFromMinimum(maximumAngle);
//Early out and compute the determine the plane normal.
if (span >= MathHelper.Pi)
{
if (planePositionMax > 0 || planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = System.Numerics.Vector2.Zero;
accumulatedImpulse = System.Numerics.Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMax > planePositionMin)
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
else
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
//There's never a non-degenerate situation where having both planes active with a span
//greater than pi is useful.
}
else
{
if (planePositionMax > 0 && planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = System.Numerics.Vector2.Zero;
accumulatedImpulse = System.Numerics.Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMin <= 0 && planePositionMax <= 0)
{
//Escape upward.
//Activate both planes.
error.X = -planePositionMin;
error.Y = -planePositionMax;
minIsActive = true;
maxIsActive = true;
}
else if (planePositionMin <= 0)
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
else
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
}
isLimitActive = true;
//****** VELOCITY BIAS ******//
//Compute the correction velocity
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
//Compute the jacobians
if (minIsActive)
{
Vector3Ex.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
if (jacobianMinA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMinA = basis.primaryAxis;
}
jacobianMinA.Normalize();
jacobianMinB.X = -jacobianMinA.X;
jacobianMinB.Y = -jacobianMinA.Y;
jacobianMinB.Z = -jacobianMinA.Z;
}
if (maxIsActive)
{
Vector3Ex.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMaxA = basis.primaryAxis;
}
jacobianMaxA.Normalize();
jacobianMaxB.X = -jacobianMaxA.X;
jacobianMaxB.Y = -jacobianMaxA.Y;
jacobianMaxB.Z = -jacobianMaxA.Z;
}
//Error is always positive
if (minIsActive)
{
biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
float relativeVelocity;
float dot;
//Find the velocity contribution from each connection
Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
relativeVelocity += dot;
biasVelocity.X = MathHelper.Max(biasVelocity.X, ComputeBounceVelocity(-relativeVelocity));
}
}
if (maxIsActive)
{
biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Find the velocity contribution from each connection
if (maxIsActive)
{
float relativeVelocity;
Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
float dot;
Vector3Ex.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
relativeVelocity += dot;
biasVelocity.Y = MathHelper.Max(biasVelocity.Y, ComputeBounceVelocity(-relativeVelocity));
}
}
}
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float minEntryA, minEntryB;
float maxEntryA, maxEntryB;
System.Numerics.Vector3 transformedAxis;
if (connectionA.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
}
else
minEntryA = 0;
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
}
else
maxEntryA = 0;
}
else
{
minEntryA = 0;
maxEntryA = 0;
}
//Connection B's contribution to the mass matrix
if (connectionB.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
}
else
minEntryB = 0;
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3Ex.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
}
else
maxEntryB = 0;
}
else
{
minEntryB = 0;
maxEntryB = 0;
}
//Compute the inverse mass matrix
//Notice that the mass matrix isn't linked, it's two separate ones.
velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);
}
/// <summary>
/// Computes per-frame information necessary for the constraint.
/// </summary>
/// <param name="dt">Time step duration.</param>
public override void Update(float dt)
{
bool isTryingToMove = movementDirection3d.LengthSquared() > 0;
if (!isTryingToMove)
TargetSpeed = 0;
maxForce = MaximumForce * dt;
//Compute the jacobians. This is basically a PointOnLineJoint with motorized degrees of freedom.
System.Numerics.Vector3 downDirection = characterBody.orientationMatrix.Down;
if (MovementMode != MovementMode.Floating)
{
//Compute the linear jacobians first.
if (isTryingToMove)
{
System.Numerics.Vector3 velocityDirection;
System.Numerics.Vector3 offVelocityDirection;
//Project the movement direction onto the support plane defined by the support normal.
//This projection is NOT along the support normal to the plane; that would cause the character to veer off course when moving on slopes.
//Instead, project along the sweep direction to the plane.
//For a 6DOF character controller, the lineStart would be different; it must be perpendicular to the local up.
System.Numerics.Vector3 lineStart = movementDirection3d;
System.Numerics.Vector3 lineEnd;
Vector3Ex.Add(ref lineStart, ref downDirection, out lineEnd);
Plane plane = new Plane(supportData.Normal, 0);
float t;
//This method can return false when the line is parallel to the plane, but previous tests and the slope limit guarantee that it won't happen.
Toolbox.GetLinePlaneIntersection(ref lineStart, ref lineEnd, ref plane, out t, out velocityDirection);
//The origin->intersection line direction defines the horizontal velocity direction in 3d space.
velocityDirection.Normalize();
//The normal and velocity direction are perpendicular and normal, so the off velocity direction doesn't need to be normalized.
Vector3Ex.Cross(ref velocityDirection, ref supportData.Normal, out offVelocityDirection);
linearJacobianA1 = velocityDirection;
linearJacobianA2 = offVelocityDirection;
linearJacobianB1 = -velocityDirection;
linearJacobianB2 = -offVelocityDirection;
}
else
{
//If the character isn't trying to move, then the velocity directions are not well defined.
//Instead, pick two arbitrary vectors on the support plane.
//First guess will be based on the previous jacobian.
//Project the old linear jacobian onto the support normal plane.
float dot;
Vector3Ex.Dot(ref linearJacobianA1, ref supportData.Normal, out dot);
System.Numerics.Vector3 toRemove;
Vector3Ex.Multiply(ref supportData.Normal, dot, out toRemove);
Vector3Ex.Subtract(ref linearJacobianA1, ref toRemove, out linearJacobianA1);
//Vector3Ex.Cross(ref linearJacobianA2, ref supportData.Normal, out linearJacobianA1);
float length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//First guess failed. Try the right vector.
Vector3Ex.Cross(ref Toolbox.RightVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
if (length < Toolbox.Epsilon)
{
//Okay that failed too! try the forward vector.
Vector3Ex.Cross(ref Toolbox.ForwardVector, ref supportData.Normal, out linearJacobianA1);
length = linearJacobianA1.LengthSquared();
//Unless something really weird is happening, we do not need to test any more axes.
}
}
Vector3Ex.Divide(ref linearJacobianA1, (float)Math.Sqrt(length), out linearJacobianA1);
//Pick another perpendicular vector. Don't need to normalize it since the normal and A1 are already normalized and perpendicular.
Vector3Ex.Cross(ref linearJacobianA1, ref supportData.Normal, out linearJacobianA2);
//B's linear jacobians are just -A's.
linearJacobianB1 = -linearJacobianA1;
linearJacobianB2 = -linearJacobianA2;
}
if (supportEntity != null)
{
//Compute the angular jacobians.
System.Numerics.Vector3 supportToContact = supportData.Position - supportEntity.Position;
//Since we treat the character to have infinite inertia, we're only concerned with the support's angular jacobians.
//Note the order of the cross product- it is reversed to negate the result.
Vector3Ex.Cross(ref linearJacobianA1, ref supportToContact, out angularJacobianB1);
Vector3Ex.Cross(ref linearJacobianA2, ref supportToContact, out angularJacobianB2);
}
else
{
//If we're not standing on an entity, there are no angular jacobians.
angularJacobianB1 = new System.Numerics.Vector3();
angularJacobianB2 = new System.Numerics.Vector3();
}
}
else
{
//If the character is floating, then the jacobians are simply the 3d movement direction and the perpendicular direction on the character's horizontal plane.
linearJacobianA1 = movementDirection3d;
linearJacobianA2 = System.Numerics.Vector3.Cross(linearJacobianA1, characterBody.orientationMatrix.Down);
}
//Compute the target velocity (in constraint space) for this frame. The hard work has already been done.
targetVelocity.X = TargetSpeed;
targetVelocity.Y = 0;
//Compute the effective mass matrix.
if (supportEntity != null && supportEntity.IsDynamic)
{
float m11, m22, m1221 = 0;
float inverseMass;
System.Numerics.Vector3 intermediate;
inverseMass = characterBody.InverseMass;
m11 = inverseMass;
m22 = inverseMass;
//Scale the inertia and mass of the support. This will make the solver view the object as 'heavier' with respect to horizontal motion.
Matrix3x3 inertiaInverse = supportEntity.InertiaTensorInverse;
Matrix3x3.Multiply(ref inertiaInverse, supportForceFactor, out inertiaInverse);
float extra;
inverseMass = supportForceFactor * supportEntity.InverseMass;
Matrix3x3.Transform(ref angularJacobianB1, ref inertiaInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularJacobianB1, out extra);
m11 += inverseMass + extra;
Vector3Ex.Dot(ref intermediate, ref angularJacobianB2, out extra);
m1221 += extra;
Matrix3x3.Transform(ref angularJacobianB2, ref inertiaInverse, out intermediate);
Vector3Ex.Dot(ref intermediate, ref angularJacobianB2, out extra);
m22 += inverseMass + extra;
massMatrix.M11 = m11;
massMatrix.M12 = m1221;
massMatrix.M21 = m1221;
massMatrix.M22 = m22;
Matrix2x2.Invert(ref massMatrix, out massMatrix);
}
else
{
//If we're not standing on a dynamic entity, then the mass matrix is defined entirely by the character.
Matrix2x2.CreateScale(characterBody.Mass, out massMatrix);
}
//If we're trying to stand still on an object that's moving, use a position correction term to keep the character
//from drifting due to accelerations.
//First thing to do is to check to see if we're moving into a traction/trying to stand still state from a
//non-traction || trying to move state. Either that, or we've switched supports and need to update the offset.
if (supportEntity != null && ((wasTryingToMove && !isTryingToMove) || (!hadTraction && supportFinder.HasTraction) || supportEntity != previousSupportEntity))
{
//We're transitioning into a new 'use position correction' state.
//Force a recomputation of the local offset.
//The time since transition is used as a flag.
timeSinceTransition = 0;
}
//The state is now up to date. Compute an error and velocity bias, if needed.
if (!isTryingToMove && MovementMode == MovementMode.Traction && supportEntity != null)
{
var distanceToBottomOfCharacter = supportFinder.BottomDistance;
if (timeSinceTransition >= 0 && timeSinceTransition < timeUntilPositionAnchor)
timeSinceTransition += dt;
if (timeSinceTransition >= timeUntilPositionAnchor)
{
Vector3Ex.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
timeSinceTransition = -1; //Negative 1 means that the offset has been computed.
}
if (timeSinceTransition < 0)
{
System.Numerics.Vector3 targetPosition;
Vector3Ex.Multiply(ref downDirection, distanceToBottomOfCharacter, out targetPosition);
targetPosition += characterBody.Position;
System.Numerics.Vector3 worldSupportLocation = Matrix3x3.Transform(positionLocalOffset, supportEntity.OrientationMatrix) + supportEntity.Position;
System.Numerics.Vector3 error;
Vector3Ex.Subtract(ref targetPosition, ref worldSupportLocation, out error);
//If the error is too large, then recompute the offset. We don't want the character rubber banding around.
if (error.LengthSquared() > PositionAnchorDistanceThreshold * PositionAnchorDistanceThreshold)
{
Vector3Ex.Multiply(ref downDirection, distanceToBottomOfCharacter, out positionLocalOffset);
positionLocalOffset = (positionLocalOffset + characterBody.Position) - supportEntity.Position;
positionLocalOffset = Matrix3x3.TransformTranspose(positionLocalOffset, supportEntity.OrientationMatrix);
positionCorrectionBias = new System.Numerics.Vector2();
}
else
{
//The error in world space is now available. We can't use this error to directly create a velocity bias, though.
//It needs to be transformed into constraint space where the constraint operates.
//Use the jacobians!
Vector3Ex.Dot(ref error, ref linearJacobianA1, out positionCorrectionBias.X);
Vector3Ex.Dot(ref error, ref linearJacobianA2, out positionCorrectionBias.Y);
//Scale the error so that a portion of the error is resolved each frame.
Vector2Ex.Multiply(ref positionCorrectionBias, .2f / dt, out positionCorrectionBias);
}
}
}
else
{
timeSinceTransition = 0;
positionCorrectionBias = new System.Numerics.Vector2();
}
wasTryingToMove = isTryingToMove;
hadTraction = supportFinder.HasTraction;
previousSupportEntity = supportEntity;
}
public static XnaVector2 ToXnaVector2(this NumVector2 value)
{
return(new XnaVector2(value.X, value.Y));
}
public void Init(IEnumerable <BearItemController> player1Objective,
IEnumerable <BearItemController> player2Objective)
{
_text.gameObject.SetActive(false);
int playerQuantity = ApplicationController.Instance.GetNextGamePlayerQuantity();
List <BearItem> bearItems = new List <BearItem>();
foreach (BearItemController bearItemController in _bearItemsControllers)
{
bearItemController.Init();
bearItems.Add(bearItemController.BearItem);
}
List <BearPart> player1BearItems = player1Objective
.Select(bearItemController => bearItemController.BearItem.BearPart).ToList();
List <BearPart> player2BearItems = player2Objective
.Select(bearItemController => bearItemController.BearItem.BearPart).ToList();
List <List <BearPart> > playerObjectives = new List <List <BearPart> > {
player1BearItems, player2BearItems
};
List <Player> players = new List <Player>();
List <PlayerController> toDestroyPlayerControllers = new List <PlayerController>();
List <List <GameObject> > spawnRects = new List <List <GameObject> >();
int i;
for (i = 0; i < _spawnPoints.Count; i++)
{
if (i % 2 == 0)
{
spawnRects.Add(new List <GameObject>());
}
spawnRects[i / 2].Add(_spawnPoints[i]);
}
for (i = 0; i < playerQuantity; i++)
{
_playersControllers[i].name = "Player" + i;
System.Numerics.Vector2 bottomLeft = spawnRects[i][0].transform.position.ToWorldVector2();
System.Numerics.Vector2 topRight = spawnRects[i][1].transform.position.ToWorldVector2();
Rect spawnPos = new Rect(bottomLeft, topRight);
Player newPlayer = new Player(_playersControllers[i].name, spawnPos.Center(),
new Objective(spawnPos, playerObjectives[i]));
_playersControllers[i].Init(newPlayer);
players.Add(newPlayer);
}
for (; i < _playersControllers.Count; i++)
{
toDestroyPlayerControllers.Add(_playersControllers[i]);
}
foreach (PlayerController toDestroyPlayerController in toDestroyPlayerControllers)
{
_playersControllers.Remove(toDestroyPlayerController);
Destroy(toDestroyPlayerController.gameObject);
}
World world = new World(new WorldConfig(0.62f), players, bearItems);
_worldController.Init(world);
StartTicking();
}
public static System.Windows.Point ToDevicePoint(this System.Numerics.Vector2 p)
{
return(new System.Windows.Point(p.X, p.Y));
}
public SvgQuadraticCurveSegment(System.Numerics.Vector2 start, System.Numerics.Vector2 controlPoint, System.Numerics.Vector2 end)
{
Start = start;
ControlPoint = controlPoint;
End = end;
}
public PointF(System.Numerics.Vector2 vector)
{
throw null;
}
public static void DrawPixelLine <TPixel>(this SpanTensor2 <TPixel> bitmap, XY a, XY b, TPixel color)
where TPixel : unmanaged
{
var bounds = bitmap.Dimensions;
var ab = b - a;
if (Math.Abs(ab.X) <= 1 && Math.Abs(ab.Y) <= 1)
// can't loop; draw a single pixel
{
int x = (int)a.X;
int y = (int)a.Y;
if (bounds.ContainsIndices(y, x))
{
bitmap[y].Span[x] = color;
}
return;
}
if (Math.Abs(ab.X) > Math.Abs(ab.Y))
// loop from left to right
{
var ptr = a;
var max = b.X;
if (ab.X < 0)
{
ab = -ab; ptr = b; max = a.X;
}
var d = new XY(1, ab.Y / ab.X);
while (ptr.X <= max)
{
int x = (int)ptr.X;
int y = (int)ptr.Y;
if (bounds.ContainsIndices(y, x))
{
bitmap[y].Span[x] = color;
}
ptr += d;
}
}
else
// loop from top to bottom
{
var ptr = a;
var max = b.Y;
if (ab.Y < 0)
{
ab = -ab; ptr = b; max = a.Y;
}
var d = new XY(ab.X / ab.Y, 1);
while (ptr.Y <= max)
{
int x = (int)ptr.X;
int y = (int)ptr.Y;
if (bounds.ContainsIndices(y, x))
{
bitmap[y].Span[x] = color;
}
ptr += d;
}
}
}
///<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.
System.Numerics.Quaternion relativeRotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisB, ref basis.primaryAxis, out relativeRotation);
System.Numerics.Vector3 axis;
float angle;
QuaternionEx.GetAxisAngleFromQuaternion(ref relativeRotation, out axis, out angle);
#if !WINDOWS
System.Numerics.Vector3 axisAngle = new System.Numerics.Vector3();
#else
System.Numerics.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;
Vector3Ex.Dot(ref axisAngle, ref basis.xAxis, out angleX);
float angleY;
Vector3Ex.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
System.Numerics.Vector2 tangent = new System.Numerics.Vector2();
#else
System.Numerics.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.
QuaternionEx.Conjugate(ref relativeRotation, out relativeRotation);
System.Numerics.Vector3 sphereTangentX, sphereTangentY;
QuaternionEx.Transform(ref basis.xAxis, ref relativeRotation, out sphereTangentX);
QuaternionEx.Transform(ref basis.yAxis, ref relativeRotation, out sphereTangentY);
Vector3Ex.Multiply(ref sphereTangentX, tangent.X, out jacobianA); //not actually jA, just storing it there.
Vector3Ex.Multiply(ref sphereTangentY, tangent.Y, out jacobianB); //not actually jB, just storing it there.
Vector3Ex.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
Vector3Ex.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity);
Vector3Ex.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;
System.Numerics.Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3Ex.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);
Vector3Ex.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
entryB = 0;
//Compute the inverse mass matrix
velocityToImpulse = 1 / (softness + entryA + entryB);
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">System.Numerics.Vector2 to transform.</param>
/// <param name="matrix">System.Numerics.Matrix4x4 to use as the transformation.</param>
/// <param name="result">Product of the transformation.</param>
public static void Transform(ref System.Numerics.Vector2 v, ref Matrix2x2 matrix, out System.Numerics.Vector2 result)
{
float vX = v.X;
float vY = v.Y;
#if !WINDOWS
result = new System.Numerics.Vector2();
#endif
result.X = vX * matrix.M11 + vY * matrix.M21;
result.Y = vX * matrix.M12 + vY * matrix.M22;
}
public void RenderItem(Entity entity, byte mode)
{
if (entity.Address != 0 && entity.IsValid)
{
var className = GameController.Files.BaseItemTypes.Translate(entity.Path).ClassName;
if (!entity.HasComponent <ExileCore.PoEMemory.Components.Map>() && !className.Contains("HeistContract"))
{
return;
}
if (className.Contains("HeistContract") && entity.GetComponent <Mods>().ItemRarity == ItemRarity.Normal)
{
return;
}
var serverData = ingameState.ServerData;
var bonusComp = serverData.BonusCompletedAreas;
var awakeComp = serverData.AwakenedAreas;
var comp = serverData.CompletedAreas;
var modsComponent = entity.GetComponent <Mods>() ?? null;
if (Settings.AlwaysShowTooltip || modsComponent != null && modsComponent.ItemRarity != ItemRarity.Normal && modsComponent.ItemMods.Count() > 0)
{
List <Warning> activeWarnings = new List <Warning>();
nuVector4 nameCol = GetRarityColor(entity.GetComponent <Mods>().ItemRarity);
var mapComponent = entity.GetComponent <ExileCore.PoEMemory.Components.Map>() ?? null;
int packSize = 0;
int quantity = entity.GetComponent <Quality>()?.ItemQuality ?? 0;
if (modsComponent != null && modsComponent.ItemRarity != ItemRarity.Unique)
{
foreach (var mod in modsComponent.ItemMods.Where(x =>
!x.Group.Contains("MapAtlasInfluence") &&
!x.Group.Contains("MapElderContainsBoss") &&
!x.Name.Equals("InfectedMap") &&
!x.Name.Equals("MapZanaSubAreaMissionDetails")
))
{
quantity += mod.Value1;
packSize += mod.Value3;
if (WarningDictionary.Where(x => mod.Name.Contains(x.Key)).Any())
{
activeWarnings.Add(WarningDictionary.Where(x => mod.Name.Contains(x.Key)).FirstOrDefault().Value);
}
}
}
if (Settings.AlwaysShowTooltip || activeWarnings.Count > 0 || Settings.ShowModCount || Settings.ShowQuantityPercent || Settings.ShowPackSizePercent)
{
// Get mouse position
nuVector2 mousePos = new nuVector2(MouseLite.GetCursorPositionVector().X + 24, MouseLite.GetCursorPositionVector().Y);
if (Settings.PadForNinjaPricer)
{
mousePos = new nuVector2(MouseLite.GetCursorPositionVector().X + 24, MouseLite.GetCursorPositionVector().Y + 56);
}
// Parsing inventory, don't use mousePos
if (mode == 1)
{
var framePos = ingameState.UIHover.Parent.Parent.GetClientRect().TopRight;
mousePos.X = framePos.X;
mousePos.Y = framePos.Y - 50 + mPad;
}
var _opened = true;
if (ImGui.Begin($"{entity.Address}", ref _opened,
ImGuiWindowFlags.NoScrollbar | ImGuiWindowFlags.AlwaysAutoResize | ImGuiWindowFlags.NoMove | ImGuiWindowFlags.NoResize |
ImGuiWindowFlags.NoInputs | ImGuiWindowFlags.NoSavedSettings | ImGuiWindowFlags.NoTitleBar | ImGuiWindowFlags.NoNavInputs))
{
if (!className.Contains("HeistContract"))
{
if (mode == 1 || Settings.ShowMapName)
{
if (!Settings.ShowCompletion)
{
ImGui.TextColored(nameCol, $"[T{mapComponent.Tier}] {entity.GetComponent<Base>().Name.Replace(" Map", "")}");
}
else
{
ImGui.TextColored(nameCol, $"[T{mapComponent.Tier}] {entity.GetComponent<Base>().Name.Replace(" Map", "")}");
if (!awakeComp.Contains(mapComponent.Area))
{
ImGui.SameLine(); ImGui.TextColored(new nuVector4(1f, 0f, 0f, 1f), $"A");
}
if (!bonusComp.Contains(mapComponent.Area))
{
ImGui.SameLine(); ImGui.TextColored(new nuVector4(1f, 0f, 0f, 1f), $"B");
}
if (!comp.Contains(mapComponent.Area))
{
ImGui.SameLine(); ImGui.TextColored(new nuVector4(1f, 0f, 0f, 1f), $"C");
}
ImGui.PushStyleColor(ImGuiCol.Separator, new nuVector4(1f, 1f, 1f, 0.2f));
}
}
}
if (mode == 1)
{
if (ZanaMods.TryGetValue(modsComponent.ItemMods.FirstOrDefault(x => x.Name == "MapZanaSubAreaMissionDetails").Value2, out string modName))
{
if (modName.Contains("Guardian"))
{
ImGui.TextColored(new nuVector4(0.5f, 1f, 0.45f, 1f), $"{modName}");
}
else
{
ImGui.TextColored(new nuVector4(0.9f, 0.85f, 0.65f, 1f), $"{modName}");
}
}
else
{
ImGui.TextColored(new nuVector4(0.9f, 0.85f, 0.65f, 1f), $"Unknown Zana Mission: {modsComponent.ItemMods.FirstOrDefault(x => x.Name == "MapZanaSubAreaMissionDetails").Value2}");
}
}
if (!className.Contains("HeistContract"))
{
// Quantity and Pack Size
nuVector4 qCol = new nuVector4(1f, 1f, 1f, 1f);
if (Settings.ColourQuantityPercent)
{
if (quantity < Settings.ColourQuantity)
{
qCol = new nuVector4(1f, 0.4f, 0.4f, 1f);
}
else
{
qCol = new nuVector4(0.4f, 1f, 0.4f, 1f);
}
}
if (Settings.ShowQuantityPercent && quantity != 0 && Settings.ShowPackSizePercent && packSize != 0)
{
ImGui.TextColored(qCol, $"{quantity}%% Quant");
ImGui.SameLine(); ImGui.TextColored(new nuVector4(1f, 1f, 1f, 1f), $"{packSize}%% Pack Size");
}
else if (Settings.ShowQuantityPercent && quantity != 0)
{
ImGui.TextColored(qCol, $"{quantity}%% Quantity");
}
else if (Settings.ShowPackSizePercent && packSize != 0)
{
ImGui.TextColored(new nuVector4(1f, 1f, 1f, 1f), $"{packSize}%% Pack Size");
}
// Separator
if (Settings.HorizontalLines && modsComponent.ItemMods.Count != 0 && (Settings.ShowModCount || Settings.ShowModWarnings))
{
if (Settings.ShowLineForZanaMaps && mode == 1 || mode == 0)
{
ImGui.Separator();
}
}
}
// Count Mods
if (Settings.ShowModCount && modsComponent.ItemMods.Count != 0)
{
if (entity.GetComponent <Base>().isCorrupted)
{
ImGui.TextColored(new nuVector4(1f, 0.33f, 0.33f, 1f), $"{modsComponent.ItemMods.Count} Mods");
}
else
{
ImGui.TextColored(new nuVector4(1f, 1f, 1f, 1f), $"{modsComponent.ItemMods.Count} Mods");
}
}
// Mod Warnings
if (Settings.ShowModWarnings)
{
foreach (Warning warning in activeWarnings.OrderBy(x => x.Color.ToString()).ToList())
{
ImGui.TextColored(warning.Color, warning.Text);
}
}
// Color background
ImGui.PushStyleColor(ImGuiCol.WindowBg, new System.Numerics.Vector4(0.256f, 0.256f, 0.256f, 1f));
// Detect and adjust for edges
var size = ImGui.GetWindowSize();
var pos = ImGui.GetWindowPos();
if ((mousePos.X + size.X) > windowArea.Width)
{
//ImGui.Text($"Overflow by {(mousePos.X + size.X) - windowArea.Width}");
ImGui.SetWindowPos(new nuVector2(mousePos.X - ((mousePos.X + size.X) - windowArea.Width) - 4, mousePos.Y + 24), ImGuiCond.Always);
}
else
{
ImGui.SetWindowPos(mousePos, ImGuiCond.Always);
}
// padding when parsing an inventory
if (mode == 1)
{
mPad += (int)size.Y + 2;
}
}
ImGui.End();
}
}
}
}
/// <inheritdoc />
protected override void PostStep()
{
if (Input.GetKeyDown(KeyCode.A))
{
_positionB.X -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.D))
{
_positionB.X += 0.1f;
}
if (Input.GetKeyDown(KeyCode.S))
{
_positionB.Y -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.W))
{
_positionB.Y += 0.1f;
}
if (Input.GetKeyDown(KeyCode.Q))
{
_angleB += 0.1f * Settings.Pi;
}
if (Input.GetKeyDown(KeyCode.E))
{
_angleB -= 0.1f * Settings.Pi;
}
_transformB.Set(_positionB, _angleB);
var input = new DistanceInput();
input.ProxyA.Set(_polygonA, 0);
input.ProxyB.Set(_polygonB, 0);
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
var cache = new SimplexCache();
DistanceAlgorithm.Distance(out var output, ref cache, input);
DrawString($"distance = {output.Distance}");
DrawString($"iterations = {output.Iterations}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
var x1 = output.PointA;
var x2 = output.PointB;
var c1 = Color.FromArgb(255, 0, 0);
Drawer.DrawPoint(x1, 4.0f, c1);
var c2 = Color.FromArgb(255, 255, 0);
Drawer.DrawPoint(x2, 4.0f, c2);
}
public EditorIcon(System.Numerics.Vector2 start, System.Numerics.Vector2 size, System.Numerics.Vector2 imageSize)
{
uvMin = new System.Numerics.Vector2(start.X / imageSize.X, start.Y / imageSize.Y);
uvMax = new System.Numerics.Vector2(uvMin.X + (size.X / imageSize.X), uvMin.Y + (size.Y / imageSize.Y));
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">System.Numerics.Vector2 to transform.</param>
/// <param name="matrix">System.Numerics.Matrix4x4 to use as the transformation.</param>
/// <param name="result">Product of the transformation.</param>
public static void Transform(ref System.Numerics.Vector2 v, ref Matrix2x2 matrix, out System.Numerics.Vector2 result)
{
float vX = v.X;
float vY = v.Y;
#if !WINDOWS
result = new System.Numerics.Vector2();
#endif
result.X = vX * matrix.M11 + vY * matrix.M21;
result.Y = vX * matrix.M12 + vY * matrix.M22;
}
public SvgMoveToSegment(System.Numerics.Vector2 position)
{
Start = position;
End = position;
}
/** Used in polygon composition to composit polygons into scan lines
* Combining polya and polyb into one super-polygon stored in polya.
**/
static void CombLeft(ref GeomPoly polya, ref GeomPoly polyb)
{
CxFastList <System.Numerics.Vector2> ap = polya.Points;
CxFastList <System.Numerics.Vector2> bp = polyb.Points;
CxFastListNode <System.Numerics.Vector2> ai = ap.Begin();
CxFastListNode <System.Numerics.Vector2> bi = bp.Begin();
System.Numerics.Vector2 b = bi.Elem();
CxFastListNode <System.Numerics.Vector2> prea = null;
while (ai != ap.End())
{
System.Numerics.Vector2 a = ai.Elem();
if (VecDsq(a, b) < Settings.Epsilon)
{
//ignore shared vertex if parallel
if (prea != null)
{
System.Numerics.Vector2 a0 = prea.Elem();
b = bi.Next().Elem();
System.Numerics.Vector2 u = a - a0;
//vec_new(u); vec_sub(a.p.p, a0.p.p, u);
System.Numerics.Vector2 v = b - a;
//vec_new(v); vec_sub(b.p.p, a.p.p, v);
float dot = VecCross(u, v);
if (dot * dot < Settings.Epsilon)
{
ap.Erase(prea, ai);
polya.Length--;
ai = prea;
}
}
//insert polyb into polya
bool fst = true;
CxFastListNode <System.Numerics.Vector2> preb = null;
while (!bp.Empty())
{
System.Numerics.Vector2 bb = bp.Front();
bp.Pop();
if (!fst && !bp.Empty())
{
ai = ap.Insert(ai, bb);
polya.Length++;
preb = ai;
}
fst = false;
}
//ignore shared vertex if parallel
ai = ai.Next();
System.Numerics.Vector2 a1 = ai.Elem();
ai = ai.Next();
if (ai == ap.End())
{
ai = ap.Begin();
}
System.Numerics.Vector2 a2 = ai.Elem();
System.Numerics.Vector2 a00 = preb.Elem();
System.Numerics.Vector2 uu = a1 - a00;
//vec_new(u); vec_sub(a1.p, a0.p, u);
System.Numerics.Vector2 vv = a2 - a1;
//vec_new(v); vec_sub(a2.p, a1.p, v);
float dot1 = VecCross(uu, vv);
if (dot1 * dot1 < Settings.Epsilon)
{
ap.Erase(preb, preb.Next());
polya.Length--;
}
return;
}
prea = ai;
ai = ai.Next();
}
}
void IInputListener.OnMouseMoved(System.Numerics.Vector2 mousePos)
{
if (!_dragging)
{
return;
}
float width = GetWidth(), height = GetHeight();
float windowX = GetX(), windowY = GetY();
var stage = GetStage();
var parentWidth = stage.GetWidth();
var parentHeight = stage.GetHeight();
var clampPosition = _keepWithinStage && GetParent() == stage.GetRoot();
if ((edge & MOVE) != 0)
{
float amountX = mousePos.X - startX, amountY = mousePos.Y - startY;
if (clampPosition)
{
if (windowX + amountX < 0)
{
amountX = -windowX;
}
if (windowY + amountY < 0)
{
amountY = -windowY;
}
if (windowX + width + amountX > parentWidth)
{
amountX = parentWidth - windowX - width;
}
if (windowY + height + amountY > parentHeight)
{
amountY = parentHeight - windowY - height;
}
}
windowX += amountX;
windowY += amountY;
}
if ((edge & (int)AlignInternal.Left) != 0)
{
float amountX = mousePos.X - startX;
if (width - amountX < MinWidth)
{
amountX = -(MinWidth - width);
}
if (clampPosition && windowX + amountX < 0)
{
amountX = -windowX;
}
width -= amountX;
windowX += amountX;
}
if ((edge & (int)AlignInternal.Top) != 0)
{
float amountY = mousePos.Y - startY;
if (height - amountY < MinHeight)
{
amountY = -(MinHeight - height);
}
if (clampPosition && windowY + amountY < 0)
{
amountY = -windowY;
}
height -= amountY;
windowY += amountY;
}
if ((edge & (int)AlignInternal.Right) != 0)
{
float amountX = mousePos.X - lastX;
if (width + amountX < MinWidth)
{
amountX = MinWidth - width;
}
if (clampPosition && windowX + width + amountX > parentWidth)
{
amountX = parentWidth - windowX - width;
}
width += amountX;
}
if ((edge & (int)AlignInternal.Bottom) != 0)
{
float amountY = mousePos.Y - lastY;
if (height + amountY < MinHeight)
{
amountY = MinHeight - height;
}
if (clampPosition && windowY + height + amountY > parentHeight)
{
amountY = parentHeight - windowY - height;
}
height += amountY;
}
lastX = mousePos.X;
lastY = mousePos.Y;
SetBounds(Mathf.Round(windowX), Mathf.Round(windowY), Mathf.Round(width), Mathf.Round(height));
}
public SvgLineSegment(System.Numerics.Vector2 start, System.Numerics.Vector2 end)
{
Start = start;
End = end;
}
/// <summary>
/// Marching squares over the given domain using the mesh defined via the dimensions
/// (wid,hei) to build a set of polygons such that f(x,y) less than 0, using the given number
/// 'bin' for recursive linear inteprolation along cell boundaries.
///
/// if 'comb' is true, then the polygons will also be composited into larger possible concave
/// polygons.
/// </summary>
/// <param name="domain"></param>
/// <param name="cellWidth"></param>
/// <param name="cellHeight"></param>
/// <param name="f"></param>
/// <param name="lerpCount"></param>
/// <param name="combine"></param>
/// <returns></returns>
public static List <Vertices> DetectSquares(AABB domain, float cellWidth, float cellHeight, sbyte[,] f,
int lerpCount, bool combine)
{
var ret = new CxFastList <GeomPoly>();
var verticesList = new List <Vertices>();
//NOTE: removed assignments as they were not used.
List <GeomPoly> polyList;
GeomPoly gp;
int xn = (int)(domain.Extents.X * 2 / cellWidth);
bool xp = xn == (domain.Extents.X * 2 / cellWidth);
int yn = (int)(domain.Extents.Y * 2 / cellHeight);
bool yp = yn == (domain.Extents.Y * 2 / cellHeight);
if (!xp)
{
xn++;
}
if (!yp)
{
yn++;
}
sbyte[,] fs = new sbyte[xn + 1, yn + 1];
GeomPolyVal[,] ps = new GeomPolyVal[xn + 1, yn + 1];
//populate shared function lookups.
for (int x = 0; x < xn + 1; x++)
{
int x0;
if (x == xn)
{
x0 = (int)domain.UpperBound.X;
}
else
{
x0 = (int)(x * cellWidth + domain.LowerBound.X);
}
for (int y = 0; y < yn + 1; y++)
{
int y0;
if (y == yn)
{
y0 = (int)domain.UpperBound.Y;
}
else
{
y0 = (int)(y * cellHeight + domain.LowerBound.Y);
}
fs[x, y] = f[x0, y0];
}
}
//generate sub-polys and combine to scan lines
for (int y = 0; y < yn; y++)
{
float y0 = y * cellHeight + domain.LowerBound.Y;
float y1;
if (y == yn - 1)
{
y1 = domain.UpperBound.Y;
}
else
{
y1 = y0 + cellHeight;
}
GeomPoly pre = null;
for (int x = 0; x < xn; x++)
{
float x0 = x * cellWidth + domain.LowerBound.X;
float x1;
if (x == xn - 1)
{
x1 = domain.UpperBound.X;
}
else
{
x1 = x0 + cellWidth;
}
gp = new GeomPoly();
int key = MarchSquare(f, fs, ref gp, x, y, x0, y0, x1, y1, lerpCount);
if (gp.Length != 0)
{
if (combine && pre != null && (key & 9) != 0)
{
CombLeft(ref pre, ref gp);
gp = pre;
}
else
{
ret.Add(gp);
}
ps[x, y] = new GeomPolyVal(gp, key);
}
else
{
gp = null;
}
pre = gp;
}
}
if (!combine)
{
polyList = ret.GetListOfElements();
foreach (GeomPoly poly in polyList)
{
verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
}
return(verticesList);
}
//combine scan lines together
for (int y = 1; y < yn; y++)
{
int x = 0;
while (x < xn)
{
GeomPolyVal p = ps[x, y];
//skip along scan line if no polygon exists at this point
if (p == null)
{
x++;
continue;
}
//skip along if current polygon cannot be combined above.
if ((p.Key & 12) == 0)
{
x++;
continue;
}
//skip along if no polygon exists above.
GeomPolyVal u = ps[x, y - 1];
if (u == null)
{
x++;
continue;
}
//skip along if polygon above cannot be combined with.
if ((u.Key & 3) == 0)
{
x++;
continue;
}
float ax = x * cellWidth + domain.LowerBound.X;
float ay = y * cellHeight + domain.LowerBound.Y;
CxFastList <System.Numerics.Vector2> bp = p.GeomP.Points;
CxFastList <System.Numerics.Vector2> ap = u.GeomP.Points;
//skip if it's already been combined with above polygon
if (u.GeomP == p.GeomP)
{
x++;
continue;
}
//combine above (but disallow the hole thingies
CxFastListNode <System.Numerics.Vector2> bi = bp.Begin();
while (Square(bi.Elem().Y - ay) > Settings.Epsilon || bi.Elem().X < ax)
{
bi = bi.Next();
}
//NOTE: Unused
//System.Numerics.Vector2 b0 = bi.elem();
System.Numerics.Vector2 b1 = bi.Next().Elem();
if (Square(b1.Y - ay) > Settings.Epsilon)
{
x++;
continue;
}
bool brk = true;
CxFastListNode <System.Numerics.Vector2> ai = ap.Begin();
while (ai != ap.End())
{
if (VecDsq(ai.Elem(), b1) < Settings.Epsilon)
{
brk = false;
break;
}
ai = ai.Next();
}
if (brk)
{
x++;
continue;
}
CxFastListNode <System.Numerics.Vector2> bj = bi.Next().Next();
if (bj == bp.End())
{
bj = bp.Begin();
}
while (bj != bi)
{
ai = ap.Insert(ai, bj.Elem()); // .clone()
bj = bj.Next();
if (bj == bp.End())
{
bj = bp.Begin();
}
u.GeomP.Length++;
}
//u.p.simplify(float.Epsilon,float.Epsilon);
//
ax = x + 1;
while (ax < xn)
{
GeomPolyVal p2 = ps[(int)ax, y];
if (p2 == null || p2.GeomP != p.GeomP)
{
ax++;
continue;
}
p2.GeomP = u.GeomP;
ax++;
}
ax = x - 1;
while (ax >= 0)
{
GeomPolyVal p2 = ps[(int)ax, y];
if (p2 == null || p2.GeomP != p.GeomP)
{
ax--;
continue;
}
p2.GeomP = u.GeomP;
ax--;
}
ret.Remove(p.GeomP);
p.GeomP = u.GeomP;
x = (int)((bi.Next().Elem().X - domain.LowerBound.X) / cellWidth) + 1;
//x++; this was already commented out!
}
}
polyList = ret.GetListOfElements();
foreach (GeomPoly poly in polyList)
{
verticesList.Add(new Vertices(poly.Points.GetListOfElements()));
}
return(verticesList);
}
void IInputListener.OnMouseUp(System.Numerics.Vector2 mousePos)
{
_dragging = false;
}
public static void Write(byte[] bytes, int i, System.Numerics.Vector2 value)
{
Write(bytes, i, value.X);
Write(bytes, i + 4, value.Y);
}
bool IInputListener.OnMousePressed(System.Numerics.Vector2 mousePos)
{
float width = GetWidth(), height = GetHeight();
edge = 0;
if (_isResizable && mousePos.X >= 0 && mousePos.X < width && mousePos.Y >= 0 && mousePos.Y < height)
{
if (mousePos.X < resizeBorderSize)
{
edge |= (int)AlignInternal.Left;
}
if (mousePos.X > width - resizeBorderSize)
{
edge |= (int)AlignInternal.Right;
}
if (mousePos.Y < resizeBorderSize)
{
edge |= (int)AlignInternal.Top;
}
if (mousePos.Y > height - resizeBorderSize)
{
edge |= (int)AlignInternal.Bottom;
}
int tempResizeBorderSize = resizeBorderSize;
if (edge != 0)
{
tempResizeBorderSize += 25;
}
if (mousePos.X < tempResizeBorderSize)
{
edge |= (int)AlignInternal.Left;
}
if (mousePos.X > width - tempResizeBorderSize)
{
edge |= (int)AlignInternal.Right;
}
if (mousePos.Y < tempResizeBorderSize)
{
edge |= (int)AlignInternal.Top;
}
if (mousePos.Y > height - tempResizeBorderSize)
{
edge |= (int)AlignInternal.Bottom;
}
}
if (_isMovable && edge == 0 && mousePos.Y >= 0 && mousePos.Y <= GetPadTop() && mousePos.X >= 0 &&
mousePos.X <= width)
{
edge = MOVE;
}
_dragging = edge != 0;
startX = mousePos.X;
startY = mousePos.Y;
lastX = mousePos.X;
lastY = mousePos.Y;
return(true);
}
/// <inheritdoc />
protected override void PostStep()
{
if (Input.GetKeyDown(KeyCode.A))
{
_positionB.X -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.D))
{
_positionB.X += 0.1f;
}
if (Input.GetKeyDown(KeyCode.S))
{
_positionB.Y -= 0.1f;
}
if (Input.GetKeyDown(KeyCode.W))
{
_positionB.Y += 0.1f;
}
if (Input.GetKeyDown(KeyCode.Q))
{
_angleB += 0.1f * Settings.Pi;
}
if (Input.GetKeyDown(KeyCode.E))
{
_angleB -= 0.1f * Settings.Pi;
}
_transformB.Set(_positionB, _angleB);
var manifold = new Manifold();
CollisionUtils.CollidePolygons(ref manifold, _polygonA, _transformA, _polygonB, _transformB);
var worldManifold = new WorldManifold();
worldManifold.Initialize(manifold, _transformA, _polygonA.Radius, _transformB, _polygonB.Radius);
DrawString($"point count = {manifold.PointCount}");
{
var color = Color.FromArgb(230, 230, 230);
var v = new Vector2[Settings.MaxPolygonVertices];
for (var i = 0; i < _polygonA.Count; ++i)
{
v[i] = MathUtils.Mul(_transformA, _polygonA.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonA.Count, color);
for (var i = 0; i < _polygonB.Count; ++i)
{
v[i] = MathUtils.Mul(_transformB, _polygonB.Vertices[i]);
}
Drawer.DrawPolygon(v, _polygonB.Count, color);
}
for (var i = 0; i < manifold.PointCount; ++i)
{
Drawer.DrawPoint(worldManifold.Points[i], 4.0f, Color.FromArgb(230, 77, 77));
}
}
bool IInputListener.OnMousePressed(System.Numerics.Vector2 mousePos)
{
return(true);
}
public static bool TestPointTriangle(System.Numerics.Vector2 point, System.Numerics.Vector2 a, System.Numerics.Vector2 b, System.Numerics.Vector2 c)
{
// if point to the right of AB then outside triangle
if (Vector2Ext.Cross(point - a, b - a) < 0f)
{
return(false);
}
// if point to the right of BC then outside of triangle
if (Vector2Ext.Cross(point - b, c - b) < 0f)
{
return(false);
}
// if point to the right of ca then outside of triangle
if (Vector2Ext.Cross(point - c, a - c) < 0f)
{
return(false);
}
// point is in or on triangle
return(true);
}
void IInputListener.OnMouseMoved(System.Numerics.Vector2 mousePos)
{
}
public Car()
{
_speed = 50.0f;
Body ground;
{
var bd = new BodyDef();
ground = World.CreateBody(bd);
var shape = new EdgeShape();
var fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 0.0f;
fd.Friction = 0.6f;
shape.Set(new Vector2(-20.0f, 0.0f), new Vector2(20.0f, 0.0f));
ground.CreateFixture(fd);
float[] hs = { 0.25f, 1.0f, 4.0f, 0.0f, 0.0f, -1.0f, -2.0f, -2.0f, -1.25f, 0.0f };
float x = 20.0f, y1 = 0.0f, dx = 5.0f;
for (var i = 0; i < 10; ++i)
{
var y2 = hs[i];
shape.Set(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
for (var i = 0; i < 10; ++i)
{
var y2 = hs[i];
shape.Set(new Vector2(x, y1), new Vector2(x + dx, y2));
ground.CreateFixture(fd);
y1 = y2;
x += dx;
}
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 80.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 10.0f, 5.0f));
ground.CreateFixture(fd);
x += 20.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x + 40.0f, 0.0f));
ground.CreateFixture(fd);
x += 40.0f;
shape.Set(new Vector2(x, 0.0f), new Vector2(x, 20.0f));
ground.CreateFixture(fd);
}
// Teeter
{
var bd = new BodyDef();
bd.Position.Set(140.0f, 1.0f);
bd.BodyType = BodyType.DynamicBody;
var body = World.CreateBody(bd);
var box = new PolygonShape();
box.SetAsBox(10.0f, 0.25f);
body.CreateFixture(box, 1.0f);
var jd = new RevoluteJointDef();
jd.Initialize(ground, body, body.GetPosition());
jd.LowerAngle = -8.0f * Settings.Pi / 180.0f;
jd.UpperAngle = 8.0f * Settings.Pi / 180.0f;
jd.EnableLimit = true;
World.CreateJoint(jd);
body.ApplyAngularImpulse(100.0f, true);
}
// Bridge
{
var N = 20;
var shape = new PolygonShape();
shape.SetAsBox(1.0f, 0.125f);
var fd = new FixtureDef();
fd.Shape = shape;
fd.Density = 1.0f;
fd.Friction = 0.6f;
var jd = new RevoluteJointDef();
var prevBody = ground;
for (var i = 0; i < N; ++i)
{
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(161.0f + 2.0f * i, -0.125f);
var body = World.CreateBody(bd);
body.CreateFixture(fd);
var anchor = new Vector2(160.0f + 2.0f * i, -0.125f);
jd.Initialize(prevBody, body, anchor);
World.CreateJoint(jd);
prevBody = body;
}
{
var anchor = new Vector2(160.0f + 2.0f * N, -0.125f);
jd.Initialize(prevBody, ground, anchor);
World.CreateJoint(jd);
}
}
// Boxes
{
var box = new PolygonShape();
box.SetAsBox(0.5f, 0.5f);
Body body = null;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(230.0f, 0.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 1.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 2.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 3.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
bd.Position.Set(230.0f, 4.5f);
body = World.CreateBody(bd);
body.CreateFixture(box, 0.5f);
}
// Car
{
var chassis = new PolygonShape();
var vertices = new Vector2[8];
vertices[0].Set(-1.5f, -0.5f);
vertices[1].Set(1.5f, -0.5f);
vertices[2].Set(1.5f, 0.0f);
vertices[3].Set(0.0f, 0.9f);
vertices[4].Set(-1.15f, 0.9f);
vertices[5].Set(-1.5f, 0.2f);
chassis.Set(vertices);
var circle = new CircleShape();
circle.Radius = 0.4f;
var bd = new BodyDef();
bd.BodyType = BodyType.DynamicBody;
bd.Position.Set(0.0f, 1.0f);
_car = World.CreateBody(bd);
_car.CreateFixture(chassis, 1.0f);
var fd = new FixtureDef();
fd.Shape = circle;
fd.Density = 1.0f;
fd.Friction = 0.9f;
bd.Position.Set(-1.0f, 0.35f);
_wheel1 = World.CreateBody(bd);
_wheel1.CreateFixture(fd);
bd.Position.Set(1.0f, 0.4f);
_wheel2 = World.CreateBody(bd);
_wheel2.CreateFixture(fd);
var jd = new WheelJointDef();
var axis = new Vector2(0.0f, 1.0f);
var mass1 = _wheel1.Mass;
var mass2 = _wheel2.Mass;
var hertz = 4.0f;
var dampingRatio = 0.7f;
var omega = 2.0f * Settings.Pi * hertz;
jd.Initialize(_car, _wheel1, _wheel1.GetPosition(), axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 20.0f;
jd.EnableMotor = true;
jd.Stiffness = mass1 * omega * omega;
jd.Damping = 2.0f * mass1 * dampingRatio * omega;
jd.LowerTranslation = -0.25f;
jd.UpperTranslation = 0.25f;
jd.EnableLimit = true;
_spring1 = (WheelJoint)World.CreateJoint(jd);
jd.Initialize(_car, _wheel2, _wheel2.GetPosition(), axis);
jd.MotorSpeed = 0.0f;
jd.MaxMotorTorque = 10.0f;
jd.EnableMotor = false;
jd.Stiffness = mass2 * omega * omega;
jd.Damping = 2.0f * mass2 * dampingRatio * omega;
jd.LowerTranslation = -0.25f;
jd.UpperTranslation = 0.25f;
jd.EnableLimit = true;
_spring2 = (WheelJoint)World.CreateJoint(jd);
}
}
void IInputListener.OnMouseUp(System.Numerics.Vector2 mousePos)
{
OnClick?.Invoke();
}
/// <summary>
/// Calculates and applies corrective impulses.
/// Called automatically by space.
/// </summary>
public override float SolveIteration()
{
#region Theory
//lambda = -mc * (Jv + b)
// PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ]
// [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ]
// [ ray -rax 0 ]
//
// PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ]
// [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ]
// [ rby -rbx 0 ]
// Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ]
// [ cx cy cz -cx -cy -cz ] [ vay ]
// [ vaz ]
// [ wax ]
// [ way ]
// [ waz ]
// [ vbx ]
// [ vby ]
// [ vbz ]
// [ wbx ]
// [ wby ]
// [ wbz ]
// va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ]
// [ cx * vax + cy * vay + cz * vaz ] [ c * va ]
// wa' = [ (PraT row 1) * wa ]
// [ (PraT row 2) * wa ]
// vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ]
// [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ]
// wb' = [ -(PrbT row 1) * wb ]
// [ -(PrbT row 2) * wb ]
// Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ]
// [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ]
// Jv = [ b * (va + wa x ra - vb - wb x rb) ]
// [ c * (va + wa x ra - vb - wb x rb) ]
//P = JT * lambda
#endregion
#if !WINDOWS
System.Numerics.Vector2 lambda = new System.Numerics.Vector2();
#else
System.Numerics.Vector2 lambda;
#endif
//float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2;
//Vector3Ex.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1);
//Vector3Ex.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2);
//wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z;
//wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z;
//Vector3Ex.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1);
//Vector3Ex.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2);
//wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z;
//wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z;
//lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X;
//lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y;
System.Numerics.Vector3 dv;
System.Numerics.Vector3 aVel, bVel;
Vector3Ex.Cross(ref connectionA.angularVelocity, ref rA, out aVel);
Vector3Ex.Add(ref aVel, ref connectionA.linearVelocity, out aVel);
Vector3Ex.Cross(ref connectionB.angularVelocity, ref rB, out bVel);
Vector3Ex.Add(ref bVel, ref connectionB.linearVelocity, out bVel);
Vector3Ex.Subtract(ref aVel, ref bVel, out dv);
Vector3Ex.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X);
Vector3Ex.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y);
lambda.X += biasVelocity.X + softness * accumulatedImpulse.X;
lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y;
//Convert to impulse
Matrix2x2.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda);
Vector2Ex.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse);
float x = lambda.X;
float y = lambda.Y;
//Apply impulse
#if !WINDOWS
System.Numerics.Vector3 impulse = new System.Numerics.Vector3();
System.Numerics.Vector3 torque= new System.Numerics.Vector3();
#else
System.Numerics.Vector3 impulse;
System.Numerics.Vector3 torque;
#endif
impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y;
impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y;
impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y;
if (connectionA.isDynamic)
{
torque.X = x * angularA1.X + y * angularA2.X;
torque.Y = x * angularA1.Y + y * angularA2.Y;
torque.Z = x * angularA1.Z + y * angularA2.Z;
connectionA.ApplyLinearImpulse(ref impulse);
connectionA.ApplyAngularImpulse(ref torque);
}
if (connectionB.isDynamic)
{
impulse.X = -impulse.X;
impulse.Y = -impulse.Y;
impulse.Z = -impulse.Z;
torque.X = x * angularB1.X + y * angularB2.X;
torque.Y = x * angularB1.Y + y * angularB2.Y;
torque.Z = x * angularB1.Z + y * angularB2.Z;
connectionB.ApplyLinearImpulse(ref impulse);
connectionB.ApplyAngularImpulse(ref torque);
}
return (Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
//Extracted from Box2D
/// <summary>
/// Returns the convex hull from the given vertices.
/// </summary>
/// <param name="vertices">The vertices.</param>
public static Vertices GetConvexHull(Vertices vertices)
{
if (vertices.Count <= 3)
{
return(vertices);
}
// Find the right most point on the hull
int i0 = 0;
float x0 = vertices[0].X;
for (int i = 1; i < vertices.Count; ++i)
{
float x = vertices[i].X;
if (x > x0 || (x == x0 && vertices[i].Y < vertices[i0].Y))
{
i0 = i;
x0 = x;
}
}
int[] hull = new int[vertices.Count];
int m = 0;
int ih = i0;
for (;;)
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < vertices.Count; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
System.Numerics.Vector2 r = vertices[ie] - vertices[hull[m]];
System.Numerics.Vector2 v = vertices[j] - vertices[hull[m]];
float c = MathUtils.Cross(ref r, ref v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
Vertices result = new Vertices(m);
// Copy vertices.
for (int i = 0; i < m; ++i)
{
result.Add(vertices[hull[i]]);
}
return(result);
}
/// <summary>
/// Computes one iteration of the constraint to meet the solver updateable's goal.
/// </summary>
/// <returns>The rough applied impulse magnitude.</returns>
public override float SolveIteration()
{
// lambda = -mc * (Jv + b)
// P = JT * lambda
System.Numerics.Vector3 velocity;
Vector3Ex.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity);
#if !WINDOWS
System.Numerics.Vector2 lambda = new System.Numerics.Vector2();
#else
System.Numerics.Vector2 lambda;
#endif
Vector3Ex.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X);
Vector3Ex.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y);
Vector2Ex.Add(ref lambda, ref biasVelocity, out lambda);
System.Numerics.Vector2 softnessImpulse;
Vector2Ex.Multiply(ref accumulatedImpulse, softness, out softnessImpulse);
Vector2Ex.Add(ref lambda, ref softnessImpulse, out lambda);
Matrix2x2.Transform(ref lambda, ref effectiveMassMatrix, out lambda);
Vector2Ex.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse);
#if !WINDOWS
System.Numerics.Vector3 impulse = new System.Numerics.Vector3();
#else
System.Numerics.Vector3 impulse;
#endif
impulse.X = worldConstrainedAxis1.X * lambda.X + worldConstrainedAxis2.X * lambda.Y;
impulse.Y = worldConstrainedAxis1.Y * lambda.X + worldConstrainedAxis2.Y * lambda.Y;
impulse.Z = worldConstrainedAxis1.Z * lambda.X + worldConstrainedAxis2.Z * lambda.Y;
if (connectionA.isDynamic)
{
connectionA.ApplyAngularImpulse(ref impulse);
}
if (connectionB.isDynamic)
{
Vector3Ex.Negate(ref impulse, out impulse);
connectionB.ApplyAngularImpulse(ref impulse);
}
return (Math.Abs(lambda.X) + Math.Abs(lambda.Y));
}
public void ShiftOrigin(System.Numerics.Vector2 newOrigin)
{
_tree.ShiftOrigin(newOrigin);
}
/// <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.
System.Numerics.Quaternion conjugate;
QuaternionEx.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3Ex.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
QuaternionEx.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
QuaternionEx.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 System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new System.Numerics.Vector3();
//Pull the hit into world space.
QuaternionEx.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.
System.Numerics.Vector2 planeDirection = new System.Numerics.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;
}
System.Numerics.Vector2 planeOrigin = new System.Numerics.Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2Ex.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
System.Numerics.Vector2 closestPoint;
Vector2Ex.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2Ex.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.
Vector3Ex.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3Ex.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 System.Numerics.Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
Vector3Ex.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
else
hit.Normal = new System.Numerics.Vector3();
//Pull the hit into world space.
QuaternionEx.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 System.Numerics.Vector3(0, halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
QuaternionEx.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 System.Numerics.Vector3(0, -halfLength, 0);
if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit))
{
//Pull the hit into world space.
QuaternionEx.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>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">System.Numerics.Vector2 to transform. Considered to be a row vector for purposes of multiplication.</param>
/// <param name="matrix">System.Numerics.Matrix4x4 to use as the transformation.</param>
/// <param name="result">Row vector product of the transformation.</param>
public static void Transform(ref System.Numerics.Vector3 v, ref Matrix3x2 matrix, out System.Numerics.Vector2 result)
{
#if !WINDOWS
result = new System.Numerics.Vector2();
#endif
result.X = v.X * matrix.M11 + v.Y * matrix.M21 + v.Z * matrix.M31;
result.Y = v.X * matrix.M12 + v.Y * matrix.M22 + v.Z * matrix.M32;
}
internal void CleanUp()
{
accumulatedImpulse = new System.Numerics.Vector2();
contactManifoldConstraint = null;
entityA = null;
entityB = null;
isActive = false;
}
static float VecDsq(System.Numerics.Vector2 a, System.Numerics.Vector2 b)
{
var d = a - b;
return(d.X * d.X + d.Y * d.Y);
}
static float VecCross(System.Numerics.Vector2 a, System.Numerics.Vector2 b)
{
return(a.X * b.Y - a.Y * b.X);
}
/** Look-up table to relate polygon key with the vertices that should be used for
* the sub polygon in marching squares
**/
/** Perform a single celled marching square for for the given cell defined by (x0,y0) (x1,y1)
* using the function f for recursive interpolation, given the look-up table 'fs' of
* the values of 'f' at cell vertices with the result to be stored in 'poly' given the actual
* coordinates of 'ax' 'ay' in the marching squares mesh.
**/
static int MarchSquare(sbyte[,] f, sbyte[,] fs, ref GeomPoly poly, int ax, int ay, float x0, float y0,
float x1, float y1, int bin)
{
//key lookup
int key = 0;
sbyte v0 = fs[ax, ay];
if (v0 < 0)
{
key |= 8;
}
sbyte v1 = fs[ax + 1, ay];
if (v1 < 0)
{
key |= 4;
}
sbyte v2 = fs[ax + 1, ay + 1];
if (v2 < 0)
{
key |= 2;
}
sbyte v3 = fs[ax, ay + 1];
if (v3 < 0)
{
key |= 1;
}
int val = _lookMarch[key];
if (val != 0)
{
CxFastListNode <System.Numerics.Vector2> pi = null;
for (int i = 0; i < 8; i++)
{
System.Numerics.Vector2 p;
if ((val & (1 << i)) != 0)
{
if (i == 7 && (val & 1) == 0)
{
poly.Points.Add(p = new System.Numerics.Vector2(x0, YLerp(y0, y1, x0, v0, v3, f, bin)));
}
else
{
if (i == 0)
{
p = new System.Numerics.Vector2(x0, y0);
}
else if (i == 2)
{
p = new System.Numerics.Vector2(x1, y0);
}
else if (i == 4)
{
p = new System.Numerics.Vector2(x1, y1);
}
else if (i == 6)
{
p = new System.Numerics.Vector2(x0, y1);
}
else if (i == 1)
{
p = new System.Numerics.Vector2(XLerp(x0, x1, y0, v0, v1, f, bin), y0);
}
else if (i == 5)
{
p = new System.Numerics.Vector2(XLerp(x0, x1, y1, v3, v2, f, bin), y1);
}
else if (i == 3)
{
p = new System.Numerics.Vector2(x1, YLerp(y0, y1, x1, v1, v2, f, bin));
}
else
{
p = new System.Numerics.Vector2(x0, YLerp(y0, y1, x0, v0, v3, f, bin));
}
pi = poly.Points.Insert(pi, p);
}
poly.Length++;
}
}
//poly.simplify(float.Epsilon,float.Epsilon);
}
return(key);
}
/// <summary>
/// Transforms the vector by the matrix.
/// </summary>
/// <param name="v">System.Numerics.Vector2 to transform. Considered to be a column vector for purposes of multiplication.</param>
/// <param name="matrix">System.Numerics.Matrix4x4 to use as the transformation.</param>
/// <param name="result">Column vector product of the transformation.</param>
public static void Transform(ref System.Numerics.Vector3 v, ref Matrix2x3 matrix, out System.Numerics.Vector2 result)
{
#if !WINDOWS
result = new System.Numerics.Vector2();
#endif
result.X = matrix.M11 * v.X + matrix.M12 * v.Y + matrix.M13 * v.Z;
result.Y = matrix.M21 * v.X + matrix.M22 * v.Y + matrix.M23 * v.Z;
}
protected string ToSvgString(System.Numerics.Vector2 point)
{
return(string.Format("{0} {1}", point.X, point.Y));
}
