private float CalculateBaseArmAngle(Vector2D elbowPoint)
{
var angle = (float)VectorUtility.Angle(Vector2D.UnitY, elbowPoint);
if (elbowPoint.X < 0)
{
angle = 2 * (float)Math.PI - angle;
}
return(angle);
}
private void MakeRotationMovement(Vector3D localProjectedTip, Vector3D localProjectedDestination, double targetRps)
{
// calculating angle diff
var diff = VectorUtility.Angle(localProjectedDestination, localProjectedTip);
// calculating the direction to which to rotate.
var movementCrossVector = Vector3D.Cross(localProjectedTip, localProjectedDestination);
var isMovingClockwise = Vector3D.Dot(movementCrossVector, new Vector3D(0d, -1d, 0d)) > 0;
if (!isMovingClockwise) // in SE rotors have their angle increased in clockwise movement when their head pointed upwards (for some reason)
{
diff = -diff;
}
// applying new angle to the rotor
var destinationAngle = AngleUtility.Positive(RotorRotation.Angle + diff);
RotorRotation.MoveTowardsAngle(destinationAngle, targetRps);
}
public void KeepMoving(Vector3D destination, double velocity /* Meters per sec*/)
{
var matrix = RotorRotation.Rotor.WorldMatrix;
// getting world points
var rotationBasePoint = RotorRotation.Rotor.GetPosition();
var armBasePoint = Rotor1.Rotor.GetPosition();
var armElbowPoint = Rotor2.Rotor.GetPosition();
var armTipPoint = Tip.GetPosition();
// adjust destination in case its something wrong with it
var averageLength = (Vector3D.Distance(armBasePoint, armElbowPoint) + Vector3D.Distance(armElbowPoint, armTipPoint)) / 2;
destination = CutTrailAtClosestToBase(rotationBasePoint, armTipPoint, destination);
destination = BringToSafeZone(destination, armBasePoint, armElbowPoint, armTipPoint);
destination = TakeNearbyPoint(destination, armTipPoint, averageLength);
// no point doing anything if the tip is already there
if (Vector3D.Distance(armTipPoint, destination) < DistanceToCancelMovement)
{
RotorRotation.Stop();
Rotor1.Stop();
Rotor2.Stop();
return;
}
var rotationEndPoint = GetRotationEndPoint(destination, rotationBasePoint, armTipPoint);
// calculate local points
var localArmBasePoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armBasePoint);
var localArmTipPoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armTipPoint);
var localDestination = VectorUtility.GetLocalVector(matrix, rotationBasePoint, destination);
var localRotationEndPoint = VectorUtility.GetLocalVector(matrix, rotationBasePoint, rotationEndPoint);
var localArmElbow = VectorUtility.GetLocalVector(matrix, rotationBasePoint, armElbowPoint);
var localArmPlaneNormalPoint = /*localArmBasePoint + */ Vector3D.Rotate(Rotor1.Rotor.WorldMatrix.Up, MatrixD.Transpose(matrix));
// calculating planes to project on
var rotationPlane = new PlaneD(Vector3D.Zero, Vector3D.UnitY); // note that rotation plane is XZ
var armPlane = new PlaneD(localArmBasePoint, localArmPlaneNormalPoint);
// normalizing arm points by projecting them to the arm plane for the arm part calculations
var normalizedArmTip = armPlane.ProjectPoint(ref localArmTipPoint);
var normalizedArmElbow = armPlane.ProjectPoint(ref localArmElbow);
var normalizedDestination = armPlane.ProjectPoint(ref localDestination);
// calculate local projected points
var localProjectedArmBase = rotationPlane.ProjectPoint(ref localArmBasePoint);
var localProjectedTip = rotationPlane.ProjectPoint(ref normalizedArmTip);
var localProjectedTipForRotation = rotationPlane.ProjectPoint(ref localArmTipPoint);
var localProjectedDestination = rotationPlane.ProjectPoint(ref localDestination); // for rotation
// calculate rotation distance
var rotationDistance = Math.Abs(VectorUtility.Angle(localProjectedTipForRotation, localProjectedDestination)) * localProjectedTip.Length();
// adjust speeds
//var rotationSpeedPart = GetRotationSpeedPart(localDestination, localArmTipPoint, localRotationBase, armDistance);
var armDistance = Vector3D.Distance(localDestination, localRotationEndPoint);
var rotationSpeedPart = rotationDistance / (rotationDistance + armDistance);
var armSpeedPart = 1d - rotationSpeedPart;
var rotationVelocity = rotationSpeedPart * velocity;
var armVelocity = armSpeedPart * velocity;
// calculate base rotor rotation speed
var targetRps = rotationVelocity / localProjectedTip.Length();
// calculating tip and destination as 2D points for the arm by rotating them parallel to YZ plane
var armBaseElevation = localArmBasePoint.Y;
var angle2D = (float)VectorUtility.Angle(localProjectedTip - localProjectedArmBase, Vector3D.UnitX);
if (localProjectedTip.Z < 0)
{
angle2D *= -1;
}
var armTipR = VectorUtility.Rotate(normalizedArmTip, Vector3D.UnitY, angle2D);
var armElbowR = VectorUtility.Rotate(normalizedArmElbow, Vector3D.UnitY, angle2D);
var armDestinationR = VectorUtility.Rotate(normalizedDestination, Vector3D.UnitY, angle2D);
var armTipPoint2D = new Vector2D(armTipR.X, armTipR.Y - armBaseElevation);
var armElbow2D = new Vector2D(armElbowR.X, armElbowR.Y - armBaseElevation);
var armDestinationPoint2D = new Vector2D(armDestinationR.X, armDestinationR.Y - armBaseElevation);
// launch movement
MakeRotationMovement(localProjectedTipForRotation, localProjectedDestination, targetRps);
MakeArmMovement(armTipPoint2D, armElbow2D, armDestinationPoint2D, armVelocity);
}
private float CalculateElbowArmAngle(Vector2D elbowPoint, Vector2D tipPoint)
{
return(VectorUtility.Cross(tipPoint - elbowPoint, elbowPoint) < 0
? (float)(Math.PI - VectorUtility.Angle(-elbowPoint, tipPoint - elbowPoint))
: (float)(Math.PI + VectorUtility.Angle(-elbowPoint, tipPoint - elbowPoint)));
}