home *** CD-ROM | disk | FTP | other *** search
/ The CDPD Public Domain Collection for CDTV 3 / CDPDIII.bin / pd / graphics / 3d / icoons / source / spl_math.c < prev    next >
C/C++ Source or Header  |  1992-10-04  |  11KB  |  332 lines
  1. /* :ts=8 */
  2. /************************************************************************/
  3. /*                                                                      */
  4. /* This file contains the hairy details of rotating and computing points*/
  5. /* on splines.                                */
  6. /*                                     */
  7. /* This file is a good candidate for optimization!            */
  8. /*                                                                      */
  9. /************************************************************************/
  10. #include  <math.h>
  11. #include  <stdio.h>
  12. #include <string.h>
  13. #include <ctype.h>
  14.  
  15. #include "general.h"
  16. #include "globals.h"
  17. #include "spl_math.h"
  18.  
  19.     /* The hermite basis matrix (Computer Graphics p. 484) */
  20. static Matrix4_T    M_Hermite    = {
  21.      2,    -2,     1,     1,
  22.     -3,     3,    -2,    -1,
  23.      0,     0,     1,     0,
  24.      1,     0,     0,     0
  25. };
  26.  
  27. void Matrix_Mult(Matrix_T MRes, Matrix_T M1, Matrix_T M2)
  28. /************************************************************************/
  29. /*                                                                      */
  30. /*                          ====   ==   ==                              */
  31. /*                 MRes = M1 * M2                */
  32. /*                                                                      */
  33. /************************************************************************/
  34. {
  35.     int        i, j;
  36.  
  37.     for (i = 0; i < 3; i++) 
  38.     for (j = 0; j < 3; j++) 
  39.         MRes[i][j] = M1[i][0] * M2[0][j] + 
  40.                      M1[i][1] * M2[1][j] + 
  41.                      M1[i][2] * M2[2][j];
  42.  
  43. } /* Matrix_Mult */
  44.  
  45.  
  46. void Matrix4_Mult(Matrix4_T MRes, Matrix4_T M1, Matrix4_T M2)
  47. /************************************************************************/
  48. /*                                                                      */
  49. /*                          ====   ==   ==                              */
  50. /*                 MRes = M1 * M2                */
  51. /*                                                                      */
  52. /************************************************************************/
  53. {
  54.     int        i, j;
  55.  
  56.     for (i = 0; i < 4; i++) 
  57.     for (j = 0; j < 4; j++) 
  58.         MRes[i][j] = M1[i][0] * M2[0][j] + 
  59.                      M1[i][1] * M2[1][j] + 
  60.                      M1[i][2] * M2[2][j] + 
  61.                      M1[i][3] * M2[3][j]; 
  62.  
  63. } /* Matrix4_Mult */
  64.  
  65. void Matrix_VMult(Vector_T VRes, Vector_T V, Matrix_T M)
  66. /************************************************************************/
  67. /*                                                                      */
  68. /*                          ----   -   =                                */
  69. /*                 VRes = V * M                */
  70. /*                                                                      */
  71. /************************************************************************/
  72. {
  73.     int        i;
  74.  
  75.     for (i = 0; i < 3; i++) 
  76.         VRes[i] = V[0] * M[0][i] +
  77.                   V[1] * M[1][i] +
  78.                   V[2] * M[2][i];
  79.  
  80. } /* Matrix_VMult */
  81.  
  82.  
  83. void Matrix4_VMult(Vector4_T VRes, Vector4_T V, Matrix4_T M)
  84. /************************************************************************/
  85. /*                                                                      */
  86. /*                          ----   -   =                                */
  87. /*                 VRes = V * M                */
  88. /*                                                                      */
  89. /************************************************************************/
  90. {
  91.     int        i;
  92.  
  93.     for (i = 0; i < 4; i++) 
  94.         VRes[i] = V[0] * M[0][i] +
  95.                   V[1] * M[1][i] +
  96.                   V[2] * M[2][i] +
  97.                   V[3] * M[3][i];
  98.  
  99. } /* Matrix4_VMult */
  100.  
  101. void Matrix_MultV(Vector_T VRes, Matrix_T M, Vector_T V)
  102. /************************************************************************/
  103. /*                                                                      */
  104. /*                          ----   =   -                                */
  105. /*                 VRes = M * V                */
  106. /*                                                                      */
  107. /************************************************************************/
  108. {
  109.     int        i;
  110.  
  111.     for (i = 0; i < 3; i++) 
  112.         VRes[i] = V[0] * M[i][0] +
  113.                   V[1] * M[i][1] +
  114.                   V[2] * M[i][2];
  115.  
  116. } /* Matrix_MultV */
  117.  
  118. void Compute_Rotation_Matrix(Matrix_T R_Matrix, Vector_T Rot) /* Degrees */
  119. /************************************************************************/
  120. /*                                                                      */
  121. /* Precompute 3d rotation matrix for a rotation of 'Rot[0]' degrees     */
  122. /* around the X-axis, 'Rot[1]' degrees around the Y-axis and 'Rot[2]'     */
  123. /* degrees around the Z-axis.                        */
  124. /*                                    */
  125. /************************************************************************/
  126. {
  127.     Matrix_T    M_X, M_Y, M_Z, M_Tmp;
  128.     double    Rot_X, Rot_Y, Rot_Z;
  129.     double    S, C;
  130.  
  131.     Rot_X = PI * Rot[0] / 180.0;
  132.     Rot_Y = PI * Rot[1] / 180.0;
  133.     Rot_Z = PI * Rot[2] / 180.0;
  134.  
  135.     S = sin(Rot_X);
  136.     C = cos(Rot_X);
  137.     M_X[0][0] = 1.0; M_X[1][0] = 0.0; M_X[2][0] = 0.0;
  138.     M_X[0][1] = 0.0; M_X[1][1] = C;   M_X[2][1] = S;
  139.     M_X[0][2] = 0.0; M_X[1][2] = -S;  M_X[2][2] = C;
  140.  
  141.     S = sin(Rot_Y);
  142.     C = cos(Rot_Y);
  143.     M_Y[0][0] = C;   M_Y[1][0] = 0.0; M_Y[2][0] = -S;
  144.     M_Y[0][1] = 0.0; M_Y[1][1] = 1.0; M_Y[2][1] = 0.0;
  145.     M_Y[0][2] = S;   M_Y[1][2] = 0.0; M_Y[2][2] = C;
  146.  
  147.     S = sin(Rot_Z);
  148.     C = cos(Rot_Z);
  149.     M_Z[0][0] = C;   M_Z[1][0] = S;   M_Z[2][0] = 0.0;
  150.     M_Z[0][1] = -S;  M_Z[1][1] = C;   M_Z[2][1] = 0.0;
  151.     M_Z[0][2] = 0.0; M_Z[1][2] = 0.0; M_Z[2][2] = 1.0;
  152.  
  153.     Matrix_Mult(M_Tmp, M_X,  M_Y);
  154.     Matrix_Mult(R_Matrix, M_Tmp, M_Z);
  155.  
  156. } /* Compute_Rotation_Matrix */
  157.  
  158. void Compute_Inverse_Rotation_Matrix(Matrix_T R_Matrix, Vector_T Rot)
  159. /************************************************************************/
  160. /*                                                                      */
  161. /* Precompute inverse 3d rotation matrix for a rotation of 'Rot[0]'     */
  162. /* degrees around the X-axis, 'Rot[1]' degrees around the Y-axis and     */
  163. /* 'Rot[2]' degrees around the Z-axis.                    */
  164. /*                                    */
  165. /************************************************************************/
  166. {
  167.     Matrix_T    M_X, M_Y, M_Z, M_Tmp;
  168.     double    Rot_X, Rot_Y, Rot_Z;
  169.     double    S, C;
  170.  
  171.     Rot_X = PI * Rot[0] / 180.0;
  172.     Rot_Y = PI * Rot[1] / 180.0;
  173.     Rot_Z = PI * Rot[2] / 180.0;
  174.  
  175.     S = sin(-Rot_X);
  176.     C = cos(-Rot_X);
  177.     M_X[0][0] = 1.0; M_X[1][0] = 0.0; M_X[2][0] = 0.0;
  178.     M_X[0][1] = 0.0; M_X[1][1] = C;   M_X[2][1] = S;
  179.     M_X[0][2] = 0.0; M_X[1][2] = -S;  M_X[2][2] = C;
  180.  
  181.     S = sin(-Rot_Y);
  182.     C = cos(-Rot_Y);
  183.     M_Y[0][0] = C;   M_Y[1][0] = 0.0; M_Y[2][0] = -S;
  184.     M_Y[0][1] = 0.0; M_Y[1][1] = 1.0; M_Y[2][1] = 0.0;
  185.     M_Y[0][2] = S;   M_Y[1][2] = 0.0; M_Y[2][2] = C;
  186.  
  187.     S = sin(-Rot_Z);
  188.     C = cos(-Rot_Z);
  189.     M_Z[0][0] = C;   M_Z[1][0] = S;   M_Z[2][0] = 0.0;
  190.     M_Z[0][1] = -S;  M_Z[1][1] = C;   M_Z[2][1] = 0.0;
  191.     M_Z[0][2] = 0.0; M_Z[1][2] = 0.0; M_Z[2][2] = 1.0;
  192.  
  193.     Matrix_Mult(M_Tmp, M_Z,  M_Y);
  194.     Matrix_Mult(R_Matrix, M_Tmp, M_X);
  195.  
  196. } /* Compute_Inverse_Rotation_Matrix */
  197.  
  198. void Precompute_Kochanek_Bartels(Matrix4_T Mres, 
  199.                  Spline_T *Spline, Knot_T *Knot)
  200. /************************************************************************/
  201. /*                                                                      */
  202. /* Precompute matrix used for Kochanek-Bartels spline interpolation.    */
  203. /* See Image Synthesis p. 36 - 38.                    */
  204. /*                                                                      */
  205. /************************************************************************/
  206. {
  207.     Matrix4_T    M_Tmp;
  208.     double    t, b, c, tm, bm, cm, bp, cp;
  209.     Knot_T    *Knot0, *Knot1, *Knot2, *Knot3;
  210.     short    PId0, PId1, PId2, PId3;
  211.     short    Nbr_Knots;
  212.  
  213.     Nbr_Knots    = Spline->Nbr_Knots;
  214.  
  215.         /* The knot before this knot */
  216.  
  217.     Knot0 = Knot->Previous;
  218.     if (Knot0 == NULL) Knot0 = Spline->Last;     /* Loop */
  219.     
  220.         /* This knot */
  221.  
  222.     Knot1 = Knot;
  223.  
  224.         /* The knot after this knot */
  225.  
  226.     Knot2 = Knot->Next;
  227.     if (Knot2 == NULL) Knot2 = Spline->First; /* Loop */
  228.  
  229.         /* The second knot after this knot */
  230.     
  231.     Knot3 = Knot2->Next;
  232.     if (Knot3 == NULL) Knot3 = Spline->First; /* Loop */
  233.  
  234.     PId0 = Knot0->Point_Id;
  235.     PId1 = Knot1->Point_Id;
  236.     PId2 = Knot2->Point_Id;
  237.     PId3 = Knot3->Point_Id;
  238.  
  239.     M_Tmp[0][0] = Points[PId1].Pos[0];
  240.     M_Tmp[0][1] = Points[PId1].Pos[1];
  241.     M_Tmp[0][2] = Points[PId1].Pos[2];
  242.     M_Tmp[0][3] = 0.0;
  243.  
  244.     M_Tmp[1][0] = Points[PId2].Pos[0];
  245.     M_Tmp[1][1] = Points[PId2].Pos[1];
  246.     M_Tmp[1][2] = Points[PId2].Pos[2];
  247.     M_Tmp[1][3] = 0.0;
  248.  
  249.     t = Knot1->Tension;      tm = 1.0 - t;
  250.     b = Knot1->Bias;      bm = 1.0 - b;  bp = 1.0 + b;
  251.     c = Knot1->Continuity;  cm = 1.0 - c;  cp = 1.0 + c;
  252.  
  253.     if (Knot != Spline->First || Spline->Loop) {
  254.  
  255.     M_Tmp[2][0] = 0.5 * tm * (
  256.          bm * cm * (Points[PId2].Pos[0] - Points[PId1].Pos[0]) +
  257.         bp * cp * (Points[PId1].Pos[0] - Points[PId0].Pos[0])
  258.     );
  259.     M_Tmp[2][1] = 0.5 * tm * (
  260.         bm * cm * (Points[PId2].Pos[1] - Points[PId1].Pos[1]) +
  261.         bp * cp * (Points[PId1].Pos[1] - Points[PId0].Pos[1])
  262.     );
  263.     M_Tmp[2][2] = 0.5 * tm * (
  264.             bm * cm * (Points[PId2].Pos[2] - Points[PId1].Pos[2]) +
  265.             bp * cp * (Points[PId1].Pos[2] - Points[PId0].Pos[2])
  266.     );
  267.  
  268.     M_Tmp[2][3] = 0.0;
  269.  
  270.     } else {
  271.  
  272.     M_Tmp[2][0] = 0.0;
  273.     M_Tmp[2][1] = 0.0;
  274.     M_Tmp[2][2] = 0.0;
  275.     M_Tmp[2][3] = 0.0;
  276.  
  277.     } /* if .. else .. */
  278.  
  279.     t = Knot2->Tension;      tm = 1.0 - t;
  280.     b = Knot2->Bias;      bm = 1.0 - b;  bp = 1.0 + b;
  281.     c = Knot2->Continuity;  cm = 1.0 - c;  cp = 1.0 + c;
  282.  
  283.     if (Knot->Next != NULL && Knot->Next->Next != NULL) {
  284.  
  285.     M_Tmp[3][0] = 0.5 * tm * (
  286.         bm * cp * (Points[PId3].Pos[0] - Points[PId2].Pos[0]) +
  287.         bp * cm * (Points[PId2].Pos[0] - Points[PId1].Pos[0])
  288.     );
  289.     M_Tmp[3][1] = 0.5 * tm * (
  290.         bm * cp * (Points[PId3].Pos[1] - Points[PId2].Pos[1]) +
  291.         bp * cm * (Points[PId2].Pos[1] - Points[PId1].Pos[1])
  292.     );
  293.     M_Tmp[3][2] = 0.5 * tm * (
  294.         bm * cp * (Points[PId3].Pos[2] - Points[PId2].Pos[2]) +
  295.         bp * cm * (Points[PId2].Pos[2] - Points[PId1].Pos[2])
  296.     );
  297.     M_Tmp[3][3] = 0.0;
  298.  
  299.     } else {
  300.  
  301.     M_Tmp[3][0] = 0.0;
  302.     M_Tmp[3][1] = 0.0;
  303.     M_Tmp[3][2] = 0.0;
  304.     M_Tmp[3][3] = 0.0;
  305.  
  306.     } /* if .. else .. */
  307.  
  308.  
  309.     Matrix4_Mult(Mres, M_Hermite, M_Tmp);
  310.  
  311. } /* Precompute_Kochanek_Bartels */
  312.  
  313. void Compute_Position(Vector4_T Vres, double T, Matrix4_T M_Precomp)
  314. /************************************************************************/
  315. /*                                                                      */
  316. /* Compute position on a Kochanek Bartels spline at parameter value T    */
  317. /* using the precomputed matrix M_Precomp.                */
  318. /* Return the result in Vres                        */
  319. /*                                                                      */
  320. /************************************************************************/
  321. {
  322.     Vector4_T    Vt;
  323.  
  324.     Vt[3] = 1.0;
  325.     Vt[2] = T;
  326.     Vt[1] = T * T;
  327.     Vt[0] = Vt[1] * T;
  328.  
  329.     Matrix4_VMult(Vres, Vt, M_Precomp);
  330.  
  331. } /* Compute_Position */
  332.