#include <math.h>
#include "../h/rccl.h"

#include "manex.560.h"

extern float square();
extern computeForce();
extern complyFxn();

computeCenter (x1,y1,x2,y2,x3,y3,Xc,Yc)
float x1;
float y1;
float x2;
float y2;
float x3;
float y3;
float *Xc;
float *Yc;
{
#define xc (*Xc)
#define yc (*Yc)

        float st;
        float ct;
        float sp;
        float cp;
        float r;

        r = sqrt(square(x2-x1)+square(y2-y1));
        if (r < .0001)
         { xc = 0;
           yc = 0;
           printf("pt1 == pt2\n");
           return;
         }
        st = (x1 - x2) / r;
        ct = (y2 - y1) / r;
        r = sqrt(square(x3-x2)+square(y3-y2));
        if (r < .0001)
         { xc = 0;
           yc = 0;
           printf("pt2 == pt3\n");
           return;
         }
        sp = (x2 - x3) / r;
        cp = (y3 - y2) / r;
        r = (cp * st) - (ct * sp);
        if (FABS(r) < .000001)
         { xc = 0;
           yc = 0;
           printf("collinear\n");
           return;
         }
        x1 = (x1 + x2) / 2;
        y1 = (y1 + y2) / 2;
        x2 = (x2 + x3) / 2;
        y2 = (y2 + y3) / 2;
        xc = ( (y2 * cp * ct)
             - (y1 * cp * ct)
             + (x1 * st * cp)
             - (x2 * sp * ct) ) / r;
        yc = ( (y2 * cp * st)
             - (x2 * sp * st)
             - (y1 * ct * sp)
             + (x1 * st * sp) ) / r;
        return;
#undef xc
#undef yc
}

COMPLY_CTRL_BLK *complyInit (mnp, tgain, rgain)
MANIP *mnp;
float tgain, rgain;
{
        COMPLY_CTRL_BLK *cbk;
        TRSF *comply;

        comply = allocTrans ("COMPLY", UNDEF);
        cbk = ALLOC_MEM (NULL, COMPLY_CTRL_BLK, UNDEF);
        cbk->tgain = tgain;
        cbk->rgain = rgain;
        cbk->comply = comply;
        cbk->mnp = mnp;

        transMotionEval (comply, complyFxn, (int)cbk, mnp);
        return (cbk);
}

complyRun (cbk)
COMPLY_CTRL_BLK *cbk;
{
        runMonitorFxn (cbk->mnp, computeForce, (int)cbk);
        rcclBlock();
}
