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