euspqp.c

Go to the documentation of this file.

#pragma init (register_euspqp)

#include "eus.h"

extern pointer ___euspqp();
static register_euspqp()
{ add_module_initializer("___euspqp", ___euspqp);}

extern eusinteger_t PQP_MakeModel();
extern void PQP_DeleteModel(eusinteger_t m);
extern int PQP_BeginModel(eusinteger_t m);
extern int PQP_EndModel(eusinteger_t m);
extern int PQP_AddTri(eusinteger_t m, double p1[], double p2[], double p3[],
                      int flag);
extern int PQP_Collide(double R1[3][3], double T1[3], eusinteger_t PQP_Model1,
                       double R2[3][3], double T2[3], eusinteger_t PQP_Model2,
                       int flag);
extern double PQP_Distance(double R1[3][3], double T1[3], eusinteger_t* PQP_Model1,
                           double R2[3][3], double T2[3], eusinteger_t* PQP_Model2,
                           double *P1, double *P2, int qsize);

pointer PQPMAKEMODEL(register context *ctx, int n, register pointer *argv)
{
    eusinteger_t addr = PQP_MakeModel();
    return makeint(addr);
}

pointer PQPDELETEMODEL(register context *ctx, int n, register pointer *argv)
{
    eusinteger_t m = intval(argv[0]);
    PQP_DeleteModel(m);
    return(NIL);
}

pointer PQPBEGINMODEL(register context *ctx, int n, register pointer *argv)
{
    eusinteger_t m = intval(argv[0]);
    PQP_BeginModel(m);
    return(NIL);
}

pointer PQPENDMODEL(register context *ctx, int n, register pointer *argv)
{
    eusinteger_t m = intval(argv[0]);
    PQP_EndModel(m);
    return(NIL);
}

pointer PQPADDTRI(register context *ctx, int n, register pointer *argv)
{
    eusinteger_t m = intval(argv[0]);
    eusfloat_t *fv1=argv[1]->c.fvec.fv;
    eusfloat_t *fv2=argv[2]->c.fvec.fv;
    eusfloat_t *fv3=argv[3]->c.fvec.fv;
    int id = (int)argv[4]>>2;
    double dv1[3], dv2[3], dv3[3];
    dv1[0] = fv1[0]; dv1[1] = fv1[1]; dv1[2] = fv1[2]; 
    dv2[0] = fv2[0]; dv2[1] = fv2[1]; dv2[2] = fv2[2]; 
    dv3[0] = fv3[0]; dv3[1] = fv3[1]; dv3[2] = fv3[2];
    #if 0
    printf("#f(%f %f %f), #f(%f %f %f), #f(%f %f %f), %d\n",
           dv1[0], dv1[1], dv1[2], 
           dv2[0], dv2[1], dv2[2], 
           dv3[0], dv3[1], dv3[2], id);
    #endif
    PQP_AddTri(m, dv1, dv2, dv3, id);
    return(NIL);
}

pointer PQPCOLLIDE(register context *ctx, int n, register pointer *argv)
{
    eusfloat_t *fr1=argv[0]->c.ary.entity->c.fvec.fv;
    eusfloat_t *ft1=argv[1]->c.fvec.fv;
    eusinteger_t m1=intval(argv[2]);
    eusfloat_t *fr2=argv[3]->c.ary.entity->c.fvec.fv;
    eusfloat_t *ft2=argv[4]->c.fvec.fv;
    eusinteger_t m2=intval(argv[5]);
    int flag=(int)argv[6]>>2;
    double dr1[3][3], dr2[3][3], dt1[3], dt2[3];

    /*printf("addr1=0x%x, addr2=0x%x, flag=%d\n", m1>>2, m2>>2, flag);*/
    
    dr1[0][0] = fr1[0]; dr1[0][1] = fr1[1]; dr1[0][2] = fr1[2]; 
    dr1[1][0] = fr1[3]; dr1[1][1] = fr1[4]; dr1[1][2] = fr1[5]; 
    dr1[2][0] = fr1[6]; dr1[2][1] = fr1[7]; dr1[2][2] = fr1[8];
#if 0
    printf("%f %f %f\n %f %f %f\n %f %f %f\n\n",
           dr1[0][0], dr1[0][1], dr1[0][2], 
           dr1[1][0], dr1[1][1], dr1[1][2], 
           dr1[2][0], dr1[2][1], dr1[2][2]);
#endif    
    dt1[0] = ft1[0]; dt1[1] = ft1[1]; dt1[2] = ft1[2];

    /*printf("%f %f %f\n", dt1[0], dt1[1], dt1[2]);*/
    
    dr2[0][0] = fr2[0]; dr2[0][1] = fr2[1]; dr2[0][2] = fr2[2]; 
    dr2[1][0] = fr2[3]; dr2[1][1] = fr2[4]; dr2[1][2] = fr2[5]; 
    dr2[2][0] = fr2[6]; dr2[2][1] = fr2[7]; dr2[2][2] = fr2[8]; 

    dt2[0] = ft2[0]; dt2[1] = ft2[1]; dt2[2] = ft2[2]; 

    return makeint((eusfloat_t)(PQP_Collide(dr1, dt1, m1, dr2, dt2, m2, flag)));
}

