public static bool IsInsideAngle(Point vectorOriginPoint, System.Windows.Vector v1, System.Windows.Vector v2, Point p)
{
var center = vectorOriginPoint;
//make angle with 2 vectors
var angle = System.Windows.Vector.AngleBetween(v1, v2);
var halfAngle = angle / 2;
v1.Normalize();
v2.Normalize();
//vector that bisects angle between v1 and v2
var bisector = v1 + v2;
bisector.Normalize();
var vectorToPoint = new System.Windows.Vector(p.X - center.X, p.Y - center.Y);
vectorToPoint.Normalize();
var AngleBetweenBisectorAndVectorToPoint = Math.Abs(System.Windows.Vector.AngleBetween(bisector, vectorToPoint));
//if angle between bisector and vector to point p is less than half of angle v1v2 then point lies inside the angle
return AngleBetweenBisectorAndVectorToPoint <= halfAngle;
}
/// <summary>
/// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases.
/// </summary>
/// <param name="worldTwistAxisA">Twist axis in world space to attach to entity A.</param>
/// <param name="worldTwistAxisB">Twist axis in world space to attach to entity B.</param>
public void SetupJointTransforms(System.Numerics.Vector3 worldTwistAxisA, System.Numerics.Vector3 worldTwistAxisB)
{
worldTwistAxisA.Normalize();
worldTwistAxisB.Normalize();
System.Numerics.Vector3 worldXAxis;
Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis);
float length = worldXAxis.LengthSquared();
if (length < Toolbox.Epsilon)
{
Vector3Ex.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis);
}
worldXAxis.Normalize();
//Complete A's basis.
System.Numerics.Vector3 worldYAxis;
Vector3Ex.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis);
basisA.rotationMatrix = connectionA.orientationMatrix;
basisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis);
//Rotate the axis to B since it could be arbitrarily rotated.
System.Numerics.Quaternion rotation;
QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation);
QuaternionEx.Transform(ref worldXAxis, ref rotation, out worldXAxis);
basisB.rotationMatrix = connectionB.orientationMatrix;
basisB.SetWorldAxes(worldTwistAxisB, worldXAxis);
}
/// <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 System.Numerics.Vector3 location, out System.Numerics.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;
System.Numerics.Quaternion localSteeringTransform;
QuaternionEx.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform);
var startingTransform = new RigidTransform
{
Position = wheel.suspension.worldAttachmentPoint,
Orientation = QuaternionEx.Concatenate(QuaternionEx.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : System.Numerics.Quaternion.Identity), wheel.vehicle.Body.orientation)
};
System.Numerics.Vector3 sweep;
Vector3Ex.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;
Vector3Ex.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.
Vector3Ex.Negate(ref normal, out normal);
}
normal.Normalize();
}
else
Vector3Ex.Negate(ref wheel.suspension.worldDirection, out normal);
return true;
}
return false;
}
/// <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 System.Numerics.Vector3 location, out System.Numerics.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;
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.RayCast(new Ray(wheel.suspension.worldAttachmentPoint, wheel.suspension.worldDirection), wheel.suspension.restLength, out rayHit) &&
rayHit.T < suspensionLength)
{
suspensionLength = rayHit.T;
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)
normal.Normalize();
else
Vector3Ex.Negate(ref wheel.suspension.worldDirection, out normal);
return true;
}
return false;
}
