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