cmsmtrx.c

Linux下的无线网卡通用驱动程序

double VEC3distance(LPVEC3 a, LPVEC3 b)
{
    double d1 = a ->n[VX] - b ->n[VX];
    double d2 = a ->n[VY] - b ->n[VY];
    double d3 = a ->n[VZ] - b ->n[VZ];

    return sqrt(d1*d1 + d2*d2 + d3*d3);
}


// Identity


void MAT3identity(LPMAT3 a)
{
        VEC3init(&a-> v[0], 1.0, 0.0, 0.0);
        VEC3init(&a-> v[1], 0.0, 1.0, 0.0);
        VEC3init(&a-> v[2], 0.0, 0.0, 1.0);
}




// Check if matrix is Identity. Allow a tolerance as %

BOOL MAT3isIdentity(LPWMAT3 a, double Tolerance)
{
       int i;
       MAT3 Idd;
       WMAT3 Idf;

       MAT3identity(&Idd);
       MAT3toFix(&Idf, &Idd);

       for (i=0; i < 3; i++)
              if (!VEC3equal(&a -> v[i], &Idf.v[i], Tolerance)) return FALSE;

       return TRUE;

}

// Multiply two matrices


void MAT3per(LPMAT3 r, LPMAT3 a, LPMAT3 b)
{
#define ROWCOL(i, j) \
    a->v[i].n[0]*b->v[0].n[j] + a->v[i].n[1]*b->v[1].n[j] + a->v[i].n[2]*b->v[2].n[j]

    VEC3init(&r-> v[0], ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2));
    VEC3init(&r-> v[1], ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2));
    VEC3init(&r-> v[2], ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2));

#undef ROWCOL //(i, j)
}



// Inverse of a matrix b = a^(-1)
// Gauss-Jordan elimination with partial pivoting

int MAT3inverse(LPMAT3 a, LPMAT3 b)
{
    register int  i, j, max;

    MAT3identity(b);

    // Loop over cols of a from left to right, eliminating above and below diag
    for (j=0; j<3; j++) {   // Find largest pivot in column j among rows j..2

    max = j;                 // Row with largest pivot candidate
    for (i=j+1; i<3; i++)
        if (fabs(a -> v[i].n[j]) > fabs(a -> v[max].n[j]))
            max = i;

    // Swap rows max and j in a and b to put pivot on diagonal

    VEC3swap(&a -> v[max], &a -> v[j]);
    VEC3swap(&b -> v[max], &b -> v[j]);

    // Scale row j to have a unit diagonal

    if (a -> v[j].n[j]==0.)
        return -1;                 // singular matrix; can't invert

    VEC3divK(&b-> v[j], &b -> v[j], a->v[j].n[j]);
    VEC3divK(&a-> v[j], &a -> v[j], a->v[j].n[j]);

    // Eliminate off-diagonal elems in col j of a, doing identical ops to b
    for (i=0; i<3; i++)

        if (i !=j) {
                  VEC3 temp;

          VEC3perK(&temp, &b -> v[j], a -> v[i].n[j]);
          VEC3minus(&b -> v[i], &b -> v[i], &temp);

          VEC3perK(&temp, &a -> v[j], a -> v[i].n[j]);
          VEC3minus(&a -> v[i], &a -> v[i], &temp);
    }
    }

    return 1;
}


// Solve a system in the form Ax = b

BOOL MAT3solve(LPVEC3 x, LPMAT3 a, LPVEC3 b)
{
	MAT3 m, a_1;

	CopyMemory(&m, a, sizeof(MAT3));

	if (!MAT3inverse(&m, &a_1)) return FALSE;  // Singular matrix

	MAT3eval(x, &a_1, b);
	return TRUE;
}


// The determinant

