reactos/dll/directx/wine/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 #include "d3drm_private.h"
  21
  22 #include <math.h>
  23
  24 /* Create a RGB color from its components */
  25 D3DCOLOR WINAPI D3DRMCreateColorRGB(D3DVALUE red, D3DVALUE green, D3DVALUE blue)
  26 {
  27     return (D3DRMCreateColorRGBA(red, green, blue, 255.0));
  28 }
  29 /* Create a RGBA color from its components */
  30 D3DCOLOR WINAPI D3DRMCreateColorRGBA(D3DVALUE red, D3DVALUE green, D3DVALUE blue, D3DVALUE alpha)
  31 {
  32     int Red, Green, Blue, Alpha;
  33     Red=floor(red*255);
  34     Green=floor(green*255);
  35     Blue=floor(blue*255);
  36     Alpha=floor(alpha*255);
  37     if (red < 0) Red=0;
  38     if (red > 1) Red=255;
  39     if (green < 0) Green=0;
  40     if (green > 1) Green=255;
  41     if (blue < 0) Blue=0;
  42     if (blue > 1) Blue=255;
  43     if (alpha < 0) Alpha=0;
  44     if (alpha > 1) Alpha=255;
  45     return (RGBA_MAKE(Red, Green, Blue, Alpha));
  46 }
  47
  48 /* Determine the alpha part of a color */
  49 D3DVALUE WINAPI D3DRMColorGetAlpha(D3DCOLOR color)
  50 {
  51     return (RGBA_GETALPHA(color)/255.0);
  52 }
  53
  54 /* Determine the blue part of a color */
  55 D3DVALUE WINAPI D3DRMColorGetBlue(D3DCOLOR color)
  56 {
  57     return (RGBA_GETBLUE(color)/255.0);
  58 }
  59
  60 /* Determine the green part of a color */
  61 D3DVALUE WINAPI D3DRMColorGetGreen(D3DCOLOR color)
  62 {
  63     return (RGBA_GETGREEN(color)/255.0);
  64 }
  65
  66 /* Determine the red part of a color */
  67 D3DVALUE WINAPI D3DRMColorGetRed(D3DCOLOR color)
  68 {
  69     return (RGBA_GETRED(color)/255.0);
  70 }
  71
  72 /* Product of 2 quaternions */
  73 D3DRMQUATERNION * WINAPI D3DRMQuaternionMultiply(D3DRMQUATERNION *q, D3DRMQUATERNION *a, D3DRMQUATERNION *b)
  74 {
  75     D3DRMQUATERNION temp;
  76     D3DVECTOR cross_product;
  77
  78     D3DRMVectorCrossProduct(&cross_product, &a->v, &b->v);
  79     temp.s = a->s * b->s - D3DRMVectorDotProduct(&a->v, &b->v);
  80     temp.v.u1.x = a->s * b->v.u1.x + b->s * a->v.u1.x + cross_product.u1.x;
  81     temp.v.u2.y = a->s * b->v.u2.y + b->s * a->v.u2.y + cross_product.u2.y;
  82     temp.v.u3.z = a->s * b->v.u3.z + b->s * a->v.u3.z + cross_product.u3.z;
  83
  84     *q = temp;
  85     return q;
  86 }
  87
  88 /* Matrix for the Rotation that a unit quaternion represents */
  89 void WINAPI D3DRMMatrixFromQuaternion(D3DRMMATRIX4D m, D3DRMQUATERNION *q)
  90 {
  91     D3DVALUE w,x,y,z;
  92     w = q->s;
  93     x = q->v.u1.x;
  94     y = q->v.u2.y;
  95     z = q->v.u3.z;
  96     m[0][0] = 1.0-2.0*(y*y+z*z);
  97     m[1][1] = 1.0-2.0*(x*x+z*z);
  98     m[2][2] = 1.0-2.0*(x*x+y*y);
  99     m[1][0] = 2.0*(x*y+z*w);
 100     m[0][1] = 2.0*(x*y-z*w);
 101     m[2][0] = 2.0*(x*z-y*w);
 102     m[0][2] = 2.0*(x*z+y*w);
 103     m[2][1] = 2.0*(y*z+x*w);
 104     m[1][2] = 2.0*(y*z-x*w);
 105     m[3][0] = 0.0;
 106     m[3][1] = 0.0;
 107     m[3][2] = 0.0;
 108     m[0][3] = 0.0;
 109     m[1][3] = 0.0;
 110     m[2][3] = 0.0;
 111     m[3][3] = 1.0;
 112 }
 113
 114 /* Return a unit quaternion that represents a rotation of an angle around an axis */
 115 D3DRMQUATERNION * WINAPI D3DRMQuaternionFromRotation(D3DRMQUATERNION *q, D3DVECTOR *v, D3DVALUE theta)
 116 {
 117     q->s = cos(theta/2.0);
 118     D3DRMVectorScale(&q->v, D3DRMVectorNormalize(v), sin(theta/2.0));
 119     return q;
 120 }
 121
 122 /* Interpolation between two quaternions */
 123 D3DRMQUATERNION * WINAPI D3DRMQuaternionSlerp(D3DRMQUATERNION *q,
 124         D3DRMQUATERNION *a, D3DRMQUATERNION *b, D3DVALUE alpha)
 125 {
 126     D3DVALUE dot, epsilon, temp, theta, u;
 127     D3DVECTOR v1, v2;
 128
 129     dot = a->s * b->s + D3DRMVectorDotProduct(&a->v, &b->v);
 130     epsilon = 1.0f;
 131     temp = 1.0f - alpha;
 132     u = alpha;
 133     if (dot < 0.0)
 134     {
 135      epsilon = -1.0;
 136      dot = -dot;
 137     }
 138     if( 1.0f - dot > 0.001f )
 139     {
 140         theta = acos(dot);
 141         temp  = sin(theta * temp) / sin(theta);
 142         u = sin(theta * alpha) / sin(theta);
 143     }
 144     q->s = temp * a->s + epsilon * u * b->s;
 145     D3DRMVectorScale(&v1, &a->v, temp);
 146     D3DRMVectorScale(&v2, &b->v, epsilon * u);
 147     D3DRMVectorAdd(&q->v, &v1, &v2);
 148     return q;
 149 }
 150
 151 /* Add Two Vectors */
 152 D3DVECTOR * WINAPI D3DRMVectorAdd(D3DVECTOR *d, D3DVECTOR *s1, D3DVECTOR *s2)
 153 {
 154     D3DVECTOR temp;
 155
 156     temp.u1.x=s1->u1.x + s2->u1.x;
 157     temp.u2.y=s1->u2.y + s2->u2.y;
 158     temp.u3.z=s1->u3.z + s2->u3.z;
 159
 160     *d = temp;
 161     return d;
 162 }
 163
 164 /* Subtract Two Vectors */
 165 D3DVECTOR * WINAPI D3DRMVectorSubtract(D3DVECTOR *d, D3DVECTOR *s1, D3DVECTOR *s2)
 166 {
 167     D3DVECTOR temp;
 168
 169     temp.u1.x=s1->u1.x - s2->u1.x;
 170     temp.u2.y=s1->u2.y - s2->u2.y;
 171     temp.u3.z=s1->u3.z - s2->u3.z;
 172
 173     *d = temp;
 174     return d;
 175 }
 176
 177 /* Cross Product of Two Vectors */
 178 D3DVECTOR * WINAPI D3DRMVectorCrossProduct(D3DVECTOR *d, D3DVECTOR *s1, D3DVECTOR *s2)
 179 {
 180     D3DVECTOR temp;
 181
 182     temp.u1.x=s1->u2.y * s2->u3.z - s1->u3.z * s2->u2.y;
 183     temp.u2.y=s1->u3.z * s2->u1.x - s1->u1.x * s2->u3.z;
 184     temp.u3.z=s1->u1.x * s2->u2.y - s1->u2.y * s2->u1.x;
 185
 186     *d = temp;
 187     return d;
 188 }
 189
 190 /* Dot Product of Two vectors */
 191 D3DVALUE WINAPI D3DRMVectorDotProduct(D3DVECTOR *s1, D3DVECTOR *s2)
 192 {
 193     D3DVALUE dot_product;
 194     dot_product=s1->u1.x * s2->u1.x + s1->u2.y * s2->u2.y + s1->u3.z * s2->u3.z;
 195     return dot_product;
 196 }
 197
 198 /* Norm of a vector */
 199 D3DVALUE WINAPI D3DRMVectorModulus(D3DVECTOR *v)
 200 {
 201     D3DVALUE result;
 202     result=sqrt(v->u1.x * v->u1.x + v->u2.y * v->u2.y + v->u3.z * v->u3.z);
 203     return result;
 204 }
 205
 206 /* Normalize a vector.  Returns (1,0,0) if INPUT is the NULL vector. */
 207 D3DVECTOR * WINAPI D3DRMVectorNormalize(D3DVECTOR *u)
 208 {
 209     D3DVALUE modulus = D3DRMVectorModulus(u);
 210     if(modulus)
 211     {
 212         D3DRMVectorScale(u,u,1.0/modulus);
 213     }
 214     else
 215     {
 216         u->u1.x=1.0;
 217         u->u2.y=0.0;
 218         u->u3.z=0.0;
 219     }
 220     return u;
 221 }
 222
 223 /* Returns a random unit vector */
 224 D3DVECTOR * WINAPI D3DRMVectorRandom(D3DVECTOR *d)
 225 {
 226     d->u1.x = rand();
 227     d->u2.y = rand();
 228     d->u3.z = rand();
 229     D3DRMVectorNormalize(d);
 230     return d;
 231 }
 232
 233 /* Reflection of a vector on a surface */
 234 D3DVECTOR * WINAPI D3DRMVectorReflect(D3DVECTOR *r, D3DVECTOR *ray, D3DVECTOR *norm)
 235 {
 236     D3DVECTOR sca, temp;
 237     D3DRMVectorSubtract(&temp, D3DRMVectorScale(&sca, norm, 2.0*D3DRMVectorDotProduct(ray,norm)), ray);
 238
 239     *r = temp;
 240     return r;
 241 }
 242
 243 /* Rotation of a vector */
 244 D3DVECTOR * WINAPI D3DRMVectorRotate(D3DVECTOR *r, D3DVECTOR *v, D3DVECTOR *axis, D3DVALUE theta)
 245 {
 246     D3DRMQUATERNION quaternion1, quaternion2, quaternion3;
 247     D3DVECTOR norm;
 248
 249     quaternion1.s = cos(theta * 0.5f);
 250     quaternion2.s = cos(theta * 0.5f);
 251     norm = *D3DRMVectorNormalize(axis);
 252     D3DRMVectorScale(&quaternion1.v, &norm, sin(theta * 0.5f));
 253     D3DRMVectorScale(&quaternion2.v, &norm, -sin(theta * 0.5f));
 254     quaternion3.s = 0.0;
 255     quaternion3.v = *v;
 256     D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion3);
 257     D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion2);
 258
 259     *r = *D3DRMVectorNormalize(&quaternion1.v);
 260     return r;
 261 }
 262
 263 /* Scale a vector */
 264 D3DVECTOR * WINAPI D3DRMVectorScale(D3DVECTOR *d, D3DVECTOR *s, D3DVALUE factor)
 265 {
 266     D3DVECTOR temp;
 267
 268     temp.u1.x=factor * s->u1.x;
 269     temp.u2.y=factor * s->u2.y;
 270     temp.u3.z=factor * s->u3.z;
 271
 272     *d = temp;
 273     return d;
 274 }