cordic.c

Go to the documentation of this file.

// Run a standalone test
// #define TEST

#define CORDIC_TABLE  /* include the generated Cordic table */

#ifdef TEST
#define MEMSPACE   /* */
#include <stdio.h>
#include <stdlib.h>
#else
#include "user_config.h"
#include <stdint.h>
#include <string.h>
#include "printf/mathio.h"
#endif

#include <math.h>

#include "cordic2c_inc.h"
#include "cordic.h"

MEMSPACE
double deg2rad(double deg)
{
    return (deg * M_PI / 180.0);
}

MEMSPACE
double angle_quad(double quads, int *quad)
{

    int s,q;
    double intpart;

    s = 0;
    if(quads < 0.0)
    {
        quads = -quads;
        s = 4;                                    // sign information flag
    }

    quads = modf(quads, &intpart);
    q = intpart;
    q &= 3;
    q |= s;
    *quad = q;
    return(quads);

}


Cordic_T X,Y,Z;
void Circular (Cordic_T x, Cordic_T y, Cordic_T z)
{
    int i;

    X = x;
    Y = y;
    Z = z;

    for (i = 0; i < Cordic_T_Bits; ++i)
    {
        x = X >> i;
        y = Y >> i;

        if(i < 14)
            z = v_atangrad[i];
        else
            z >>= 1;

        if(Z >= 0)
        {
            X -= y;
            Y += x;
            Z -= z;
        }
        else
        {
            X += y;
            Y -= x;
            Z += z;
        }
    }
}


MEMSPACE
void cordic_quad(double angle, double *s, double *c)
{
#ifdef TEST
    double ts,tc;
    double es,ec;
    double degree, rad, orig;
#endif

    double cs,cc;
    double quads, tmp;
    int quad;
    Cordic_T a;

    quads = angle_quad(angle,&quad);

    a = FP2Cordic(quads);                         /* convert to CORDIC fixed point */
    Circular(Cordic_K,0,a);
    cc = Cordic2FP(X);
    cs = Cordic2FP(Y);

// Angle 90 to < 180 degrees swap sin and cos, and negate cos
    if(quad & 1)
    {
        tmp = cc;
        cc = cs;
        cs = tmp;
        cc = -cc;
    }

// Angle 180 >.. 270 degrees negate both cos and sin
    if(quad & 2 )
    {
        cs = -cs;
        cc = -cc;
    }
// Angle < 0 degrees negate only sin
    if(quad & 4 )
    {
        cs = -cs;
    }

    *c = cc;
    *s = cs;

// Test difference between CORDIC and LIBC cos() and sin()
#ifdef TEST
    degree = angle * 90;
    rad = angle * M_PI / 2.0;
    tc = cos(rad);
    ts = sin(rad);

    ec = cc - tc;
    es = cs - ts;
    printf("Degree:%lf, rad:%lf, quad:%d\n",
        (double)angle, (double) rad, (int)quad);
    printf("ccos:%lf, cos:%lf, error:%lf\n",
        cc, tc, ec);
    printf("csin:%lf, sin:%lf, error:%lf\n",
        cs, ts, es);
#endif

}


MEMSPACE
void cordic_deg(double deg, double *s, double *c)
{
    cordic_quad(deg/90.0, s, c);
}


MEMSPACE
void cordic_rad(double rad, double *s, double *c)
{
    cordic_quad(rad * 2.0 /M_PI, s, c);
}


MEMSPACE
void scale_point(point *P, double scale)
{
    P->x *= scale;
    P->y *= scale;
    P->z *= scale;
}


MEMSPACE
void shift_point(point *P, point *shift)
{
    P->x += shift->x;
    P->y += shift->y;
    P->z += shift->x;
}


MEMSPACE
void rotate(point *P, point *V)
{
    double sinx, siny, sinz, cosx, cosy, cosz;
    double x,y,z,x1,y1,z1;

// Point
    x = P->x;
    y = P->y;
    z = P->z;

// Transform Point

// View is in degrees
    cordic_deg(V->x, &sinx, &cosx);
    cordic_deg(V->y, &siny, &cosy);
    cordic_deg(V->z, &sinz, &cosz);

    x1 = x*cosz + y*sinz;                         // Rotation around axis Z
    y1 = -x*sinz + y*cosz;
    z1 = z;

    x = x1;                                       // Rotation around axis X
    y = y1*cosx + z1*sinx;
    z = -y1*sinx + z1*cosx;

    x1 = x*cosy - z*siny;                         // Rotation around axis Y
    y1 = y;
    z1 = x*siny + z*cosy;

// Transformed point
    P->x = x1;
    P->y = y1;
    P->z = z1;
}


/*
 @brief // Perspective Projection with Offset and Scale
 @param[in] *P: x,y,z point, out: x,y ( z = 0 )
 @param [in] scale: scale factor
 @param [out] x: X offset
 @param [out] y: Y offset
 @return void
*/
MEMSPACE 
void PerspectiveProjection(point *P, double scale, int x, int y)
{
    P->x = (P->x + P->z / 2) * scale + x;
    P->y = (P->y - P->z / 2) * scale + y;
    P->z = 0;
}

#ifdef TEST
int main()
{
    double d;
    double s,c;
    int i;

    i = 0;

// Make sure CORIC converges correctly over a few rotations
    for(d=0;d<=360;d+=45)
    {
        cordic_deg(d-1,&s,&c);
        cordic_deg(d,&s,&c);
        cordic_deg(d+1,&s,&c);
    }
    return(0);
}
#endif