cmsmtrx.c

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

//
//  Little cms
//  Copyright (C) 1998-2005 Marti Maria
//
// Permission is hereby granted, free of charge, to any person obtaining 
// a copy of this software and associated documentation files (the "Software"), 
// to deal in the Software without restriction, including without limitation 
// the rights to use, copy, modify, merge, publish, distribute, sublicense, 
// and/or sell copies of the Software, and to permit persons to whom the Software 
// is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in 
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO 
// THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

// Vector & Matrix stuff

#include "lcms.h"


void cdecl VEC3init(LPVEC3 r, double x, double y, double z);
void cdecl VEC3initF(LPWVEC3 r, double x, double y, double z);
void cdecl VEC3toFix(LPWVEC3 r, LPVEC3 v);
void cdecl VEC3scaleFix(LPWORD r, LPWVEC3 Scale);
void cdecl VEC3swap(LPVEC3 a, LPVEC3 b);
void cdecl VEC3divK(LPVEC3 r, LPVEC3 v, double d);
void cdecl VEC3perK(LPVEC3 r, LPVEC3 v, double d);
void cdecl VEC3perComp(LPVEC3 r, LPVEC3 a, LPVEC3 b);
void cdecl VEC3minus(LPVEC3 r, LPVEC3 a, LPVEC3 b);
void cdecl VEC3scaleAndCut(LPWVEC3 r, LPVEC3 v, double d);
void cdecl VEC3cross(LPVEC3 r, LPVEC3 u, LPVEC3 v);
void cdecl VEC3saturate(LPVEC3 v);

double cdecl VEC3length(LPVEC3 a);
double cdecl VEC3distance(LPVEC3 a, LPVEC3 b);


void   cdecl MAT3identity(LPMAT3 a);
void   cdecl MAT3per(LPMAT3 r, LPMAT3 a, LPMAT3 b);
int    cdecl MAT3inverse(LPMAT3 a, LPMAT3 b);
BOOL   cdecl MAT3solve(LPVEC3 x, LPMAT3 a, LPVEC3 b);
double cdecl MAT3det(LPMAT3 m);
void   cdecl MAT3eval(LPVEC3 r, LPMAT3 a, LPVEC3 v);
void   cdecl MAT3toFix(LPWMAT3 r, LPMAT3 v);
void   cdecl MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v);
void   cdecl MAT3perK(LPMAT3 r, LPMAT3 v, double d);
void   cdecl MAT3scaleAndCut(LPWMAT3 r, LPMAT3 v, double d);

// --------------------- Implementation ----------------------------

#define DSWAP(x, y)     {double tmp = (x); (x)=(y); (y)=tmp;}



#ifdef USE_ASSEMBLER


#ifdef _MSC_VER
#pragma warning(disable : 4033)
#endif



Fixed32 FixedMul(Fixed32 a, Fixed32 b)
{
       ASM {
              
              mov    eax, ss:a
              mov    edx, ss:b
              imul   edx
              add    eax, 0x8000
              adc    edx, 0
              shrd   eax, edx, 16
              
       }

       RET(_EAX);
}




Fixed32 FixedSquare(Fixed32 a)
{
       ASM {
              pushf
              push   edx
              mov    eax, ss:a
              imul   eax
              add    eax, 0x8000
              adc    edx, 0
              shrd   eax, edx, 16
              sar    eax, 16
              pop    edx
              popf
       }

       RET(_EAX);
}




// Linear intERPolation
// a * (h - l) >> 16 + l

Fixed32 FixedLERP(Fixed32 a, Fixed32 l, Fixed32 h)
{
       ASM {
              mov    eax, dword ptr ss:h
              mov    edx, dword ptr ss:l
              push   edx
              mov    ecx, dword ptr ss:a
              sub    eax, edx
              imul   ecx
              add    eax, 0x8000
              adc    edx, 0
              shrd   eax, edx, 16
              pop    edx
              add    eax, edx
       }

       RET(_EAX);
}


// a as word is scaled by s as float

WORD FixedScale(WORD a, Fixed32 s)
{
       ASM {

              xor    eax,eax
              mov    ax, ss:a        // This is faster that movzx  eax, ss:a
              sal    eax, 16
              mov    edx, ss:s
              mul    edx
              add    eax, 0x8000
              adc    edx, 0
              mov    eax, edx
       }

       RET(_EAX);
}

#ifdef _MSC_VER
#pragma warning(default : 4033)
#endif

#else


// These are floating point versions for compilers that doesn't
// support asm at all. Use with care, since this will slow down
// all operations


Fixed32 FixedMul(Fixed32 a, Fixed32 b)
{
#ifdef USE_INT64
       LCMSULONGLONG l = (LCMSULONGLONG) (LCMSSLONGLONG) a * (LCMSULONGLONG) (LCMSSLONGLONG) b + (LCMSULONGLONG) 0x8000;
       l >>= 16;
       return (Fixed32) l;
#else
       return DOUBLE_TO_FIXED(FIXED_TO_DOUBLE(a) * FIXED_TO_DOUBLE(b));
#endif
}

Fixed32 FixedSquare(Fixed32 a)
{
       return FixedMul(a, a);
}


Fixed32 FixedLERP(Fixed32 a, Fixed32 l, Fixed32 h)
{
#ifdef USE_INT64

       LCMSULONGLONG dif = (LCMSULONGLONG) (h - l) * a + 0x8000;           
       dif = (dif >> 16) + l;        
       return (Fixed32) (dif);
#else
       double dif = h - l;

       dif *= a;
       dif /= 65536.0;
       dif += l;

       return (Fixed32) (dif + 0.5);
#endif
     
}


WORD FixedScale(WORD a, Fixed32 s)
{
       return (WORD) (a * FIXED_TO_DOUBLE(s));
}

