public bool AlignBlockTo(Vector3D vPos, IMyTerminalBlock block = null)
{
if (block == null)
{
block = Remote;
}
Matrix m = block.WorldMatrix;
Vector3D forward = m.Forward;
Vector3D left = m.Left;
Vector3D up = m.Up;
Vector3D vTarget = (vPos - block.GetPosition());
vTarget.Normalize();
// Check angle.
var angleF = VectorHelper.AngleBetween(vTarget, m.Forward);
this.Program.Echo(string.Format("Angle: {0}", angleF));
if (angleF <= 0.01)
{
DisableGyroOverride();
return(true);
}
double yaw, pitch;
GetRotationAngles(vTarget, forward, left, up, out yaw, out pitch);
// Correct with PID.
var freq = this.Program.Runtime.TimeSinceLastRun.TotalMilliseconds / 1000;
pitch = PitchPID.CorrectError(pitch, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
yaw = YawPID.CorrectError(yaw, freq) * 0.1; // * speedFactor; apply factor to reduce the speed if needed.
// Apply gyro overrides.
ApplyGyroOverride(pitch, yaw, 0, Gyros, m);
// Return not aligned for now. Will keep aligning :)
return(false);
}
private void GetRotationAngles(Vector3D v_target, Vector3D v_front, Vector3D v_left, Vector3D v_up, out double yaw, out double pitch)
{
//Dependencies: ProjectVector() | GetAngleBetween()
var projectTargetUp = VectorHelper.VectorProject(v_target, v_up);
var projTargetFrontLeft = v_target - projectTargetUp;
yaw = VectorHelper.AngleBetween(v_front, projTargetFrontLeft);
pitch = VectorHelper.AngleBetween(v_target, projTargetFrontLeft);
//---Check if yaw angle is left or right
//multiplied by -1 to convert from right hand rule to left hand rule
yaw = -1 * Math.Sign(v_left.Dot(v_target)) * yaw;
//---Check if pitch angle is up or down
pitch = Math.Sign(v_up.Dot(v_target)) * pitch;
//---Check if target vector is pointing opposite the front vector
if (Math.Abs(pitch) < eps && Math.Abs(yaw) < eps && v_target.Dot(v_front) < 0)
{
yaw = Math.PI;
}
}