double MAT3det(LPMAT3 m)
{

    double a1 = m ->v[VX].n[VX];
    double a2 = m ->v[VX].n[VY];
    double a3 = m ->v[VX].n[VZ];
    double b1 = m ->v[VY].n[VX];
    double b2 = m ->v[VY].n[VY];
    double b3 = m ->v[VY].n[VZ];
    double c1 = m ->v[VZ].n[VX];
    double c2 = m ->v[VZ].n[VY];
    double c3 = m ->v[VZ].n[VZ];

    
    return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 - a3*b1*c2 - a3*b2*c1;
}


// linear transform


void MAT3eval(LPVEC3 r, LPMAT3 a, LPVEC3 v)
{
    r->n[VX] = a->v[0].n[VX]*v->n[VX] + a->v[0].n[VY]*v->n[VY] + a->v[0].n[VZ]*v->n[VZ];
    r->n[VY] = a->v[1].n[VX]*v->n[VX] + a->v[1].n[VY]*v->n[VY] + a->v[1].n[VZ]*v->n[VZ];
    r->n[VZ] = a->v[2].n[VX]*v->n[VX] + a->v[2].n[VY]*v->n[VY] + a->v[2].n[VZ]*v->n[VZ];
}


// Ok, this is another bottleneck of performance.


#ifdef USE_ASSEMBLER

// ecx:ebx is result in 64 bits format
// edi points to matrix, esi points to input vector
// since only 3 accesses are in output, this is a stack variable


void MAT3evalW(LPWVEC3 r_, LPWMAT3 a_, LPWVEC3 v_)
{

       ASM {


       mov    esi, dword ptr ss:v_
       mov    edi, dword ptr ss:a_

   //     r->n[VX] = FixedMul(a->v[0].n[0], v->n[0]) +

       mov       eax,dword ptr [esi]
       mov       edx,dword ptr [edi]
       imul      edx
       mov       ecx, eax
       mov       ebx, edx

   //          FixedMul(a->v[0].n[1], v->n[1]) +

       mov       eax,dword ptr [esi+4]
       mov       edx,dword ptr [edi+4]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

   //         FixedMul(a->v[0].n[2], v->n[2]);

       mov       eax,dword ptr [esi+8]
       mov       edx,dword ptr [edi+8]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

   //  Back to Fixed 15.16

       add       ecx, 0x8000
       adc       ebx, 0
       shrd      ecx, ebx, 16

       push      edi
       mov       edi, dword ptr ss:r_
       mov       dword ptr [edi], ecx      //  r -> n[VX]
       pop       edi



   //   2nd row ***************************

   //        FixedMul(a->v[1].n[0], v->n[0])

       mov       eax,dword ptr [esi]
       mov       edx,dword ptr [edi+12]
       imul      edx
       mov       ecx, eax
       mov       ebx, edx

   //         FixedMul(a->v[1].n[1], v->n[1]) +

       mov       eax,dword ptr [esi+4]
       mov       edx,dword ptr [edi+16]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

       //     FixedMul(a->v[1].n[2], v->n[2]);

       mov       eax,dword ptr [esi+8]
       mov       edx,dword ptr [edi+20]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

       add       ecx, 0x8000
       adc       ebx, 0
       shrd      ecx, ebx, 16

       push      edi
       mov       edi, dword ptr ss:r_
       mov       dword ptr [edi+4], ecx      // r -> n[VY]
       pop       edi

//     3d row **************************

   //       r->n[VZ] = FixedMul(a->v[2].n[0], v->n[0]) +

       mov       eax,dword ptr [esi]
       mov       edx,dword ptr [edi+24]
       imul      edx
       mov       ecx, eax
       mov       ebx, edx

   //    FixedMul(a->v[2].n[1], v->n[1]) +

       mov       eax,dword ptr [esi+4]
       mov       edx,dword ptr [edi+28]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

   //   FixedMul(a->v[2].n[2], v->n[2]);

       mov       eax,dword ptr [esi+8]
       mov       edx,dword ptr [edi+32]
       imul      edx
       add       ecx, eax
       adc       ebx, edx

       add       ecx, 0x8000
       adc       ebx, 0
       shrd      ecx, ebx, 16

       mov       edi, dword ptr ss:r_
       mov       dword ptr [edi+8], ecx      // r -> n[VZ]
       }
}


