math.c 7.5 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
/*
 * Copyright 2007 David Adam
 * Copyright 2007 Vijay Kiran Kamuju
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */

20 21
#define NONAMELESSUNION

22
#include <math.h>
23
#include <stdarg.h>
24 25 26 27 28
#include "windef.h"
#include "winbase.h"
#include "wingdi.h"
#include "d3drmdef.h"

29 30 31 32 33
/* Create a RGB color from its components */
D3DCOLOR WINAPI D3DRMCreateColorRGB(D3DVALUE red, D3DVALUE green, D3DVALUE blue)
{
    return (D3DRMCreateColorRGBA(red, green, blue, 255.0));
}
34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52
/* Create a RGBA color from its components */
D3DCOLOR WINAPI D3DRMCreateColorRGBA(D3DVALUE red, D3DVALUE green, D3DVALUE blue, D3DVALUE alpha)
{
    int Red, Green, Blue, Alpha;
    Red=floor(red*255);
    Green=floor(green*255);
    Blue=floor(blue*255);
    Alpha=floor(alpha*255);
    if (red < 0) Red=0;
    if (red > 1) Red=255;
    if (green < 0) Green=0;
    if (green > 1) Green=255;
    if (blue < 0) Blue=0;
    if (blue > 1) Blue=255;
    if (alpha < 0) Alpha=0;
    if (alpha > 1) Alpha=255;
    return (RGBA_MAKE(Red, Green, Blue, Alpha));
}

53 54 55 56 57 58
/* Determine the alpha part of a color */
D3DVALUE WINAPI D3DRMColorGetAlpha(D3DCOLOR color)
{
    return (RGBA_GETALPHA(color)/255.0);
}

59 60 61 62 63 64
/* Determine the blue part of a color */
D3DVALUE WINAPI D3DRMColorGetBlue(D3DCOLOR color)
{
    return (RGBA_GETBLUE(color)/255.0);
}

65 66 67 68 69 70
/* Determine the green part of a color */
D3DVALUE WINAPI D3DRMColorGetGreen(D3DCOLOR color)
{
    return (RGBA_GETGREEN(color)/255.0);
}

71 72 73 74 75 76
/* Determine the red part of a color */
D3DVALUE WINAPI D3DRMColorGetRed(D3DCOLOR color)
{
    return (RGBA_GETRED(color)/255.0);
}

77 78 79
/* Product of 2 quaternions */
LPD3DRMQUATERNION WINAPI D3DRMQuaternionMultiply(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b)
{
80
    D3DRMQUATERNION temp;
81
    D3DVECTOR cross_product;
82

83
    D3DRMVectorCrossProduct(&cross_product, &a->v, &b->v);
84 85 86 87 88 89
    temp.s = a->s * b->s - D3DRMVectorDotProduct(&a->v, &b->v);
    temp.v.u1.x = a->s * b->v.u1.x + b->s * a->v.u1.x + cross_product.u1.x;
    temp.v.u2.y = a->s * b->v.u2.y + b->s * a->v.u2.y + cross_product.u2.y;
    temp.v.u3.z = a->s * b->v.u3.z + b->s * a->v.u3.z + cross_product.u3.z;

    *q = temp;
90 91 92
    return q;
}

93 94 95 96 97
/* Matrix for the Rotation that a unit quaternion represents */
void WINAPI D3DRMMatrixFromQuaternion(D3DRMMATRIX4D m, LPD3DRMQUATERNION q)
{
    D3DVALUE w,x,y,z;
    w = q->s;
98 99 100
    x = q->v.u1.x;
    y = q->v.u2.y;
    z = q->v.u3.z;
101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118
    m[0][0] = 1.0-2.0*(y*y+z*z);
    m[1][1] = 1.0-2.0*(x*x+z*z);
    m[2][2] = 1.0-2.0*(x*x+y*y);
    m[1][0] = 2.0*(x*y+z*w);
    m[0][1] = 2.0*(x*y-z*w);
    m[2][0] = 2.0*(x*z-y*w);
    m[0][2] = 2.0*(x*z+y*w);
    m[2][1] = 2.0*(y*z+x*w);
    m[1][2] = 2.0*(y*z-x*w);
    m[3][0] = 0.0;
    m[3][1] = 0.0;
    m[3][2] = 0.0;
    m[0][3] = 0.0;
    m[1][3] = 0.0;
    m[2][3] = 0.0;
    m[3][3] = 1.0;
}

119 120 121 122 123 124 125 126
/* Return a unit quaternion that represents a rotation of an angle around an axis */
LPD3DRMQUATERNION WINAPI D3DRMQuaternionFromRotation(LPD3DRMQUATERNION q, LPD3DVECTOR v, D3DVALUE theta)
{
    q->s = cos(theta/2.0);
    D3DRMVectorScale(&q->v, D3DRMVectorNormalize(v), sin(theta/2.0));
    return q;
}

127 128 129
/* Interpolation between two quaternions */
LPD3DRMQUATERNION WINAPI D3DRMQuaternionSlerp(LPD3DRMQUATERNION q, LPD3DRMQUATERNION a, LPD3DRMQUATERNION b, D3DVALUE alpha)
{
130
    D3DVALUE dot, epsilon, temp, theta, u;
131

132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147
    dot = a->s * b->s + D3DRMVectorDotProduct(&a->v, &b->v);
    epsilon = 1.0f;
    temp = 1.0f - alpha;
    u = alpha;
    if (dot < 0.0)
    {
     epsilon = -1.0;
     dot = -dot;
    }
    if( 1.0f - dot > 0.001f )
    {
        theta = acos(dot);
        temp  = sin(theta * temp) / sin(theta);
        u = sin(theta * alpha) / sin(theta);
    }
    q->s = temp * a->s + epsilon * u * b->s;
148 149
    D3DRMVectorAdd(&q->v, D3DRMVectorScale(&a->v, &a->v, temp),
                   D3DRMVectorScale(&b->v, &b->v, epsilon * u));
150 151 152
    return q;
}

