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