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