Rev 784 | Go to most recent revision | Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 782 | chris | 1 | using System; |
| 2 | using System.Collections.Generic; |
||
| 3 | using System.Linq; |
||
| 4 | using System.Text; |
||
| 5 | using System.Threading.Tasks; |
||
| 6 | |||
| 7 | namespace BauzoidNET.math |
||
| 8 | { |
||
| 9 | class Matrix3 |
||
| 10 | { |
||
| 11 | public static const int M11 = 0; |
||
| 12 | public static const int M21 = 1; |
||
| 13 | public static const int M31 = 2; |
||
| 14 | public static const int M12 = 3; |
||
| 15 | public static const int M22 = 4; |
||
| 16 | public static const int M32 = 5; |
||
| 17 | public static const int M13 = 6; |
||
| 18 | public static const int M23 = 7; |
||
| 19 | public static const int M33 = 8; |
||
| 20 | |||
| 21 | public static const int MATRIX_SIZE = 3; |
||
| 22 | |||
| 23 | public float[] values = new float[MATRIX_SIZE*MATRIX_SIZE]; |
||
| 24 | |||
| 25 | public Matrix3() |
||
| 26 | { |
||
| 27 | } |
||
| 28 | |||
| 29 | public float get(int row, int col) |
||
| 30 | { |
||
| 31 | return values[col*MATRIX_SIZE + row]; |
||
| 32 | } |
||
| 33 | |||
| 34 | public void set(int row, int col, float value) |
||
| 35 | { |
||
| 36 | values[col*MATRIX_SIZE+row] = value; |
||
| 37 | } |
||
| 38 | |||
| 39 | public void zero() |
||
| 40 | { |
||
| 41 | for (int i = 0; i < values.Length; i++) |
||
| 42 | values[i] = 0.0f; |
||
| 43 | } |
||
| 44 | |||
| 45 | public void identity() |
||
| 46 | { |
||
| 47 | zero(); |
||
| 48 | values[M11] = 1.0f; values[M22] = 1.0f; values[M33] = 1.0f; |
||
| 49 | } |
||
| 50 | |||
| 51 | public bool equals(Matrix3 other) |
||
| 52 | { |
||
| 53 | for (int i = 0; i < values.Length; i++) |
||
| 54 | { |
||
| 55 | if (this.values[i] != other.values[i]) |
||
| 56 | return false; |
||
| 57 | } |
||
| 58 | return true; |
||
| 59 | } |
||
| 60 | |||
| 61 | public bool isSingular() |
||
| 62 | { |
||
| 63 | float determinant = getDeterminant(); |
||
| 64 | return ((float)Math.Abs(determinant) < MathUtil.EPSILON); |
||
| 65 | } |
||
| 66 | |||
| 67 | public Matrix3 getTranspose() |
||
| 68 | { |
||
| 69 | Matrix3 result = new Matrix3(); |
||
| 70 | result.values[M11] = values[M11]; |
||
| 71 | result.values[M12] = values[M21]; |
||
| 72 | result.values[M13] = values[M31]; |
||
| 73 | result.values[M21] = values[M12]; |
||
| 74 | result.values[M22] = values[M22]; |
||
| 75 | result.values[M23] = values[M32]; |
||
| 76 | result.values[M31] = values[M13]; |
||
| 77 | result.values[M32] = values[M23]; |
||
| 78 | result.values[M33] = values[M33]; |
||
| 79 | return result; |
||
| 80 | } |
||
| 81 | |||
| 82 | public float getDeterminant() |
||
| 83 | { |
||
| 84 | float result = |
||
| 85 | values[M11] * (values[M22] * values[M33] - values[M23] * values[M32]) - |
||
| 86 | values[M21] * (values[M12] * values[M33] - values[M13] * values[M32]) + |
||
| 87 | values[M31] * (values[M12] * values[M23] - values[M13] * values[M22]); |
||
| 88 | return result; |
||
| 89 | } |
||
| 90 | |||
| 91 | public Matrix3 getInverse() |
||
| 92 | { |
||
| 93 | Matrix3 result = new Matrix3(); |
||
| 94 | |||
| 95 | float determinant = getDeterminant(); |
||
| 96 | if (determinant == 0.0f) |
||
| 97 | { |
||
| 98 | // singular matrix cannot be inverted |
||
| 99 | return this; |
||
| 100 | } |
||
| 101 | |||
| 102 | result.values[M11] = values[M22] * values[M33] - values[M32] * values[M23]; |
||
| 103 | result.values[M21] = -(values[M21] * values[M33] - values[M23] * values[M31]); |
||
| 104 | result.values[M31] = values[M21] * values[M32] - values[M22] * values[M31]; |
||
| 105 | result.values[M12] = -(values[M12] * values[M33] - values[M32] * values[M13]); |
||
| 106 | result.values[M22] = values[M11] * values[M33] - values[M13] * values[M31]; |
||
| 107 | result.values[M32] = -(values[M11] * values[M32] - values[M12] * values[M31]); |
||
| 108 | result.values[M13] = values[M12] * values[M23] - values[M13] * values[M22]; |
||
| 109 | result.values[M23] = -(values[M11] * values[M23] - values[M13] * values[M21]); |
||
| 110 | result.values[M33] = values[M11] * values[M22] - values[M21] * values[M12]; |
||
| 111 | |||
| 112 | float multiply = 1.0f / determinant; |
||
| 113 | result.values[M11] *= multiply; |
||
| 114 | result.values[M21] *= multiply; |
||
| 115 | result.values[M31] *= multiply; |
||
| 116 | result.values[M12] *= multiply; |
||
| 117 | result.values[M22] *= multiply; |
||
| 118 | result.values[M32] *= multiply; |
||
| 119 | result.values[M13] *= multiply; |
||
| 120 | result.values[M23] *= multiply; |
||
| 121 | result.values[M33] *= multiply; |
||
| 122 | |||
| 123 | return result; |
||
| 124 | } |
||
| 125 | |||
| 126 | public void copyTo(Matrix3 target) |
||
| 127 | { |
||
| 128 | target.values[M11] = values[M11]; target.values[M12] = values[M12]; target.values[M13] = values[M13]; |
||
| 129 | target.values[M21] = values[M21]; target.values[M22] = values[M22]; target.values[M23] = values[M23]; |
||
| 130 | target.values[M31] = values[M31]; target.values[M32] = values[M32]; target.values[M33] = values[M33]; |
||
| 131 | } |
||
| 132 | |||
| 133 | public void copyFrom(Matrix3 source) |
||
| 134 | { |
||
| 135 | values[M11] = source.values[M11]; values[M12] = source.values[M12]; values[M13] = source.values[M13]; |
||
| 136 | values[M21] = source.values[M21]; values[M22] = source.values[M22]; values[M23] = source.values[M23]; |
||
| 137 | values[M31] = source.values[M31]; values[M32] = source.values[M32]; values[M33] = source.values[M33]; |
||
| 138 | } |
||
| 139 | |||
| 140 | public void postMultiply(Matrix3 v) |
||
| 141 | { |
||
| 142 | multiply(this, this, v); |
||
| 143 | } |
||
| 144 | |||
| 145 | public void preMultiply(Matrix3 v) |
||
| 146 | { |
||
| 147 | multiply(this, v, this); |
||
| 148 | } |
||
| 149 | |||
| 150 | static public void multiply(Matrix3 result, Matrix3 a, Matrix3 b) |
||
| 151 | { |
||
| 152 | float result11 = a.values[M11] * b.values[M11] + a.values[M21] * b.values[M12] + a.values[M31] * b.values[M13]; |
||
| 153 | float result21 = a.values[M11] * b.values[M21] + a.values[M21] * b.values[M22] + a.values[M31] * b.values[M23]; |
||
| 154 | float result31 = a.values[M11] * b.values[M31] + a.values[M21] * b.values[M32] + a.values[M31] * b.values[M33]; |
||
| 155 | float result12 = a.values[M12] * b.values[M11] + a.values[M22] * b.values[M12] + a.values[M32] * b.values[M13]; |
||
| 156 | float result22 = a.values[M12] * b.values[M21] + a.values[M22] * b.values[M22] + a.values[M32] * b.values[M23]; |
||
| 157 | float result32 = a.values[M12] * b.values[M31] + a.values[M22] * b.values[M32] + a.values[M32] * b.values[M33]; |
||
| 158 | float result13 = a.values[M13] * b.values[M11] + a.values[M23] * b.values[M12] + a.values[M33] * b.values[M13]; |
||
| 159 | float result23 = a.values[M13] * b.values[M21] + a.values[M23] * b.values[M22] + a.values[M33] * b.values[M23]; |
||
| 160 | float result33 = a.values[M13] * b.values[M31] + a.values[M23] * b.values[M32] + a.values[M33] * b.values[M33]; |
||
| 161 | result.values[M11] = result11; |
||
| 162 | result.values[M21] = result21; |
||
| 163 | result.values[M31] = result31; |
||
| 164 | result.values[M12] = result12; |
||
| 165 | result.values[M22] = result22; |
||
| 166 | result.values[M32] = result32; |
||
| 167 | result.values[M13] = result13; |
||
| 168 | result.values[M23] = result23; |
||
| 169 | result.values[M33] = result33; |
||
| 170 | } |
||
| 171 | |||
| 172 | static public Matrix3 multiply(Matrix3 a, Matrix3 b) |
||
| 173 | { |
||
| 174 | Matrix3 result = new Matrix3(); |
||
| 175 | multiply(result, a, b); |
||
| 176 | return result; |
||
| 177 | } |
||
| 178 | |||
| 179 | static public Matrix3 createIdentity() |
||
| 180 | { |
||
| 181 | return createScale(1.0f, 1.0f, 1.0f); |
||
| 182 | } |
||
| 183 | |||
| 184 | static public Matrix3 createScale(float sx, float sy, float sz) |
||
| 185 | { |
||
| 186 | Matrix3 result = new Matrix3(); |
||
| 187 | |||
| 188 | result.values[M11] = sx; |
||
| 189 | result.values[M22] = sy; |
||
| 190 | result.values[M33] = sz; |
||
| 191 | |||
| 192 | return result; |
||
| 193 | } |
||
| 194 | |||
| 195 | static public Matrix3 createScale(Vector3 scale) |
||
| 196 | { |
||
| 197 | return createScale(scale.x, scale.y, scale.z); |
||
| 198 | } |
||
| 199 | |||
| 200 | static public Matrix3 createScale(float s) |
||
| 201 | { |
||
| 202 | return createScale(s, s, s); |
||
| 203 | } |
||
| 204 | |||
| 205 | static public Matrix3 createRotationX(float degrees) |
||
| 206 | { |
||
| 207 | float degreesrad = MathUtil.degToRad(degrees); |
||
| 208 | float sinvalue = (float)Math.Sin(degreesrad); |
||
| 209 | float cosvalue = (float)Math.Cos(degreesrad); |
||
| 210 | |||
| 211 | Matrix3 matrix = createIdentity(); |
||
| 212 | matrix.values[M22] = cosvalue; |
||
| 213 | matrix.values[M32] = -sinvalue; |
||
| 214 | matrix.values[M23] = sinvalue; |
||
| 215 | matrix.values[M33] = cosvalue; |
||
| 216 | return matrix; |
||
| 217 | } |
||
| 218 | |||
| 219 | static public Matrix3 createRotationY(float degrees) |
||
| 220 | { |
||
| 221 | float degreesrad = MathUtil.degToRad(degrees); |
||
| 222 | float sinvalue = (float)Math.Sin(degreesrad); |
||
| 223 | float cosvalue = (float)Math.Cos(degreesrad); |
||
| 224 | |||
| 225 | Matrix3 matrix = createIdentity(); |
||
| 226 | matrix.values[M11] = cosvalue; |
||
| 227 | matrix.values[M31] = sinvalue; |
||
| 228 | matrix.values[M13] = -sinvalue; |
||
| 229 | matrix.values[M33] = cosvalue; |
||
| 230 | return matrix; |
||
| 231 | } |
||
| 232 | |||
| 233 | static public Matrix3 createRotationZ(float degrees) |
||
| 234 | { |
||
| 235 | float degreesrad = MathUtil.degToRad(degrees); |
||
| 236 | float sinvalue = (float)Math.Sin(degreesrad); |
||
| 237 | float cosvalue = (float)Math.Cos(degreesrad); |
||
| 238 | |||
| 239 | Matrix3 matrix = createIdentity(); |
||
| 240 | matrix.values[M11] = cosvalue; |
||
| 241 | matrix.values[M21] = -sinvalue; |
||
| 242 | matrix.values[M12] = sinvalue; |
||
| 243 | matrix.values[M22] = cosvalue; |
||
| 244 | return matrix; |
||
| 245 | } |
||
| 246 | |||
| 247 | static public Matrix3 createRotation(Vector3 axis, float degrees) |
||
| 248 | { |
||
| 249 | float radangle = MathUtil.degToRad(degrees); |
||
| 250 | float radcos = (float)Math.Cos(radangle); |
||
| 251 | float radsin = (float)Math.Sin(radangle); |
||
| 252 | |||
| 253 | Matrix3 matrix = new Matrix3(); |
||
| 254 | matrix.values[M11] = radcos + axis.x * axis.x * (1 - radcos); |
||
| 255 | matrix.values[M21] = axis.z * radsin + axis.y * axis.x * (1 - radcos); |
||
| 256 | matrix.values[M31] = -axis.y * radsin + axis.z * axis.x * (1 - radcos); |
||
| 257 | matrix.values[M12] = -axis.z * radsin + axis.x * axis.y * (1 - radcos); |
||
| 258 | matrix.values[M22] = radcos + axis.y * axis.y * (1 - radcos); |
||
| 259 | matrix.values[M32] = axis.x * radsin + axis.z * axis.y * (1 - radcos); |
||
| 260 | matrix.values[M13] = axis.y * radsin + axis.x * axis.z * (1 - radcos); |
||
| 261 | matrix.values[M23] = -axis.x * radsin + axis.y * axis.z * (1 - radcos); |
||
| 262 | matrix.values[M33] = radcos + axis.z * axis.z * (1 - radcos); |
||
| 263 | return matrix; |
||
| 264 | } |
||
| 265 | |||
| 266 | static public Matrix3 createFromVectors(Vector3 forward, Vector3 up, Vector3 right) |
||
| 267 | { |
||
| 268 | forward.normalize(); |
||
| 269 | up.normalize(); |
||
| 270 | right.normalize(); |
||
| 271 | |||
| 272 | Matrix3 result = createIdentity(); |
||
| 273 | |||
| 274 | result.values[M11] = right.x; |
||
| 275 | result.values[M12] = right.y; |
||
| 276 | result.values[M13] = right.z; |
||
| 277 | result.values[M21] = up.x; |
||
| 278 | result.values[M22] = up.y; |
||
| 279 | result.values[M23] = up.z; |
||
| 280 | result.values[M31] = -forward.x; |
||
| 281 | result.values[M32] = -forward.y; |
||
| 282 | result.values[M33] = -forward.z; |
||
| 283 | |||
| 284 | return result; |
||
| 285 | } |
||
| 286 | |||
| 287 | public Vector3 transform(Vector3 vector) |
||
| 288 | { |
||
| 289 | return new Vector3( |
||
| 290 | values[M11] * vector.x + values[M21] * vector.y + values[M31] * vector.z, |
||
| 291 | values[M12] * vector.x + values[M22] * vector.y + values[M32] * vector.z, |
||
| 292 | values[M13] * vector.x + values[M23] * vector.y + values[M33] * vector.z); |
||
| 293 | } |
||
| 294 | |||
| 295 | static public void interpolate(Matrix3 result, Matrix3 a, Matrix3 b, float ratio) |
||
| 296 | { |
||
| 297 | result.values[M11] = a.values[M11] + (b.values[M11] - a.values[M11]) * ratio; |
||
| 298 | result.values[M21] = a.values[M21] + (b.values[M21] - a.values[M21]) * ratio; |
||
| 299 | result.values[M31] = a.values[M31] + (b.values[M31] - a.values[M31]) * ratio; |
||
| 300 | result.values[M12] = a.values[M12] + (b.values[M12] - a.values[M12]) * ratio; |
||
| 301 | result.values[M22] = a.values[M22] + (b.values[M22] - a.values[M22]) * ratio; |
||
| 302 | result.values[M32] = a.values[M32] + (b.values[M32] - a.values[M32]) * ratio; |
||
| 303 | result.values[M13] = a.values[M13] + (b.values[M13] - a.values[M13]) * ratio; |
||
| 304 | result.values[M23] = a.values[M23] + (b.values[M23] - a.values[M23]) * ratio; |
||
| 305 | result.values[M33] = a.values[M33] + (b.values[M33] - a.values[M33]) * ratio; |
||
| 306 | } |
||
| 307 | |||
| 308 | static public Matrix3 interpolate(Matrix3 a, Matrix3 b, float ratio) |
||
| 309 | { |
||
| 310 | Matrix3 result = createIdentity(); |
||
| 311 | interpolate(result, a, b, ratio); |
||
| 312 | return result; |
||
| 313 | } |
||
| 314 | |||
| 315 | public Vector3 getForwardVector() |
||
| 316 | { |
||
| 317 | Vector3 forward = new Vector3(-values[M31], -values[M32], -values[M33]); |
||
| 318 | forward.normalize(); |
||
| 319 | return forward; |
||
| 320 | } |
||
| 321 | |||
| 322 | public Vector3 getUpVector() |
||
| 323 | { |
||
| 324 | Vector3 up = new Vector3(values[M21], values[M22], values[M23]); |
||
| 325 | up.normalize(); |
||
| 326 | return up; |
||
| 327 | } |
||
| 328 | |||
| 329 | public Vector3 getRightVector() |
||
| 330 | { |
||
| 331 | Vector3 right = new Vector3(values[M11], values[M12], values[M13]); |
||
| 332 | right.normalize(); |
||
| 333 | return right; |
||
| 334 | } |
||
| 335 | |||
| 336 | public float[] toGLMatrix() |
||
| 337 | { |
||
| 338 | return values; |
||
| 339 | } |
||
| 340 | } |
||
| 341 | } |