The Ecere SDK - sdk/blob - ecere/src/gfx/3D/Vector3D.ec

   1 namespace gfx3D;
   2
   3 import "Display"
   4
   5 public struct Vector3D
   6 {
   7    double x, y, z;
   8
   9    void Add(Vector3D vector1, Vector3D vector2)
  10    {
  11       x = vector1.x + vector2.x;
  12       y = vector1.y + vector2.y;
  13       z = vector1.z + vector2.z;
  14    }
  15
  16    void Subtract(Vector3D vector1, Vector3D vector2)
  17    {
  18       x = vector1.x - vector2.x;
  19       y = vector1.y - vector2.y;
  20       z = vector1.z - vector2.z;
  21    }
  22
  23    void Scale(Vector3D vector1, double s)
  24    {
  25       x = vector1.x * s;
  26       y = vector1.y * s;
  27       z = vector1.z * s;
  28    }
  29
  30    double DotProduct(Vector3D vector2)
  31    {
  32       return x * vector2.x + y * vector2.y + z * vector2.z;
  33    }
  34
  35    double DotProductf(Vector3Df vector2)
  36    {
  37       return x * vector2.x + y * vector2.y + z * vector2.z;
  38    }
  39
  40    void CrossProduct(Vector3D vector1, Vector3D vector2)
  41    {
  42       x = vector1.y * vector2.z - vector1.z * vector2.y;
  43       y = vector1.z * vector2.x - vector1.x * vector2.z;
  44       z = vector1.x * vector2.y - vector1.y * vector2.x;
  45    }
  46
  47    void Normalize(Vector3D source)
  48    {
  49       double m = source.length;
  50       if(m)
  51       {
  52          x = source.x/m;
  53          y = source.y/m;
  54          z = source.z/m;
  55       }
  56       else
  57          x = y = z = 0;
  58    }
  59
  60    void MultMatrix(Vector3D source, Matrix matrix)
  61    {
  62        x = source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0];
  63        y = source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1];
  64        z = source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2];
  65    }
  66
  67    void MultMatrixf(Vector3Df source, Matrix matrix)
  68    {
  69        x = source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0];
  70        y = source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1];
  71        z = source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2];
  72    }
  73
  74    void DivideMatrix(Vector3D source, Matrix matrix)
  75    {
  76       /*
  77       solve(
  78       {
  79          vectorX=sourceX*m00+sourceY*m10+sourceZ*m20+m30,
  80          vectorY=sourceZ*m01+sourceY*m11+sourceZ*m21+m31,
  81          vectorZ=sourceX*m02+sourceY*m12+sourceZ*m22+m32
  82       }, { sourceX, sourceY, sourceZ });
  83       */
  84
  85       double var1 =
  86          matrix.m[2][0] * matrix.m[0][2] * matrix.m[1][1]
  87        - matrix.m[0][2] * matrix.m[2][1] * matrix.m[1][0]
  88        - matrix.m[2][2] * matrix.m[0][0] * matrix.m[1][1]
  89        - matrix.m[0][2] * matrix.m[0][1] * matrix.m[1][0]
  90        + matrix.m[2][1] * matrix.m[0][0] * matrix.m[1][2]
  91        + matrix.m[0][1] * matrix.m[0][0] * matrix.m[1][2];
  92
  93       x = (
  94          - matrix.m[2][2] * source.x * matrix.m[1][1]
  95          + matrix.m[2][2] * matrix.m[1][0] * source.y
  96          - matrix.m[2][2] * matrix.m[1][0] * matrix.m[3][1]
  97          + matrix.m[2][2] * matrix.m[3][0] * matrix.m[1][1]
  98          - matrix.m[2][0] * matrix.m[3][2] * matrix.m[1][1]
  99          + source.x * matrix.m[2][1] * matrix.m[1][2]
 100          + source.x * matrix.m[0][1] * matrix.m[1][2]
 101          - matrix.m[1][0] * matrix.m[0][1] * source.z
 102          + matrix.m[1][0] * matrix.m[2][1] * matrix.m[3][2]
 103          + matrix.m[1][0] * matrix.m[0][1] * matrix.m[3][2]
 104          - matrix.m[1][0] * matrix.m[2][1] * source.z
 105          - matrix.m[3][0] * matrix.m[2][1] * matrix.m[1][2]
 106          - matrix.m[2][0] * matrix.m[1][2] * source.y
 107          + matrix.m[2][0] * matrix.m[1][2] * matrix.m[3][1]
 108          + matrix.m[2][0] * source.z * matrix.m[1][1]
 109          - matrix.m[3][0] * matrix.m[0][1] * matrix.m[1][2]
 110          ) / var1;
 111
 112       y = - (
 113          - matrix.m[2][0] * matrix.m[0][2] * source.y
 114          + matrix.m[2][1] * matrix.m[0][0] * matrix.m[3][2]
 115          + matrix.m[2][0] * matrix.m[0][2] * matrix.m[3][1]
 116          + matrix.m[0][1] * matrix.m[0][0] * matrix.m[3][2]
 117          - matrix.m[2][1] * matrix.m[0][0] * source.z
 118          - matrix.m[0][2] * matrix.m[2][1] * matrix.m[3][0]
 119          + matrix.m[0][2] * matrix.m[0][1] * source.x
 120          - matrix.m[0][2] * matrix.m[0][1] * matrix.m[3][0]
 121          + matrix.m[0][2] * matrix.m[2][1] * source.x
 122          + matrix.m[2][2] * matrix.m[0][0] * source.y
 123          - matrix.m[0][1] * matrix.m[0][0] * source.z
 124          - matrix.m[2][2] * matrix.m[0][0] * matrix.m[3][1]
 125          ) / var1;
 126
 127       z = (
 128          source.x * matrix.m[0][2] * matrix.m[1][1]
 129          + matrix.m[0][0] * matrix.m[3][2] * matrix.m[1][1]
 130          + matrix.m[0][0] * matrix.m[1][2] * source.y
 131          - matrix.m[0][0] * matrix.m[1][2] * matrix.m[3][1]
 132          - matrix.m[0][0] * source.z * matrix.m[1][1]
 133          - matrix.m[1][0] * matrix.m[0][2] * source.y
 134          + matrix.m[1][0] * matrix.m[0][2] * matrix.m[3][1]
 135          - matrix.m[3][0] * matrix.m[0][2] * matrix.m[1][1]
 136          ) / var1;
 137    }
 138
 139    property double length { get { return (double)sqrt(x * x + y * y + z * z); } };
 140    property double lengthApprox
 141    {
 142       get
 143       {
 144          double ix = Abs(x), iy = Abs(y), iz = Abs(z);
 145          double tmp;
 146          if(ix < iy) { tmp = ix; ix = iy; iy = tmp; }
 147          if(ix < iz) { tmp = ix; ix = iz; iz = tmp; }
 148          if(iz > iy) { iz = iy; }
 149          return ix + (iz/2);
 150       }
 151    }
 152 };
 153
 154 public /*inline */float FastInvSqrt(float x)
 155 {
 156   union { float f; uint u; } i;
 157   float halfX = x / 2;
 158   i.f = x;
 159   i.u = 0x5f375a86 - (i.u >> 1);
 160   x = i.f;
 161   return x * (1.5f - (halfX * x * x));
 162 }
 163
 164 public struct Vector3Df
 165 {
 166    float x, y, z;
 167
 168    void Add(Vector3Df vector1, Vector3Df vector2)
 169    {
 170       x = vector1.x + vector2.x;
 171       y = vector1.y + vector2.y;
 172       z = vector1.z + vector2.z;
 173    }
 174
 175    void Subtract(Vector3Df vector1, Vector3Df vector2)
 176    {
 177       x = vector1.x - vector2.x;
 178       y = vector1.y - vector2.y;
 179       z = vector1.z - vector2.z;
 180    }
 181
 182    void Scale(Vector3Df vector1, float s)
 183    {
 184       x = vector1.x * s;
 185       y = vector1.y * s;
 186       z = vector1.z * s;
 187    }
 188
 189    double DotProduct(Vector3Df vector2)
 190    {
 191       return (double)x * (double)vector2.x + (double)y * (double)vector2.y + (double)z * (double)vector2.z;
 192    }
 193
 194    void CrossProduct(Vector3Df vector1, Vector3Df vector2)
 195    {
 196       x = vector1.y * vector2.z - vector1.z * vector2.y;
 197       y = vector1.z * vector2.x - vector1.x * vector2.z;
 198       z = vector1.x * vector2.y - vector1.y * vector2.x;
 199    }
 200
 201    void Normalize(Vector3Df source)
 202    {
 203       /*
 204       float m = source.length;
 205       if(m)
 206       {
 207          x = source.x/m;
 208          y = source.y/m;
 209          z = source.z/m;
 210       }
 211       else
 212          x = y = z = 0;
 213       */
 214       float i = FastInvSqrt(source.x * source.x + source.y * source.y + source.z * source.z);
 215       x = source.x * i;
 216       y = source.y * i;
 217       z = source.z * i;
 218    }
 219
 220    void MultMatrix(Vector3Df source, Matrix matrix)
 221    {
 222        x = (float)(source.x * matrix.m[0][0] + source.y * matrix.m[1][0] + source.z * matrix.m[2][0] + matrix.m[3][0]);
 223        y = (float)(source.x * matrix.m[0][1] + source.y * matrix.m[1][1] + source.z * matrix.m[2][1] + matrix.m[3][1]);
 224        z = (float)(source.x * matrix.m[0][2] + source.y * matrix.m[1][2] + source.z * matrix.m[2][2] + matrix.m[3][2]);
 225    }
 226
 227    void DivideMatrix(Vector3Df source, Matrix matrix)
 228    {
 229       /*
 230       solve(
 231       {
 232          vectorX=sourceX*m00+sourceY*m10+sourceZ*m20+m30,
 233          vectorY=sourceZ*m01+sourceY*m11+sourceZ*m21+m31,
 234          vectorZ=sourceX*m02+sourceY*m12+sourceZ*m22+m32
 235       }, { sourceX, sourceY, sourceZ });
 236       */
 237
 238       float var1 = (float)(
 239          matrix.m[2][0] * matrix.m[0][2] * matrix.m[1][1]
 240        - matrix.m[0][2] * matrix.m[2][1] * matrix.m[1][0]
 241        - matrix.m[2][2] * matrix.m[0][0] * matrix.m[1][1]
 242        - matrix.m[0][2] * matrix.m[0][1] * matrix.m[1][0]
 243        + matrix.m[2][1] * matrix.m[0][0] * matrix.m[1][2]
 244        + matrix.m[0][1] * matrix.m[0][0] * matrix.m[1][2]);
 245
 246       x = (float)(
 247          - matrix.m[2][2] * source.x * matrix.m[1][1]
 248          + matrix.m[2][2] * matrix.m[1][0] * source.y
 249          - matrix.m[2][2] * matrix.m[1][0] * matrix.m[3][1]
 250          + matrix.m[2][2] * matrix.m[3][0] * matrix.m[1][1]
 251          - matrix.m[2][0] * matrix.m[3][2] * matrix.m[1][1]
 252          + source.x * matrix.m[2][1] * matrix.m[1][2]
 253          + source.x * matrix.m[0][1] * matrix.m[1][2]
 254          - matrix.m[1][0] * matrix.m[0][1] * source.z
 255          + matrix.m[1][0] * matrix.m[2][1] * matrix.m[3][2]
 256          + matrix.m[1][0] * matrix.m[0][1] * matrix.m[3][2]
 257          - matrix.m[1][0] * matrix.m[2][1] * source.z
 258          - matrix.m[3][0] * matrix.m[2][1] * matrix.m[1][2]
 259          - matrix.m[2][0] * matrix.m[1][2] * source.y
 260          + matrix.m[2][0] * matrix.m[1][2] * matrix.m[3][1]
 261          + matrix.m[2][0] * source.z * matrix.m[1][1]
 262          - matrix.m[3][0] * matrix.m[0][1] * matrix.m[1][2]
 263          ) / var1;
 264
 265       y = - (float)(
 266          - matrix.m[2][0] * matrix.m[0][2] * source.y
 267          + matrix.m[2][1] * matrix.m[0][0] * matrix.m[3][2]
 268          + matrix.m[2][0] * matrix.m[0][2] * matrix.m[3][1]
 269          + matrix.m[0][1] * matrix.m[0][0] * matrix.m[3][2]
 270          - matrix.m[2][1] * matrix.m[0][0] * source.z
 271          - matrix.m[0][2] * matrix.m[2][1] * matrix.m[3][0]
 272          + matrix.m[0][2] * matrix.m[0][1] * source.x
 273          - matrix.m[0][2] * matrix.m[0][1] * matrix.m[3][0]
 274          + matrix.m[0][2] * matrix.m[2][1] * source.x
 275          + matrix.m[2][2] * matrix.m[0][0] * source.y
 276          - matrix.m[0][1] * matrix.m[0][0] * source.z
 277          - matrix.m[2][2] * matrix.m[0][0] * matrix.m[3][1]
 278          ) / var1;
 279
 280       z = (float)(
 281          source.x * matrix.m[0][2] * matrix.m[1][1]
 282          + matrix.m[0][0] * matrix.m[3][2] * matrix.m[1][1]
 283          + matrix.m[0][0] * matrix.m[1][2] * source.y
 284          - matrix.m[0][0] * matrix.m[1][2] * matrix.m[3][1]
 285          - matrix.m[0][0] * source.z * matrix.m[1][1]
 286          - matrix.m[1][0] * matrix.m[0][2] * source.y
 287          + matrix.m[1][0] * matrix.m[0][2] * matrix.m[3][1]
 288          - matrix.m[3][0] * matrix.m[0][2] * matrix.m[1][1]
 289          ) / var1;
 290    }
 291
 292    property float length { get { return (float)sqrt(x * x + y * y + z * z); } };
 293    property float lengthApprox
 294    {
 295       get
 296       {
 297          float ix = Abs(x), iy = Abs(y), iz = Abs(z);
 298          float tmp;
 299          if(ix < iy) { tmp = ix; ix = iy; iy = tmp; }
 300          if(ix < iz) { tmp = ix; ix = iz; iz = tmp; }
 301          if(iz > iy) { iz = iy; }
 302          return ix + (iz/2);
 303       }
 304    }
 305    /*
 306    property Vector3D
 307    {
 308       get
 309       {
 310          value.x = (double) x;
 311          value.y = (double) y;
 312          value.z = (double) z;
 313       }
 314       set
 315       {
 316          x = (float) value.x;
 317          y = (float) value.y;
 318          z = (float) value.z;
 319       }
 320    }
 321    */
 322 };