극 좌표계(Polar Coordinate Systems)

// Radial distance float r; // Angle in RADIANS float theta; // Declare a constant for 2*pi (360 degrees) const float TWOPI = 2.0f*PI; // Check if we are exactly at the origin if (r == 0.0f) { // At the origin - slam theta to zero theta = 0.0f; } else { // Handle negative distance if (r < 0.0f) { r = -r; theta += PI; } // Theta out of range? Note that this if() check is not // strictly necessary, but we try to avoid doing floating // point operations if they aren't necessary. Why // incur floating point precision loss if we don't // need to? if (fabs(theta) > PI) { // Offset by PI theta += PI; // Wrap in range 0...TWOPI theta -= floor(theta / TWOPI) * TWOPI; // Undo offset, shifting angle back in range -PI...PI theta -= PI; } }

// Input: Cartesian coordinates float x,y; // Output: polar radial distance, and angle in RADIANS float r, theta; // Check if we are at the origin if (x == 0.0f && y == 0.0f) { // At the origin - slam both polar coordinates to zero r = 0.0f; theta = 0.0f; } else { // Compute values. Isn't the atan2 function great? r = sqrt(x*x + y*y); theta = atan2(y,x); }

// Radial distance float r; // Angles in radians float heading, pitch; // Declare a few constants const float TWOPI = 2.0f*PI; // 360 degrees const float PIOVERTWO = PI/2.0f; // 90 degrees // Check if we are exactly at the origin if (r == 0.0f) { // At the origin - slam angles to zero heading = pitch = 0.0f; } else { // Handle negative distance if (r < 0.0f) { r = -r; heading += PI; pitch = -pitch; } // Pitch out of range? if (fabs(pitch) > PIOVERTWO) { // Offset by 90 degrees pitch += PIOVERTWO; // Wrap in range 0...TWOPI pitch -= floor(pitch / TWOPI) * TWOPI; // Out of range? if (pitch > PI) { // Flip heading heading += PI; // Undo offset and also set pitch = 180-pitch pitch = 3.0f*PI/2.0f - pitch; // p = 270 degrees - p } else { // Undo offset, shifting pitch in range // -90 degrees ... +90 degrees pitch -= PIOVERTWO; } } // Gimbal lock? Test using a relatively small tolerance // here, close to the limits of single precision. if (fabs(pitch) >= PIOVERTWO*0.9999) { heading = 0.0f; } else { // Wrap heading, avoiding math when possible // to preserve precision if (fabs(heading) > PI) { // Offset by PI heading += PI; // Wrap in range 0...TWOPI heading -= floor(heading / TWOPI) * TWOPI; // Undo offset, shifting angle back in range -PI...PI heading -= PI; } } }

// Input Cartesian coordinates float x,y,z; // Output radial distance float r; // Output angles in radians float heading, pitch; // Declare a few constants const float TWOPI = 2.0f*PI; // 360 degrees const float PIOVERTWO = PI/2.0f; // 90 degrees // Compute radial distance r = sqrt(x*x + y*y + z*z); // Check if we are exactly at the origin if (r &gt; 0.0f) { // Compute pitch pitch = asin(-y/r); // Check for gimbal lock, since the library atan2 // function is undefined at the (2D) origin if (fabs(pitch) &gt;= PIOVERTWO*0.9999) { heading = 0.0f; } else { heading = atan2(x,z); } } else { // At the origin - slam angles to zero heading = pitch = 0.0f; }