private Vector3D BringToSafeZone(Vector3D destination, Vector3D armBasePoint, Vector3D armElbowPoint, Vector3D armTipPoint)
{
// calculating blind radius and max reach radius
var segment1Length = Vector3D.Distance(armBasePoint, armElbowPoint);
var segment2Length = Vector3D.Distance(armTipPoint, armElbowPoint);
var armBlindRadius = Math.Abs(segment1Length - segment2Length) * (InnerLimit + 1);
var armMaxReachRadius = Math.Abs(segment1Length + segment2Length) * OuterLimit;
// calculating just an average thing not to let segments angle too small
var averageSegmentLength = (segment1Length + segment2Length) / 2;
var averageSafeRadius = averageSegmentLength * InnerLimit;
// taking most strict limitation for inner radius
var safeInnerRadius = Math.Max(armBlindRadius, averageSafeRadius);
// taking limitation for outer radius
var destinationDistance = Vector3D.Distance(destination, armBasePoint);
var safeOuterRadius = Math.Min(armMaxReachRadius, destinationDistance);
// checking if it is OK
if (destinationDistance >= safeInnerRadius && destinationDistance <= safeOuterRadius)
{
return(destination);
}
// if not OK, then calculating closest point inside safe zone
var awayVector = VectorUtility.Normalize(destination - armBasePoint);
if (destinationDistance < safeInnerRadius)
{
return(armBasePoint + awayVector * safeInnerRadius);
}
return(armBasePoint + awayVector * safeOuterRadius);
}
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 Vector3D TakeNearbyPoint(Vector3D destination, Vector3D armTipPoint, double averageLength)
{
// calculating where the tip is heading
var generalDirection = VectorUtility.Normalize(destination - armTipPoint);
// calculating how far should we cut the trail to make destination more nearby
var generalDistance = Vector3D.Distance(armTipPoint, destination);
var preferredDistance = averageLength * NearbyPointFromLengthMultiplier;
var minDistance = Math.Min(generalDistance, preferredDistance);
return(armTipPoint + generalDirection * minDistance);
}
private Vector3D CutTrailAtClosestToBase(Vector3D rotationBasePoint, Vector3D armTipPoint, Vector3D destination)
{
// if the trail is so that the tip approaches the base at first, and then move away, we will aim to the point when it turns away
// so that arm will have to move in one direction, either towards the base or away from it
// and such we can calculate everything for tip to follow straight line (kind of)
var closestPoint = VectorUtility.FindClosestPointOnSegment(rotationBasePoint, armTipPoint, destination);
var isCloseToBasePoint = Vector3D.Distance(armTipPoint, closestPoint) <
Vector3D.Distance(destination, closestPoint) * 0.1d;
return(isCloseToBasePoint
? destination
: closestPoint);
}
private Vector3D GetRotationEndPoint(Vector3D destination, Vector3D rotationBasePoint, Vector3D armTipPoint)
{
// caution: local points here are different than in main method
var localDestination = destination - rotationBasePoint;
var localTip = armTipPoint - rotationBasePoint;
var rotorUpVector = RotorRotation.Rotor.WorldMatrix.Up;
var plane = new PlaneD(new Vector3D(0d, 0d, 0d), rotorUpVector);
// Project destination point onto horizontal rotation plane
var projectedLocalDestination = plane.ProjectPoint(ref localDestination);
var projectedLocalTip = plane.ProjectPoint(ref localTip);
var abovePlaneVector = localTip - projectedLocalTip;
return(VectorUtility.Normalize(projectedLocalDestination) * projectedLocalTip.Length() + rotationBasePoint + abovePlaneVector);
}
public Crawler(RoboticArm leftBack, RoboticArm leftFront, RoboticArm rightBack, RoboticArm rightFront, IMyRemoteControl rc)
{
LeftBack = leftBack;
LeftFront = leftFront;
RightBack = rightBack;
RightFront = rightFront;
Rc = rc;
StepIndent = new []
{
GetAverageArmSegmentLength(LeftBack),
GetAverageArmSegmentLength(LeftFront),
GetAverageArmSegmentLength(RightBack),
GetAverageArmSegmentLength(RightFront),
}.Average() *StepIndentMultiplier;
// getting local positions of leg base rotation rotors
var lb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftBack.RotorRotation.Rotor.GetPosition());
var lf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), LeftFront.RotorRotation.Rotor.GetPosition());
var rb = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightBack.RotorRotation.Rotor.GetPosition());
var rf = VectorUtility.GetLocalVector(Rc.WorldMatrix, Rc.GetPosition(), RightFront.RotorRotation.Rotor.GetPosition());
Center = VectorUtility.GetAverageVector(
VectorUtility.GetAverageVector(lb, lf),
VectorUtility.GetAverageVector(rb, rf));
lb -= Center;
lf -= Center;
rb -= Center;
rf -= Center;
LbMarchPoint = VectorUtility.Normalize(lb) * (StepIndent + lb.Length());
LfMarchPoint = VectorUtility.Normalize(lf) * (StepIndent + lf.Length());
RbMarchPoint = VectorUtility.Normalize(rb) * (StepIndent + rb.Length());
RfMarchPoint = VectorUtility.Normalize(rf) * (StepIndent + rf.Length());
Lbh = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 0.5, LbMarchPoint.Z);
Lfh = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 0.5, LfMarchPoint.Z);
Rbh = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 0.5, RbMarchPoint.Z);
Rfh = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 0.5, RfMarchPoint.Z);
Lbs = new Vector3D(LbMarchPoint.X, LbMarchPoint.Y - 1.5, LbMarchPoint.Z + 3);
Lfs = new Vector3D(LfMarchPoint.X, LfMarchPoint.Y - 1.5, LfMarchPoint.Z - 3);
Rbs = new Vector3D(RbMarchPoint.X, RbMarchPoint.Y - 1.5, RbMarchPoint.Z + 3);
Rfs = new Vector3D(RfMarchPoint.X, RfMarchPoint.Y - 1.5, RfMarchPoint.Z - 3);
}
private double GetRotationSpeedPart(Vector3D localDestination, Vector3D localArmTipPoint, Vector3D localRotationBase, double armDistance)
{
var localArmDirectionPoint = VectorUtility.Normalize(localRotationBase - localArmTipPoint) * armDistance + localArmTipPoint;
var pathCenter = new Vector3D(
(localArmTipPoint.X + localDestination.X) / 2,
(localArmTipPoint.Y + localDestination.Y) / 2,
(localArmTipPoint.Z + localDestination.Z) / 2);
// using completion to parallelogram
var centerToArmVector = localArmDirectionPoint - pathCenter;
var localRotationDirectionPoint = localArmDirectionPoint - 2 * centerToArmVector;
var armSpeed = Vector3D.Distance(localArmTipPoint, localArmDirectionPoint);
var rotationSpeed = Vector3D.Distance(localArmTipPoint, localRotationDirectionPoint);
return(rotationSpeed / (rotationSpeed + armSpeed));
}
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()
{
var lbd = State == 1 ? Lbs : Lbh;
var lfd = State == 1 ? Lfh : Lfs;
var rbd = State == 1 ? Rbh : Rbs;
var rfd = State == 1 ? Rfs : Rfh;
lbd = VectorUtility.GetWorldPoint(Rc.WorldMatrix, Rc.GetPosition(), lbd + Center);
lfd = VectorUtility.GetWorldPoint(Rc.WorldMatrix, Rc.GetPosition(), lfd + Center);
rbd = VectorUtility.GetWorldPoint(Rc.WorldMatrix, Rc.GetPosition(), rbd + Center);
rfd = VectorUtility.GetWorldPoint(Rc.WorldMatrix, Rc.GetPosition(), rfd + Center);
if (NearDest(LeftBack, lbd) && NearDest(LeftFront, lfd) && NearDest(RightBack, rbd) && NearDest(RightFront, rfd))
{
FlipState();
}
LeftBack.KeepMoving(lbd, 2);
LeftFront.KeepMoving(lfd, 2);
RightBack.KeepMoving(rbd, 2);
RightFront.KeepMoving(rfd, 2);
var pts = new[]
{
lbd, lfd, rbd, rfd, LeftBack.Tip.GetPosition(), LeftFront.Tip.GetPosition(),
RightBack.Tip.GetPosition(), RightFront.Tip.GetPosition()
};
var str = "";
int i = 1;
foreach (var p in pts)
{
str += VectorUtility.ToLabel(p, i.ToString()) + "\n";
i++;
}
Rc.CustomData = str;
}
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)));
}