153 154 155
/* Add Two Vectors */
LPD3DVECTOR WINAPI D3DRMVectorAdd(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
{
156 157 158 159 160 161 162
    D3DVECTOR temp;

    temp.u1.x=s1->u1.x + s2->u1.x;
    temp.u2.y=s1->u2.y + s2->u2.y;
    temp.u3.z=s1->u3.z + s2->u3.z;

    *d = temp;
163 164
    return d;
}
165 166 167 168

/* Subtract Two Vectors */
LPD3DVECTOR WINAPI D3DRMVectorSubtract(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
{
169 170 171 172 173 174 175
    D3DVECTOR temp;

    temp.u1.x=s1->u1.x - s2->u1.x;
    temp.u2.y=s1->u2.y - s2->u2.y;
    temp.u3.z=s1->u3.z - s2->u3.z;

    *d = temp;
176 177
    return d;
}
178 179 180 181

/* Cross Product of Two Vectors */
LPD3DVECTOR WINAPI D3DRMVectorCrossProduct(LPD3DVECTOR d, LPD3DVECTOR s1, LPD3DVECTOR s2)
{
182 183 184 185 186 187 188
    D3DVECTOR temp;

    temp.u1.x=s1->u2.y * s2->u3.z - s1->u3.z * s2->u2.y;
    temp.u2.y=s1->u3.z * s2->u1.x - s1->u1.x * s2->u3.z;
    temp.u3.z=s1->u1.x * s2->u2.y - s1->u2.y * s2->u1.x;

    *d = temp;
189 190
    return d;
}
191 192 193 194 195

/* Dot Product of Two vectors */
D3DVALUE WINAPI D3DRMVectorDotProduct(LPD3DVECTOR s1, LPD3DVECTOR s2)
{
    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 198
    return dot_product;
}
199 200 201 202 203

/* Norm of a vector */
D3DVALUE WINAPI D3DRMVectorModulus(LPD3DVECTOR v)
{
    D3DVALUE result;
204
    result=sqrt(v->u1.x * v->u1.x + v->u2.y * v->u2.y + v->u3.z * v->u3.z);
205 206
    return result;
}
207

208 209 210 211 212 213 214 215 216 217
/* Normalize a vector.  Returns (1,0,0) if INPUT is the NULL vector. */
LPD3DVECTOR WINAPI D3DRMVectorNormalize(LPD3DVECTOR u)
{
    D3DVALUE modulus = D3DRMVectorModulus(u);
    if(modulus)
    {
        D3DRMVectorScale(u,u,1.0/modulus);
    }
    else
    {
218 219 220
        u->u1.x=1.0;
        u->u2.y=0.0;
        u->u3.z=0.0;
221 222 223 224
    }
    return u;
}

225 226 227
/* Returns a random unit vector */
LPD3DVECTOR WINAPI D3DRMVectorRandom(LPD3DVECTOR d)
{
228 229 230
    d->u1.x = rand();
    d->u2.y = rand();
    d->u3.z = rand();
231 232 233 234
    D3DRMVectorNormalize(d);
    return d;
}

235 236 237
/* Reflection of a vector on a surface */
LPD3DVECTOR WINAPI D3DRMVectorReflect(LPD3DVECTOR r, LPD3DVECTOR ray, LPD3DVECTOR norm)
{
238 239 240 241
    D3DVECTOR sca, temp;
    D3DRMVectorSubtract(&temp, D3DRMVectorScale(&sca, norm, 2.0*D3DRMVectorDotProduct(ray,norm)), ray);

    *r = temp;
242 243 244
    return r;
}

245 246 247
/* Rotation of a vector */
LPD3DVECTOR WINAPI D3DRMVectorRotate(LPD3DVECTOR r, LPD3DVECTOR v, LPD3DVECTOR axis, D3DVALUE theta)
{
248 249 250 251 252 253 254 255
    D3DRMQUATERNION quaternion1, quaternion2, quaternion3;
    D3DVECTOR norm;

    quaternion1.s = cos(theta * 0.5f);
    quaternion2.s = cos(theta * 0.5f);
    norm = *D3DRMVectorNormalize(axis);
    D3DRMVectorScale(&quaternion1.v, &norm, sin(theta * 0.5f));
    D3DRMVectorScale(&quaternion2.v, &norm, -sin(theta * 0.5f));
256 257
    quaternion3.s = 0.0;
    quaternion3.v = *v;
258 259 260 261
    D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion3);
    D3DRMQuaternionMultiply(&quaternion1, &quaternion1, &quaternion2);

    *r = *D3DRMVectorNormalize(&quaternion1.v);
262 263 264
    return r;
}

265 266 267
/* Scale a vector */
LPD3DVECTOR WINAPI D3DRMVectorScale(LPD3DVECTOR d, LPD3DVECTOR s, D3DVALUE factor)
{
268 269 270 271 272 273 274
    D3DVECTOR temp;

    temp.u1.x=factor * s->u1.x;
    temp.u2.y=factor * s->u2.y;
    temp.u3.z=factor * s->u3.z;

    *d = temp;
275 276
    return d;
}