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