#endif


#ifndef USE_INLINE

Fixed32 ToFixedDomain(int a)
{
    return a + ((a + 0x7fff) / 0xffff);
}


int FromFixedDomain(Fixed32 a)
{
    return a - ((a + 0x7fff) >> 16); 
}

#endif



// Initiate a vector (double version)


void VEC3init(LPVEC3 r, double x, double y, double z)
{
       r -> n[VX] = x;
       r -> n[VY] = y;
       r -> n[VZ] = z;
}

// Init a vector (fixed version)

void VEC3initF(LPWVEC3 r, double x, double y, double z)
{
       r -> n[VX] = DOUBLE_TO_FIXED(x);
       r -> n[VY] = DOUBLE_TO_FIXED(y);
       r -> n[VZ] = DOUBLE_TO_FIXED(z);
}


// Convert to fixed point encoding is 1.0 = 0xFFFF

void VEC3toFix(LPWVEC3 r, LPVEC3 v)
{
       r -> n[VX] = DOUBLE_TO_FIXED(v -> n[VX]);
       r -> n[VY] = DOUBLE_TO_FIXED(v -> n[VY]);
       r -> n[VZ] = DOUBLE_TO_FIXED(v -> n[VZ]);
}

// Convert from fixed point

void VEC3fromFix(LPVEC3 r, LPWVEC3 v)
{
       r -> n[VX] = FIXED_TO_DOUBLE(v -> n[VX]);
       r -> n[VY] = FIXED_TO_DOUBLE(v -> n[VY]);
       r -> n[VZ] = FIXED_TO_DOUBLE(v -> n[VZ]);
}


// Swap two double vectors

void VEC3swap(LPVEC3 a, LPVEC3 b)
{
        DSWAP(a-> n[VX], b-> n[VX]);
        DSWAP(a-> n[VY], b-> n[VY]);
        DSWAP(a-> n[VZ], b-> n[VZ]);
}

// Divide a vector by a constant

void VEC3divK(LPVEC3 r, LPVEC3 v, double d)
{
        double d_inv = 1./d;

        r -> n[VX] = v -> n[VX] * d_inv;
        r -> n[VY] = v -> n[VY] * d_inv;
        r -> n[VZ] = v -> n[VZ] * d_inv;
}

// Multiply by a constant

void VEC3perK(LPVEC3 r, LPVEC3 v, double d )
{
        r -> n[VX] = v -> n[VX] * d;
        r -> n[VY] = v -> n[VY] * d;
        r -> n[VZ] = v -> n[VZ] * d;
}


void VEC3perComp(LPVEC3 r, LPVEC3 a, LPVEC3 b)
{
       r -> n[VX] = a->n[VX]*b->n[VX];
       r -> n[VY] = a->n[VY]*b->n[VY];
       r -> n[VZ] = a->n[VZ]*b->n[VZ];
}

// Minus


void VEC3minus(LPVEC3 r, LPVEC3 a, LPVEC3 b)
{
  r -> n[VX] = a -> n[VX] - b -> n[VX];
  r -> n[VY] = a -> n[VY] - b -> n[VY];
  r -> n[VZ] = a -> n[VZ] - b -> n[VZ];
}


// Check id two vectors are the same, allowing tolerance

static
BOOL RangeCheck(double l, double h, double v)
{
       return (v >= l && v <= h);
}


BOOL VEC3equal(LPWVEC3 a, LPWVEC3 b, double Tolerance)
{
       int i;
       double c;

       for (i=0; i < 3; i++)
       {
              c = FIXED_TO_DOUBLE(a -> n[i]);
              if (!RangeCheck(c - Tolerance,
                              c + Tolerance,
                              FIXED_TO_DOUBLE(b->n[i]))) return FALSE;
       }

       return TRUE;
}

BOOL VEC3equalF(LPVEC3 a, LPVEC3 b, double Tolerance)
{
       int i;
       double c;

       for (i=0; i < 3; i++)
       {
              c = a -> n[i];
              if (!RangeCheck(c - Tolerance,
                              c + Tolerance,
                              b->n[i])) return FALSE;
       }

       return TRUE;
}


void VEC3scaleFix(LPWORD r, LPWVEC3 Scale)
{
       if (Scale -> n[VX] == 0x00010000L &&
           Scale -> n[VY] == 0x00010000L &&
           Scale -> n[VZ] == 0x00010000L) return;

       r[0] = (WORD) FixedScale(r[0], Scale -> n[VX]);
       r[1] = (WORD) FixedScale(r[1], Scale -> n[VY]);
       r[2] = (WORD) FixedScale(r[2], Scale -> n[VZ]);

}



// Vector cross product

void VEC3cross(LPVEC3 r, LPVEC3 u, LPVEC3 v)
{

    r ->n[VX] = u->n[VY] * v->n[VZ] - v->n[VY] * u->n[VZ];
    r ->n[VY] = u->n[VZ] * v->n[VX] - v->n[VZ] * u->n[VX];
    r ->n[VZ] = u->n[VX] * v->n[VY] - v->n[VX] * u->n[VY];
}
    


// The vector size

double VEC3length(LPVEC3 a)
{
    return sqrt(a ->n[VX] * a ->n[VX] +
                a ->n[VY] * a ->n[VY] +
                a ->n[VZ] * a ->n[VZ]);
}


// Saturate a vector into 0..1.0 range

void VEC3saturate(LPVEC3 v)
{
    int i;
    for (i=0; i < 3; i++) {
        if (v ->n[i] < 0)
                v ->n[i] = 0;
        else
        if (v ->n[i] > 1.0)
                v ->n[i] = 1.0;
    }
}


// Euclidean distance