/* rotate.m,
 *
 * Some useful rotation routines.
 */

#include <math.h>
#include "rotate.h"


#define ABS(x)		((x) * (x))


/* PW:
 * This function comes from a project from two years ago.  I had to
 * look up this up on the net back then.  Two years and a lot more
 * maths exposure later, and I still don't know how it works.
 *
 * Rotate point x,y by angle theta and return via rx,ry.
 */
inline void rotate(double x, double y, double theta, double *rx, double *ry)
{
    *rx = x*cos(theta) - y*sin(theta);
    *ry = x*sin(theta) + y*cos(theta);
}


/* Rotate towards theta_desired, but at most theta_max (radians),
 * taking into account that +pi == -pi.
 */
void limited_rotate(double *theta, double theta_desired, double theta_max)
{
    /* We are within one step of the desired angle. */
    if (ABS(*theta - theta_desired) < theta_max)
        *theta = theta_desired;

    /* Make sure things don't rotate stupidly when passing +-PI. */
    else if (*theta > theta_desired + M_PI) *theta += theta_max;
    else if (*theta < theta_desired - M_PI) *theta -= theta_max;

    /* Normal rotation. */
    else if (*theta < theta_desired) *theta += theta_max;
    else if (*theta > theta_desired) *theta -= theta_max;
}