#else


#ifdef USE_FLOAT

void MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v)
{
    r->n[VX] = DOUBLE_TO_FIXED(
                 FIXED_TO_DOUBLE(a->v[0].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
                 FIXED_TO_DOUBLE(a->v[0].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
                 FIXED_TO_DOUBLE(a->v[0].n[2]) * FIXED_TO_DOUBLE(v->n[2])
                );

    r->n[VY] = DOUBLE_TO_FIXED(
                 FIXED_TO_DOUBLE(a->v[1].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
                 FIXED_TO_DOUBLE(a->v[1].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
                 FIXED_TO_DOUBLE(a->v[1].n[2]) * FIXED_TO_DOUBLE(v->n[2])
               );

    r->n[VZ] = DOUBLE_TO_FIXED(
                FIXED_TO_DOUBLE(a->v[2].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
                FIXED_TO_DOUBLE(a->v[2].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
                FIXED_TO_DOUBLE(a->v[2].n[2]) * FIXED_TO_DOUBLE(v->n[2])
               );
}


#else

void MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v)
{

#ifdef USE_INT64

    LCMSULONGLONG l1 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[0] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[1] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[2] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;

    LCMSULONGLONG l2 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[0] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[1] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[2] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;

    LCMSULONGLONG l3 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[0] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[1] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
                       (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[2] * 
                       (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;
    l1 >>= 16;
    l2 >>= 16;
    l3 >>= 16;

    r->n[VX] = (Fixed32) l1;               
    r->n[VY] = (Fixed32) l2;
    r->n[VZ] = (Fixed32) l3;

#else
    
    // FIXME: Rounding should be done at very last stage. There is 1-Contone rounding error!

    r->n[VX] = FixedMul(a->v[0].n[0], v->n[0]) +
               FixedMul(a->v[0].n[1], v->n[1]) +
               FixedMul(a->v[0].n[2], v->n[2]);

    r->n[VY] = FixedMul(a->v[1].n[0], v->n[0]) +
               FixedMul(a->v[1].n[1], v->n[1]) +
               FixedMul(a->v[1].n[2], v->n[2]);

    r->n[VZ] = FixedMul(a->v[2].n[0], v->n[0]) +
               FixedMul(a->v[2].n[1], v->n[1]) +
               FixedMul(a->v[2].n[2], v->n[2]);
#endif
}

#endif
#endif


void MAT3perK(LPMAT3 r, LPMAT3 v, double d)
{
       VEC3perK(&r -> v[0], &v -> v[0], d);
       VEC3perK(&r -> v[1], &v -> v[1], d);
       VEC3perK(&r -> v[2], &v -> v[2], d);
}


void MAT3toFix(LPWMAT3 r, LPMAT3 v)
{
       VEC3toFix(&r -> v[0], &v -> v[0]);
       VEC3toFix(&r -> v[1], &v -> v[1]);
       VEC3toFix(&r -> v[2], &v -> v[2]);
}

void MAT3fromFix(LPMAT3 r, LPWMAT3 v)
{
       VEC3fromFix(&r -> v[0], &v -> v[0]);
       VEC3fromFix(&r -> v[1], &v -> v[1]);
       VEC3fromFix(&r -> v[2], &v -> v[2]);
}



// Scale v by d and store it in r giving INTEGER

void VEC3scaleAndCut(LPWVEC3 r, LPVEC3 v, double d)
{
        r -> n[VX] = (int) floor(v -> n[VX] * d + .5);
        r -> n[VY] = (int) floor(v -> n[VY] * d + .5);
        r -> n[VZ] = (int) floor(v -> n[VZ] * d + .5);
}

void MAT3scaleAndCut(LPWMAT3 r, LPMAT3 v, double d)
{
       VEC3scaleAndCut(&r -> v[0], &v -> v[0], d);
       VEC3scaleAndCut(&r -> v[1], &v -> v[1], d);
       VEC3scaleAndCut(&r -> v[2], &v -> v[2], d);
}