dlls/d3drm/math.c

   1 /*
   2  * Copyright 2007 David Adam
   3  * Copyright 2007 Vijay Kiran Kamuju
   4  *
   5  * This library is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU Lesser General Public
   7  * License as published by the Free Software Foundation; either
   8  * version 2.1 of the License, or (at your option) any later version.
   9  *
  10  * This library is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Lesser General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Lesser General Public
  16  * License along with this library; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
  18  */
  19
  20 #define NONAMELESSUNION
  21
  22 #include <math.h>
  23 #include <stdarg.h>
  24 #include "windef.h"
  25 #include "winbase.h"
  26 #include "wingdi.h"
  27 #include "d3drmdef.h"
  28
  29 /* Create a RGB color from its components */
  30 D3DCOLOR WINAPI D3DRMCreateColorRGB(D3DVALUE red, D3DVALUE green, D3DVALUE blue)
  31 {
  32     return (D3DRMCreateColorRGBA(red, green, blue, 255.0));
  33 }
  34 /* Create a RGBA color from its components */
  35 D3DCOLOR WINAPI D3DRMCreateColorRGBA(D3DVALUE red, D3DVALUE green, D3DVALUE blue, D3DVALUE alpha)
  36 {
  37     int Red, Green, Blue, Alpha;
  38     Red=floor(red*255);
  39     Green=floor(green*255);
  40     Blue=floor(blue*255);
  41     Alpha=floor(alpha*255);
  42     if (red < 0) Red=0;
  43     if (red > 1) Red=255;
  44     if (green < 0) Green=0;
  45     if (green > 1) Green=255;
  46     if (blue < 0) Blue=0;
  47     if (blue > 1) Blue=255;
  48     if (alpha < 0) Alpha=0;
  49     if (alpha > 1) Alpha=255;
  50     return (RGBA_MAKE(Red, Green, Blue, Alpha));
  51 }
  52
  53 /* Determine the alpha part of a color */
  54 D3DVALUE WINAPI D3DRMColorGetAlpha(D3DCOLOR color)
  55 {
  56     return (RGBA_GETALPHA(color)/255.0);
  57 }
  58
  59 /* Determine the blue part of a color */
  60 D3DVALUE WINAPI D3DRMColorGetBlue(D3DCOLOR color)
  61 {
  62     return (RGBA_GETBLUE(color)/255.0);
  63 }
  64
  65 /* Determine the green part of a color */
  66 D3DVALUE WINAPI D3DRMColorGetGreen(D3DCOLOR color)
  67 {
  68     return (RGBA_GETGREEN(color)/255.0);
  69 }
  70
  71 /* Determine the red part of a color */
  72 D3DVALUE WINAPI D3DRMColorGetRed(D3DCOLOR color)
  73 {
  74     return (RGBA_GETRED(color)/255.0);
  75 }
  76
  77 /* Product of 2 quaternions */
  78 LPD3DRMQUATERNION WINAPI D3DRMQuaternionMultiply(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b)
  79 {
  80     D3DRMQUATERNION temp;
  81     D3DVECTOR cross_product;
  82
  83     D3DRMVectorCrossProduct(&cross_product, &a->v, &b->v);
  84     temp.s = a->s * b->s - D3DRMVectorDotProduct(&a->v, &b->v);
  85     temp.v.u1.x = a->s * b->v.u1.x + b->s * a->v.u1.x + cross_product.u1.x;
  86     temp.v.u2.y = a->s * b->v.u2.y + b->s * a->v.u2.y + cross_product.u2.y;
  87     temp.v.u3.z = a->s * b->v.u3.z + b->s * a->v.u3.z + cross_product.u3.z;
  88
  89     *q = temp;
  90     return q;
  91 }
  92
  93 /* Matrix for the Rotation that a unit quaternion represents */
  94 void WINAPI D3DRMMatrixFromQuaternion(D3DRMMATRIX4D m, LPD3DRMQUATERNION q)
  95 {
  96     D3DVALUE w,x,y,z;
  97     w = q->s;
  98     x = q->v.u1.x;
  99     y = q->v.u2.y;
 100     z = q->v.u3.z;
 101     m[0][0] = 1.0-2.0*(y*y+z*z);
 102     m[1][1] = 1.0-2.0*(x*x+z*z);
 103     m[2][2] = 1.0-2.0*(x*x+y*y);
 104     m[1][0] = 2.0*(x*y+z*w);
 105     m[0][1] = 2.0*(x*y-z*w);
 106     m[2][0] = 2.0*(x*z-y*w);
 107     m[0][2] = 2.0*(x*z+y*w);
 108     m[2][1] = 2.0*(y*z+x*w);
 109     m[1][2] = 2.0*(y*z-x*w);
 110     m[3][0] = 0.0;
 111     m[3][1] = 0.0;
 112     m[3][2] = 0.0;
 113     m[0][3] = 0.0;
 114     m[1][3] = 0.0;
 115     m[2][3] = 0.0;
 116     m[3][3] = 1.0;
 117 }
 118
 119 /* Return a unit quaternion that represents a rotation of an angle around an axis */
 120 LPD3DRMQUATERNION WINAPI D3DRMQuaternionFromRotation(LPD3DRMQUATERNION q, LPD3DVECTOR v, D3DVALUE theta)
 121 {
 122     q->s = cos(theta/2.0);
 123     D3DRMVectorScale(&q->v, D3DRMVectorNormalize(v), sin(theta/2.0));
 124     return q;
 125 }
 126
 127 /* Interpolation between two quaternions */
 128 LPD3DRMQUATERNION WINAPI D3DRMQuaternionSlerp(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b, D3DVALUE alpha)
 129 {
 130     D3DVALUE dot, epsilon, temp, theta, u;
 131
 132     dot = a->s * b->s + D3DRMVectorDotProduct(&a->v, &b->v);
 133     epsilon = 1.0f;
 134     temp = 1.0f - alpha;
 135     u = alpha;
 136     if (dot < 0.0)
 137     {
 138      epsilon = -1.0;
 139      dot = -dot;
 140     }
 141     if( 1.0f - dot > 0.001f )
 142     {
 143         theta = acos(dot);
 144         temp  = sin(theta * temp) / sin(theta);
 145         u = sin(theta * alpha) / sin(theta);
 146     }
 147     q->s = temp * a->s + epsilon * u * b->s;
 148     D3DRMVectorAdd(&q->v, D3DRMVectorScale(&a->v, &a->v, temp),
 149                    D3DRMVectorScale(&b->v, &b->v, epsilon * u));
 150     return q;
 151 }
 152
 153 /* Add Two Vectors */
 154 LPD3DVECTOR WINAPI D3DRMVectorAdd(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
 155 {
 156     D3DVECTOR temp;
 157
 158     temp.u1.x=s1->u1.x + s2->u1.x;
 159     temp.u2.y=s1->u2.y + s2->u2.y;
 160     temp.u3.z=s1->u3.z + s2->u3.z;
 161
 162     *d = temp;
 163     return d;
 164 }
 165
 166 /* Subtract Two Vectors */
 167 LPD3DVECTOR WINAPI D3DRMVectorSubtract(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
 168 {
 169     D3DVECTOR temp;
 170
 171     temp.u1.x=s1->u1.x - s2->u1.x;
 172     temp.u2.y=s1->u2.y - s2->u2.y;
 173     temp.u3.z=s1->u3.z - s2->u3.z;
 174
 175     *d = temp;
 176     return d;
 177 }
 178
 179 /* Cross Product of Two Vectors */
 180 LPD3DVECTOR WINAPI D3DRMVectorCrossProduct(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
 181 {
 182     D3DVECTOR temp;
 183
 184     temp.u1.x=s1->u2.y * s2->u3.z - s1->u3.z * s2->u2.y;
 185     temp.u2.y=s1->u3.z * s2->u1.x - s1->u1.x * s2->u3.z;
 186     temp.u3.z=s1->u1.x * s2->u2.y - s1->u2.y * s2->u1.x;
 187
 188     *d = temp;
 189     return d;
 190 }
 191
 192 /* Dot Product of Two vectors */
 193 D3DVALUE WINAPI D3DRMVectorDotProduct(LPD3DVECTOR s1, LPD3DVECTOR s2)
 194 {
 195     D3DVALUE dot_product;
 196     dot_product=s1->u1.x * s2->u1.x + s1->u2.y * s2->u2.y + s1->u3.z * s2->u3.z;
 197     return dot_product;
 198 }
 199
 200 /* Norm of a vector */
 201 D3DVALUE WINAPI D3DRMVectorModulus(LPD3DVECTOR v)
 202 {
 203     D3DVALUE result;
 204     result=sqrt(v->u1.x * v->u1.x + v->u2.y * v->u2.y + v->u3.z * v->u3.z);
 205     return result;
 206 }
 207
 208 /* Normalize a vector.  Returns (1,0,0) if INPUT is the NULL vector. */
 209 LPD3DVECTOR WINAPI D3DRMVectorNormalize(LPD3DVECTOR u)
 210 {
 211     D3DVALUE modulus = D3DRMVectorModulus(u);
 212     if(modulus)
 213     {
 214         D3DRMVectorScale(u,u,1.0/modulus);
 215     }
 216     else
 217     {
 218         u->u1.x=1.0;
 219         u->u2.y=0.0;
 220         u->u3.z=0.0;
 221     }
 222     return u;
 223 }
 224
 225 /* Returns a random unit vector */
 226 LPD3DVECTOR WINAPI D3DRMVectorRandom(LPD3DVECTOR d)
 227 {
 228     d->u1.x = rand();
 229     d->u2.y = rand();
 230     d->u3.z = rand();
 231     D3DRMVectorNormalize(d);
 232     return d;
 233 }
 234
 235 /* Reflection of a vector on a surface */
 236 LPD3DVECTOR WINAPI D3DRMVectorReflect(LPD3DVECTOR r, LPD3DVECTOR ray, LPD3DVECTOR norm)
 237 {
 238     D3DVECTOR sca, temp;
 239     D3DRMVectorSubtract(&temp, D3DRMVectorScale(&sca, norm, 2.0*D3DRMVectorDotProduct(ray,norm)), ray);
 240
 241     *r = temp;
 242     return r;
 243 }
 244
 245 /* Rotation of a vector */
 246 LPD3DVECTOR WINAPI D3DRMVectorRotate(LPD3DVECTOR r, LPD3DVECTOR v, LPD3DVECTOR axis, D3DVALUE theta)
 247 {
 248     D3DRMQUATERNION quaternion1, quaternion2, quaternion3;
 249     D3DVECTOR norm;
 250
 251     quaternion1.s = cos(theta * 0.5f);
 252     quaternion2.s = cos(theta * 0.5f);
 253     norm = *D3DRMVectorNormalize(axis);
 254     D3DRMVectorScale(&quaternion1.v, &norm, sin(theta * 0.5f));
 255     D3DRMVectorScale(&quaternion2.v, &norm, -sin(theta * 0.5f));
 256     quaternion3.s = 0.0;
 257     quaternion3.v = *v;
 258     D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion3);
 259     D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion2);
 260
 261     *r = *D3DRMVectorNormalize(&quaternion1.v);
 262     return r;
 263 }
 264
 265 /* Scale a vector */
 266 LPD3DVECTOR WINAPI D3DRMVectorScale(LPD3DVECTOR d, LPD3DVECTOR s, D3DVALUE factor)
 267 {
 268     D3DVECTOR temp;
 269
 270     temp.u1.x=factor * s->u1.x;
 271     temp.u2.y=factor * s->u2.y;
 272     temp.u3.z=factor * s->u3.z;
 273
 274     *d = temp;
 275     return d;
 276 }