pointer PQPDISTANCE(register context *ctx, int n, register pointer *argv)
{
    eusfloat_t *fr1=argv[0]->c.ary.entity->c.fvec.fv;
    eusfloat_t *ft1=argv[1]->c.fvec.fv;
    eusinteger_t m1=intval(argv[2]);
    eusfloat_t *fr2=argv[3]->c.ary.entity->c.fvec.fv;
    eusfloat_t *ft2=argv[4]->c.fvec.fv;
    eusinteger_t m2=intval(argv[5]);
    eusfloat_t *p1=argv[6]->c.fvec.fv;
    eusfloat_t *p2=argv[7]->c.fvec.fv;
    double dr1[3][3], dr2[3][3], dt1[3], dt2[3];
    int qsize = 2;
    numunion nu;

    if (n == 9) qsize = intval(argv[8]);

    /*printf("addr1=0x%x, addr2=0x%x, flag=%d\n", m1>>2, m2>>2, flag);*/
    
    dr1[0][0] = fr1[0]; dr1[0][1] = fr1[1]; dr1[0][2] = fr1[2]; 
    dr1[1][0] = fr1[3]; dr1[1][1] = fr1[4]; dr1[1][2] = fr1[5]; 
    dr1[2][0] = fr1[6]; dr1[2][1] = fr1[7]; dr1[2][2] = fr1[8];
#if 0
    printf("%f %f %f\n %f %f %f\n %f %f %f\n\n",
           dr1[0][0], dr1[0][1], dr1[0][2], 
           dr1[1][0], dr1[1][1], dr1[1][2], 
           dr1[2][0], dr1[2][1], dr1[2][2]);
#endif    
    dt1[0] = ft1[0]; dt1[1] = ft1[1]; dt1[2] = ft1[2];

    /*printf("%f %f %f\n", dt1[0], dt1[1], dt1[2]);*/
    
    dr2[0][0] = fr2[0]; dr2[0][1] = fr2[1]; dr2[0][2] = fr2[2]; 
    dr2[1][0] = fr2[3]; dr2[1][1] = fr2[4]; dr2[1][2] = fr2[5]; 
    dr2[2][0] = fr2[6]; dr2[2][1] = fr2[7]; dr2[2][2] = fr2[8]; 

    dt2[0] = ft2[0]; dt2[1] = ft2[1]; dt2[2] = ft2[2]; 

    double distance, P1[3], P2[3];
    distance = PQP_Distance(dr1, dt1, (void *)(m1), dr2, dt2, (void *)(m2), P1, P2, qsize);
    p1[0]=P1[0]; p1[1]=P1[1]; p1[2]=P1[2];
    p2[0]=P2[0]; p2[1]=P2[1]; p2[2]=P2[2];
    return (makeflt(distance));
}

pointer ___euspqp(register context *ctx, int n, register pointer *argv)
{
    pointer mod=argv[0];

    defun(ctx, "PQPMAKEMODEL", mod, PQPMAKEMODEL);
    defun(ctx, "PQPDELETEMODEL", mod, PQPDELETEMODEL);
    defun(ctx, "PQPBEGINMODEL", mod, PQPBEGINMODEL);
    defun(ctx, "PQPENDMODEL", mod, PQPENDMODEL);
    defun(ctx, "PQPADDTRI", mod, PQPADDTRI);
    defun(ctx, "PQPCOLLIDE", mod, PQPCOLLIDE);
    defun(ctx, "PQPDISTANCE", mod, PQPDISTANCE);
}