modules/lcms/src/cmsmtrx.c

   1 //
   2 //  Little cms
   3 //  Copyright (C) 1998-2007 Marti Maria
   4 //
   5 // Permission is hereby granted, free of charge, to any person obtaining
   6 // a copy of this software and associated documentation files (the "Software"),
   7 // to deal in the Software without restriction, including without limitation
   8 // the rights to use, copy, modify, merge, publish, distribute, sublicense,
   9 // and/or sell copies of the Software, and to permit persons to whom the Software
  10 // is furnished to do so, subject to the following conditions:
  11 //
  12 // The above copyright notice and this permission notice shall be included in
  13 // all copies or substantial portions of the Software.
  14 //
  15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  16 // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO
  17 // THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  18 // NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
  19 // LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
  20 // OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
  21 // WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  22
  23 // Vector & Matrix stuff
  24
  25 #include "lcms.h"
  26
  27
  28 void cdecl VEC3init(LPVEC3 r, double x, double y, double z);
  29 void cdecl VEC3initF(LPWVEC3 r, double x, double y, double z);
  30 void cdecl VEC3toFix(LPWVEC3 r, LPVEC3 v);
  31 void cdecl VEC3toFloat(LPFVEC3 r, LPVEC3 v);
  32 void cdecl VEC3scaleFix(LPWORD r, LPWVEC3 Scale);
  33 void cdecl VEC3swap(LPVEC3 a, LPVEC3 b);
  34 void cdecl VEC3divK(LPVEC3 r, LPVEC3 v, double d);
  35 void cdecl VEC3perK(LPVEC3 r, LPVEC3 v, double d);
  36 void cdecl VEC3perComp(LPVEC3 r, LPVEC3 a, LPVEC3 b);
  37 void cdecl VEC3minus(LPVEC3 r, LPVEC3 a, LPVEC3 b);
  38 void cdecl VEC3scaleAndCut(LPWVEC3 r, LPVEC3 v, double d);
  39 void cdecl VEC3cross(LPVEC3 r, LPVEC3 u, LPVEC3 v);
  40 void cdecl VEC3saturate(LPVEC3 v);
  41
  42 double cdecl VEC3length(LPVEC3 a);
  43 double cdecl VEC3distance(LPVEC3 a, LPVEC3 b);
  44
  45
  46 void      cdecl MAT3identity(LPMAT3 a);
  47 void      cdecl MAT3per(LPMAT3 r, LPMAT3 a, LPMAT3 b);
  48 int       cdecl MAT3inverse(LPMAT3 a, LPMAT3 b);
  49 LCMSBOOL  cdecl MAT3solve(LPVEC3 x, LPMAT3 a, LPVEC3 b);
  50 double    cdecl MAT3det(LPMAT3 m);
  51 void      cdecl MAT3eval(LPVEC3 r, LPMAT3 a, LPVEC3 v);
  52 void      cdecl MAT3toFix(LPWMAT3 r, LPMAT3 v);
  53 void      cdecl MAT3toFloat(LPFMAT3 r, LPMAT3 v);
  54 void      cdecl MAT3toFloatTranspose(LPFMAT3 r, LPMAT3 v);
  55 void      cdecl MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v);
  56 void      cdecl MAT3perK(LPMAT3 r, LPMAT3 v, double d);
  57 void      cdecl MAT3scaleAndCut(LPWMAT3 r, LPMAT3 v, double d);
  58
  59 // --------------------- Implementation ----------------------------
  60
  61 #define DSWAP(x, y)     {double tmp = (x); (x)=(y); (y)=tmp;}
  62
  63
  64
  65 #ifdef USE_ASSEMBLER
  66
  67
  68 #ifdef _MSC_VER
  69 #pragma warning(disable : 4033)
  70 #pragma warning(disable : 4035)
  71 #endif
  72
  73
  74
  75 Fixed32 FixedMul(Fixed32 a, Fixed32 b)
  76 {
  77        ASM {
  78
  79               mov    eax, ss:a
  80               mov    edx, ss:b
  81               imul   edx
  82               add    eax, 0x8000
  83               adc    edx, 0
  84               shrd   eax, edx, 16
  85
  86        }
  87
  88        RET(_EAX);
  89 }
  90
  91
  92
  93
  94 Fixed32 FixedSquare(Fixed32 a)
  95 {
  96        ASM {
  97               pushf
  98               push   edx
  99               mov    eax, ss:a
 100               imul   eax
 101               add    eax, 0x8000
 102               adc    edx, 0
 103               shrd   eax, edx, 16
 104               sar    eax, 16
 105               pop    edx
 106               popf
 107        }
 108
 109        RET(_EAX);
 110 }
 111
 112
 113
 114
 115 // Linear intERPolation
 116 // a * (h - l) >> 16 + l
 117
 118 Fixed32 FixedLERP(Fixed32 a, Fixed32 l, Fixed32 h)
 119 {
 120        ASM {
 121               mov    eax, dword ptr ss:h
 122               mov    edx, dword ptr ss:l
 123               push   edx
 124               mov    ecx, dword ptr ss:a
 125               sub    eax, edx
 126               imul   ecx
 127               add    eax, 0x8000
 128               adc    edx, 0
 129               shrd   eax, edx, 16
 130               pop    edx
 131               add    eax, edx
 132        }
 133
 134        RET(_EAX);
 135 }
 136
 137
 138 // a as word is scaled by s as float
 139
 140 WORD FixedScale(WORD a, Fixed32 s)
 141 {
 142        ASM {
 143
 144               xor    eax,eax
 145               mov    ax, ss:a        // This is faster that movzx  eax, ss:a
 146               sal    eax, 16
 147               mov    edx, ss:s
 148               mul    edx
 149               add    eax, 0x8000
 150               adc    edx, 0
 151               mov    eax, edx
 152        }
 153
 154        RET(_EAX);
 155 }
 156
 157 #ifdef _MSC_VER
 158 #pragma warning(default : 4033)
 159 #pragma warning(default : 4035)
 160 #endif
 161
 162 #else
 163
 164
 165 // These are floating point versions for compilers that doesn't
 166 // support asm at all. Use with care, since this will slow down
 167 // all operations
 168
 169
 170 Fixed32 FixedMul(Fixed32 a, Fixed32 b)
 171 {
 172 #ifdef USE_INT64
 173        LCMSULONGLONG l = (LCMSULONGLONG) (LCMSSLONGLONG) a * (LCMSULONGLONG) (LCMSSLONGLONG) b + (LCMSULONGLONG) 0x8000;
 174        l >>= 16;
 175        return (Fixed32) l;
 176 #else
 177        return DOUBLE_TO_FIXED(FIXED_TO_DOUBLE(a) * FIXED_TO_DOUBLE(b));
 178 #endif
 179 }
 180
 181 Fixed32 FixedSquare(Fixed32 a)
 182 {
 183        return FixedMul(a, a);
 184 }
 185
 186
 187 Fixed32 FixedLERP(Fixed32 a, Fixed32 l, Fixed32 h)
 188 {
 189 #ifdef USE_INT64
 190
 191        LCMSULONGLONG dif = (LCMSULONGLONG) (h - l) * a + 0x8000;
 192        dif = (dif >> 16) + l;
 193        return (Fixed32) (dif);
 194 #else
 195        double dif = h - l;
 196
 197        dif *= a;
 198        dif /= 65536.0;
 199        dif += l;
 200
 201        return (Fixed32) (dif + 0.5);
 202 #endif
 203
 204 }
 205
 206
 207 WORD FixedScale(WORD a, Fixed32 s)
 208 {
 209        return (WORD) (a * FIXED_TO_DOUBLE(s));
 210 }
 211
 212 #endif
 213
 214
 215 #ifndef USE_INLINE
 216
 217 Fixed32 ToFixedDomain(int a)
 218 {
 219     return a + ((a + 0x7fff) / 0xffff);
 220 }
 221
 222
 223 int FromFixedDomain(Fixed32 a)
 224 {
 225     return a - ((a + 0x7fff) >> 16);
 226 }
 227
 228
 229 Float ToFloatDomain(int a)
 230 {
 231     return ((float) a)/65536f;
 232 }
 233
 234 int FromFloatDomain(Float a)
 235 {
 236     return (int) (a * 65536f);
 237 }
 238
 239 #endif
 240
 241
 242 // Helper function to set up the alignment for LPFMAT3A
 243 void FMAT3ASetup(LPFMAT3A m)
 244 {
 245        m -> F = (LPFMAT3) (m -> _Buffer + (16 - (((unsigned) m -> _Buffer) % 16)));
 246 }
 247
 248
 249 // Initiate a vector (double version)
 250
 251
 252 void VEC3init(LPVEC3 r, double x, double y, double z)
 253 {
 254        r -> n[VX] = x;
 255        r -> n[VY] = y;
 256        r -> n[VZ] = z;
 257 }
 258
 259 // Init a vector (fixed version)
 260
 261 void VEC3initF(LPWVEC3 r, double x, double y, double z)
 262 {
 263        r -> n[VX] = DOUBLE_TO_FIXED(x);
 264        r -> n[VY] = DOUBLE_TO_FIXED(y);
 265        r -> n[VZ] = DOUBLE_TO_FIXED(z);
 266 }
 267
 268
 269 // Convert to fixed point encoding is 1.0 = 0xFFFF
 270
 271 void VEC3toFix(LPWVEC3 r, LPVEC3 v)
 272 {
 273        r -> n[VX] = DOUBLE_TO_FIXED(v -> n[VX]);
 274        r -> n[VY] = DOUBLE_TO_FIXED(v -> n[VY]);
 275        r -> n[VZ] = DOUBLE_TO_FIXED(v -> n[VZ]);
 276 }
 277
 278 // Convert to float
 279
 280 void VEC3toFloat(LPFVEC3 r, LPVEC3 v)
 281 {
 282        r -> n[VX] = DOUBLE_TO_FLOAT(v -> n[VX]);
 283        r -> n[VY] = DOUBLE_TO_FLOAT(v -> n[VY]);
 284        r -> n[VZ] = DOUBLE_TO_FLOAT(v -> n[VZ]);
 285 }
 286
 287 // Convert from fixed point
 288
 289 void VEC3fromFix(LPVEC3 r, LPWVEC3 v)
 290 {
 291        r -> n[VX] = FIXED_TO_DOUBLE(v -> n[VX]);
 292        r -> n[VY] = FIXED_TO_DOUBLE(v -> n[VY]);
 293        r -> n[VZ] = FIXED_TO_DOUBLE(v -> n[VZ]);
 294 }
 295
 296
 297 // Swap two double vectors
 298
 299 void VEC3swap(LPVEC3 a, LPVEC3 b)
 300 {
 301         DSWAP(a-> n[VX], b-> n[VX]);
 302         DSWAP(a-> n[VY], b-> n[VY]);
 303         DSWAP(a-> n[VZ], b-> n[VZ]);
 304 }
 305
 306 // Divide a vector by a constant
 307
 308 void VEC3divK(LPVEC3 r, LPVEC3 v, double d)
 309 {
 310         double d_inv = 1./d;
 311
 312         r -> n[VX] = v -> n[VX] * d_inv;
 313         r -> n[VY] = v -> n[VY] * d_inv;
 314         r -> n[VZ] = v -> n[VZ] * d_inv;
 315 }
 316
 317 // Multiply by a constant
 318
 319 void VEC3perK(LPVEC3 r, LPVEC3 v, double d )
 320 {
 321         r -> n[VX] = v -> n[VX] * d;
 322         r -> n[VY] = v -> n[VY] * d;
 323         r -> n[VZ] = v -> n[VZ] * d;
 324 }
 325
 326
 327 void VEC3perComp(LPVEC3 r, LPVEC3 a, LPVEC3 b)
 328 {
 329        r -> n[VX] = a->n[VX]*b->n[VX];
 330        r -> n[VY] = a->n[VY]*b->n[VY];
 331        r -> n[VZ] = a->n[VZ]*b->n[VZ];
 332 }
 333
 334 // Minus
 335
 336
 337 void VEC3minus(LPVEC3 r, LPVEC3 a, LPVEC3 b)
 338 {
 339   r -> n[VX] = a -> n[VX] - b -> n[VX];
 340   r -> n[VY] = a -> n[VY] - b -> n[VY];
 341   r -> n[VZ] = a -> n[VZ] - b -> n[VZ];
 342 }
 343
 344
 345 // Check id two vectors are the same, allowing tolerance
 346
 347 static
 348 LCMSBOOL RangeCheck(double l, double h, double v)
 349 {
 350        return (v >= l && v <= h);
 351 }
 352
 353
 354 LCMSBOOL VEC3equal(LPWVEC3 a, LPWVEC3 b, double Tolerance)
 355 {
 356        int i;
 357        double c;
 358
 359        for (i=0; i < 3; i++)
 360        {
 361               c = FIXED_TO_DOUBLE(a -> n[i]);
 362               if (!RangeCheck(c - Tolerance,
 363                               c + Tolerance,
 364                               FIXED_TO_DOUBLE(b->n[i]))) return FALSE;
 365        }
 366
 367        return TRUE;
 368 }
 369
 370 LCMSBOOL VEC3equalF(LPVEC3 a, LPVEC3 b, double Tolerance)
 371 {
 372        int i;
 373        double c;
 374
 375        for (i=0; i < 3; i++)
 376        {
 377               c = a -> n[i];
 378               if (!RangeCheck(c - Tolerance,
 379                               c + Tolerance,
 380                               b->n[i])) return FALSE;
 381        }
 382
 383        return TRUE;
 384 }
 385
 386 LCMSBOOL FVEC3equal(LPFVEC3 a, LPFVEC3 b, float Tolerance)
 387 {
 388        int i;
 389        float c;
 390
 391        for (i=0; i < 3; i++)
 392        {
 393               c = a -> n[i];
 394               if (!RangeCheck(c - Tolerance,
 395                               c + Tolerance,
 396                               b->n[i])) return FALSE;
 397        }
 398
 399        return TRUE;
 400 }
 401
 402
 403 void VEC3scaleFix(LPWORD r, LPWVEC3 Scale)
 404 {
 405        if (Scale -> n[VX] == 0x00010000L &&
 406            Scale -> n[VY] == 0x00010000L &&
 407            Scale -> n[VZ] == 0x00010000L) return;
 408
 409        r[0] = (WORD) FixedScale(r[0], Scale -> n[VX]);
 410        r[1] = (WORD) FixedScale(r[1], Scale -> n[VY]);
 411        r[2] = (WORD) FixedScale(r[2], Scale -> n[VZ]);
 412
 413 }
 414
 415
 416
 417 // Vector cross product
 418
 419 void VEC3cross(LPVEC3 r, LPVEC3 u, LPVEC3 v)
 420 {
 421
 422     r ->n[VX] = u->n[VY] * v->n[VZ] - v->n[VY] * u->n[VZ];
 423     r ->n[VY] = u->n[VZ] * v->n[VX] - v->n[VZ] * u->n[VX];
 424     r ->n[VZ] = u->n[VX] * v->n[VY] - v->n[VX] * u->n[VY];
 425 }
 426
 427
 428
 429 // The vector size
 430
 431 double VEC3length(LPVEC3 a)
 432 {
 433     return sqrt(a ->n[VX] * a ->n[VX] +
 434                 a ->n[VY] * a ->n[VY] +
 435                 a ->n[VZ] * a ->n[VZ]);
 436 }
 437
 438
 439 // Saturate a vector into 0..1.0 range
 440
 441 void VEC3saturate(LPVEC3 v)
 442 {
 443     int i;
 444     for (i=0; i < 3; i++) {
 445         if (v ->n[i] < 0)
 446                 v ->n[i] = 0;
 447         else
 448         if (v ->n[i] > 1.0)
 449                 v ->n[i] = 1.0;
 450     }
 451 }
 452
 453
 454 // Euclidean distance
 455
 456 double VEC3distance(LPVEC3 a, LPVEC3 b)
 457 {
 458     double d1 = a ->n[VX] - b ->n[VX];
 459     double d2 = a ->n[VY] - b ->n[VY];
 460     double d3 = a ->n[VZ] - b ->n[VZ];
 461
 462     return sqrt(d1*d1 + d2*d2 + d3*d3);
 463 }
 464
 465
 466 // Identity
 467
 468
 469 void MAT3identity(LPMAT3 a)
 470 {
 471         VEC3init(&a-> v[0], 1.0, 0.0, 0.0);
 472         VEC3init(&a-> v[1], 0.0, 1.0, 0.0);
 473         VEC3init(&a-> v[2], 0.0, 0.0, 1.0);
 474 }
 475
 476
 477
 478
 479 // Check if matrix is Identity. Allow a tolerance as %
 480
 481 LCMSBOOL MAT3isIdentity(LPWMAT3 a, double Tolerance)
 482 {
 483        int i;
 484        MAT3 Idd;
 485        WMAT3 Idf;
 486
 487        MAT3identity(&Idd);
 488        MAT3toFix(&Idf, &Idd);
 489
 490        for (i=0; i < 3; i++)
 491               if (!VEC3equal(&a -> v[i], &Idf.v[i], Tolerance)) return FALSE;
 492
 493        return TRUE;
 494
 495 }
 496
 497 // Floating point version of the above.
 498
 499 LCMSBOOL FMAT3isIdentity(LPFMAT3 a, float Tolerance)
 500 {
 501        int i;
 502        MAT3 Idd;
 503        FMAT3 Idf;
 504
 505        MAT3identity(&Idd);
 506        MAT3toFloat(&Idf, &Idd);
 507
 508        for (i=0; i < 3; i++)
 509               if (!FVEC3equal(&a -> v[i], &Idf.v[i], Tolerance)) return FALSE;
 510
 511        return TRUE;
 512
 513 }
 514 // Multiply two matrices
 515
 516
 517 void MAT3per(LPMAT3 r, LPMAT3 a, LPMAT3 b)
 518 {
 519 #define ROWCOL(i, j) \
 520     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]
 521
 522     VEC3init(&r-> v[0], ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2));
 523     VEC3init(&r-> v[1], ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2));
 524     VEC3init(&r-> v[2], ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2));
 525
 526 #undef ROWCOL //(i, j)
 527 }
 528
 529
 530
 531 // Inverse of a matrix b = a^(-1)
 532 // Gauss-Jordan elimination with partial pivoting
 533
 534 int MAT3inverse(LPMAT3 a, LPMAT3 b)
 535 {
 536     register int  i, j, max;
 537
 538     MAT3identity(b);
 539
 540     // Loop over cols of a from left to right, eliminating above and below diag
 541     for (j=0; j<3; j++) {   // Find largest pivot in column j among rows j..2
 542
 543     max = j;                 // Row with largest pivot candidate
 544     for (i=j+1; i<3; i++)
 545         if (fabs(a -> v[i].n[j]) > fabs(a -> v[max].n[j]))
 546             max = i;
 547
 548     // Swap rows max and j in a and b to put pivot on diagonal
 549
 550     VEC3swap(&a -> v[max], &a -> v[j]);
 551     VEC3swap(&b -> v[max], &b -> v[j]);
 552
 553     // Scale row j to have a unit diagonal
 554
 555     if (a -> v[j].n[j]==0.)
 556         return -1;                 // singular matrix; can't invert
 557
 558     VEC3divK(&b-> v[j], &b -> v[j], a->v[j].n[j]);
 559     VEC3divK(&a-> v[j], &a -> v[j], a->v[j].n[j]);
 560
 561     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
 562     for (i=0; i<3; i++)
 563
 564         if (i !=j) {
 565                   VEC3 temp;
 566
 567           VEC3perK(&temp, &b -> v[j], a -> v[i].n[j]);
 568           VEC3minus(&b -> v[i], &b -> v[i], &temp);
 569
 570           VEC3perK(&temp, &a -> v[j], a -> v[i].n[j]);
 571           VEC3minus(&a -> v[i], &a -> v[i], &temp);
 572     }
 573     }
 574
 575     return 1;
 576 }
 577
 578
 579 // Solve a system in the form Ax = b
 580
 581 LCMSBOOL MAT3solve(LPVEC3 x, LPMAT3 a, LPVEC3 b)
 582 {
 583         MAT3 m, a_1;
 584
 585         CopyMemory(&m, a, sizeof(MAT3));
 586
 587         if (!MAT3inverse(&m, &a_1)) return FALSE;  // Singular matrix
 588
 589         MAT3eval(x, &a_1, b);
 590         return TRUE;
 591 }
 592
 593
 594 // The determinant
 595
 596 double MAT3det(LPMAT3 m)
 597 {
 598
 599     double a1 = m ->v[VX].n[VX];
 600     double a2 = m ->v[VX].n[VY];
 601     double a3 = m ->v[VX].n[VZ];
 602     double b1 = m ->v[VY].n[VX];
 603     double b2 = m ->v[VY].n[VY];
 604     double b3 = m ->v[VY].n[VZ];
 605     double c1 = m ->v[VZ].n[VX];
 606     double c2 = m ->v[VZ].n[VY];
 607     double c3 = m ->v[VZ].n[VZ];
 608
 609
 610     return a1*b2*c3 - a1*b3*c2 + a2*b3*c1 - a2*b1*c3 - a3*b1*c2 - a3*b2*c1;
 611 }
 612
 613
 614 // linear transform
 615
 616
 617 void MAT3eval(LPVEC3 r, LPMAT3 a, LPVEC3 v)
 618 {
 619     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];
 620     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];
 621     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];
 622 }
 623
 624 void MAT3evalF(LPFVEC3 r, LPFMAT3 a, LPFVEC3 v)
 625 {
 626     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];
 627     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];
 628     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];
 629 }
 630
 631
 632 // Ok, this is another bottleneck of performance.
 633
 634
 635 #ifdef USE_ASSEMBLER
 636
 637 // ecx:ebx is result in 64 bits format
 638 // edi points to matrix, esi points to input vector
 639 // since only 3 accesses are in output, this is a stack variable
 640
 641
 642 void MAT3evalW(LPWVEC3 r_, LPWMAT3 a_, LPWVEC3 v_)
 643 {
 644
 645        ASM {
 646
 647
 648        mov    esi, dword ptr ss:v_
 649        mov    edi, dword ptr ss:a_
 650
 651    //     r->n[VX] = FixedMul(a->v[0].n[0], v->n[0]) +
 652
 653        mov       eax,dword ptr [esi]
 654        mov       edx,dword ptr [edi]
 655        imul      edx
 656        mov       ecx, eax
 657        mov       ebx, edx
 658
 659    //          FixedMul(a->v[0].n[1], v->n[1]) +
 660
 661        mov       eax,dword ptr [esi+4]
 662        mov       edx,dword ptr [edi+4]
 663        imul      edx
 664        add       ecx, eax
 665        adc       ebx, edx
 666
 667    //         FixedMul(a->v[0].n[2], v->n[2]);
 668
 669        mov       eax,dword ptr [esi+8]
 670        mov       edx,dword ptr [edi+8]
 671        imul      edx
 672        add       ecx, eax
 673        adc       ebx, edx
 674
 675    //  Back to Fixed 15.16
 676
 677        add       ecx, 0x8000
 678        adc       ebx, 0
 679        shrd      ecx, ebx, 16
 680
 681        push      edi
 682        mov       edi, dword ptr ss:r_
 683        mov       dword ptr [edi], ecx      //  r -> n[VX]
 684        pop       edi
 685
 686
 687
 688    //   2nd row ***************************
 689
 690    //        FixedMul(a->v[1].n[0], v->n[0])
 691
 692        mov       eax,dword ptr [esi]
 693        mov       edx,dword ptr [edi+12]
 694        imul      edx
 695        mov       ecx, eax
 696        mov       ebx, edx
 697
 698    //         FixedMul(a->v[1].n[1], v->n[1]) +
 699
 700        mov       eax,dword ptr [esi+4]
 701        mov       edx,dword ptr [edi+16]
 702        imul      edx
 703        add       ecx, eax
 704        adc       ebx, edx
 705
 706        //     FixedMul(a->v[1].n[2], v->n[2]);
 707
 708        mov       eax,dword ptr [esi+8]
 709        mov       edx,dword ptr [edi+20]
 710        imul      edx
 711        add       ecx, eax
 712        adc       ebx, edx
 713
 714        add       ecx, 0x8000
 715        adc       ebx, 0
 716        shrd      ecx, ebx, 16
 717
 718        push      edi
 719        mov       edi, dword ptr ss:r_
 720        mov       dword ptr [edi+4], ecx      // r -> n[VY]
 721        pop       edi
 722
 723 //     3d row **************************
 724
 725    //       r->n[VZ] = FixedMul(a->v[2].n[0], v->n[0]) +
 726
 727        mov       eax,dword ptr [esi]
 728        mov       edx,dword ptr [edi+24]
 729        imul      edx
 730        mov       ecx, eax
 731        mov       ebx, edx
 732
 733    //    FixedMul(a->v[2].n[1], v->n[1]) +
 734
 735        mov       eax,dword ptr [esi+4]
 736        mov       edx,dword ptr [edi+28]
 737        imul      edx
 738        add       ecx, eax
 739        adc       ebx, edx
 740
 741    //   FixedMul(a->v[2].n[2], v->n[2]);
 742
 743        mov       eax,dword ptr [esi+8]
 744        mov       edx,dword ptr [edi+32]
 745        imul      edx
 746        add       ecx, eax
 747        adc       ebx, edx
 748
 749        add       ecx, 0x8000
 750        adc       ebx, 0
 751        shrd      ecx, ebx, 16
 752
 753        mov       edi, dword ptr ss:r_
 754        mov       dword ptr [edi+8], ecx      // r -> n[VZ]
 755        }
 756 }
 757
 758
 759 #else
 760
 761
 762 #ifdef USE_FLOAT
 763
 764 void MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v)
 765 {
 766     r->n[VX] = DOUBLE_TO_FIXED(
 767                  FIXED_TO_DOUBLE(a->v[0].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
 768                  FIXED_TO_DOUBLE(a->v[0].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
 769                  FIXED_TO_DOUBLE(a->v[0].n[2]) * FIXED_TO_DOUBLE(v->n[2])
 770                 );
 771
 772     r->n[VY] = DOUBLE_TO_FIXED(
 773                  FIXED_TO_DOUBLE(a->v[1].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
 774                  FIXED_TO_DOUBLE(a->v[1].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
 775                  FIXED_TO_DOUBLE(a->v[1].n[2]) * FIXED_TO_DOUBLE(v->n[2])
 776                );
 777
 778     r->n[VZ] = DOUBLE_TO_FIXED(
 779                 FIXED_TO_DOUBLE(a->v[2].n[0]) * FIXED_TO_DOUBLE(v->n[0]) +
 780                 FIXED_TO_DOUBLE(a->v[2].n[1]) * FIXED_TO_DOUBLE(v->n[1]) +
 781                 FIXED_TO_DOUBLE(a->v[2].n[2]) * FIXED_TO_DOUBLE(v->n[2])
 782                );
 783 }
 784
 785
 786 #else
 787
 788 void MAT3evalW(LPWVEC3 r, LPWMAT3 a, LPWVEC3 v)
 789 {
 790
 791 #ifdef USE_INT64
 792
 793     LCMSULONGLONG l1 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[0] *
 794                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
 795                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[1] *
 796                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
 797                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[0].n[2] *
 798                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;
 799
 800     LCMSULONGLONG l2 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[0] *
 801                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
 802                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[1] *
 803                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
 804                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[1].n[2] *
 805                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;
 806
 807     LCMSULONGLONG l3 = (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[0] *
 808                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[0] +
 809                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[1] *
 810                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[1] +
 811                        (LCMSULONGLONG) (LCMSSLONGLONG) a->v[2].n[2] *
 812                        (LCMSULONGLONG) (LCMSSLONGLONG) v->n[2] + (LCMSULONGLONG) 0x8000;
 813     l1 >>= 16;
 814     l2 >>= 16;
 815     l3 >>= 16;
 816
 817     r->n[VX] = (Fixed32) l1;
 818     r->n[VY] = (Fixed32) l2;
 819     r->n[VZ] = (Fixed32) l3;
 820
 821 #else
 822
 823     // FIXME: Rounding should be done at very last stage. There is 1-Contone rounding error!
 824
 825     r->n[VX] = FixedMul(a->v[0].n[0], v->n[0]) +
 826                FixedMul(a->v[0].n[1], v->n[1]) +
 827                FixedMul(a->v[0].n[2], v->n[2]);
 828
 829     r->n[VY] = FixedMul(a->v[1].n[0], v->n[0]) +
 830                FixedMul(a->v[1].n[1], v->n[1]) +
 831                FixedMul(a->v[1].n[2], v->n[2]);
 832
 833     r->n[VZ] = FixedMul(a->v[2].n[0], v->n[0]) +
 834                FixedMul(a->v[2].n[1], v->n[1]) +
 835                FixedMul(a->v[2].n[2], v->n[2]);
 836 #endif
 837 }
 838
 839 #endif
 840 #endif
 841
 842
 843 void MAT3perK(LPMAT3 r, LPMAT3 v, double d)
 844 {
 845        VEC3perK(&r -> v[0], &v -> v[0], d);
 846        VEC3perK(&r -> v[1], &v -> v[1], d);
 847        VEC3perK(&r -> v[2], &v -> v[2], d);
 848 }
 849
 850
 851 void MAT3toFix(LPWMAT3 r, LPMAT3 v)
 852 {
 853        VEC3toFix(&r -> v[0], &v -> v[0]);
 854        VEC3toFix(&r -> v[1], &v -> v[1]);
 855        VEC3toFix(&r -> v[2], &v -> v[2]);
 856 }
 857
 858 void MAT3toFloat(LPFMAT3 r, LPMAT3 v)
 859 {
 860        VEC3toFloat(&r -> v[0], &v -> v[0]);
 861        VEC3toFloat(&r -> v[1], &v -> v[1]);
 862        VEC3toFloat(&r -> v[2], &v -> v[2]);
 863 }
 864
 865 void MAT3toFloatTranspose(LPFMAT3 r, LPMAT3 v)
 866 {
 867        unsigned i, j;
 868
 869        /* for each row of the source. */
 870        for (i = 0; i < 3; ++i)
 871
 872            /* For element in the row. */
 873            for (j = 0; j < 3; ++j)
 874
 875                /* Col=>Row, Row=>Col. */
 876                r -> v[j].n[i] = DOUBLE_TO_FLOAT(v -> v[i].n[j]);
 877 }
 878
 879 void MAT3fromFix(LPMAT3 r, LPWMAT3 v)
 880 {
 881        VEC3fromFix(&r -> v[0], &v -> v[0]);
 882        VEC3fromFix(&r -> v[1], &v -> v[1]);
 883        VEC3fromFix(&r -> v[2], &v -> v[2]);
 884 }
 885
 886
 887
 888 // Scale v by d and store it in r giving INTEGER
 889
 890 void VEC3scaleAndCut(LPWVEC3 r, LPVEC3 v, double d)
 891 {
 892         r -> n[VX] = (int) floor(v -> n[VX] * d + .5);
 893         r -> n[VY] = (int) floor(v -> n[VY] * d + .5);
 894         r -> n[VZ] = (int) floor(v -> n[VZ] * d + .5);
 895 }
 896
 897 void MAT3scaleAndCut(LPWMAT3 r, LPMAT3 v, double d)
 898 {
 899        VEC3scaleAndCut(&r -> v[0], &v -> v[0], d);
 900        VEC3scaleAndCut(&r -> v[1], &v -> v[1], d);
 901        VEC3scaleAndCut(&r -> v[2], &v -> v[2], d);
 902 }
 903
 904
 905
 906