From 134f86e8d5c414409631b25b8c6f0ee45fbd8631 Mon Sep 17 00:00:00 2001 From: David Walter Seikel Date: Thu, 3 Nov 2016 21:44:39 +1000 Subject: Initial update to OpenSim 0.8.2.1 source code. --- .../ConvexDecompositionDotNet/float4x4.cs | 284 +++++++++++++++++++++ 1 file changed, 284 insertions(+) create mode 100644 OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs (limited to 'OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs') diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs new file mode 100644 index 0000000..087eba7 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs @@ -0,0 +1,284 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +{ + public class float4x4 + { + public float4 x = new float4(); + public float4 y = new float4(); + public float4 z = new float4(); + public float4 w = new float4(); + + public float4x4() + { + } + + public float4x4(float4 _x, float4 _y, float4 _z, float4 _w) + { + x = new float4(_x); + y = new float4(_y); + z = new float4(_z); + w = new float4(_w); + } + + public float4x4( + float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23, + float m30, float m31, float m32, float m33) + { + x = new float4(m00, m01, m02, m03); + y = new float4(m10, m11, m12, m13); + z = new float4(m20, m21, m22, m23); + w = new float4(m30, m31, m32, m33); + } + + public float4x4(float4x4 m) + { + x = new float4(m.x); + y = new float4(m.y); + z = new float4(m.z); + w = new float4(m.w); + } + + public float4 this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + throw new ArgumentOutOfRangeException(); + } + set + { + switch (i) + { + case 0: x = value; return; + case 1: y = value; return; + case 2: z = value; return; + case 3: w = value; return; + } + throw new ArgumentOutOfRangeException(); + } + } + + public override int GetHashCode() + { + return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); + } + + public override bool Equals(object obj) + { + float4x4 m = obj as float4x4; + if (m == null) + return false; + + return this == m; + } + + public static float4x4 operator *(float4x4 a, float4x4 b) + { + return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b); + } + + public static bool operator ==(float4x4 a, float4x4 b) + { + return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); + } + + public static bool operator !=(float4x4 a, float4x4 b) + { + return !(a == b); + } + + public static float4x4 Inverse(float4x4 m) + { + float4x4 d = new float4x4(); + //float dst = d.x.x; + float[] tmp = new float[12]; // temp array for pairs + float[] src = new float[16]; // array of transpose source matrix + float det; // determinant + // transpose matrix + for (int i = 0; i < 4; i++) + { + src[i] = m[i].x; + src[i + 4] = m[i].y; + src[i + 8] = m[i].z; + src[i + 12] = m[i].w; + } + // calculate pairs for first 8 elements (cofactors) + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + // calculate first 8 elements (cofactors) + d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; + d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; + d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; + d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; + d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; + d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; + d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; + d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; + d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; + d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; + d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; + d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; + d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; + d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; + d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; + d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; + // calculate pairs for second 8 elements (cofactors) + tmp[0] = src[2]*src[7]; + tmp[1] = src[3]*src[6]; + tmp[2] = src[1]*src[7]; + tmp[3] = src[3]*src[5]; + tmp[4] = src[1]*src[6]; + tmp[5] = src[2]*src[5]; + tmp[6] = src[0]*src[7]; + tmp[7] = src[3]*src[4]; + tmp[8] = src[0]*src[6]; + tmp[9] = src[2]*src[4]; + tmp[10] = src[0]*src[5]; + tmp[11] = src[1]*src[4]; + // calculate second 8 elements (cofactors) + d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; + d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; + d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; + d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; + d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; + d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; + d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; + d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; + d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; + d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; + d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; + d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; + d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; + d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; + d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; + d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; + // calculate determinant + det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w; + // calculate matrix inverse + det = 1/det; + for (int j = 0; j < 4; j++) + d[j] *= det; + return d; + } + + public static float4x4 MatrixRigidInverse(float4x4 m) + { + float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); + float4x4 rot = new float4x4(m); + rot.w = new float4(0f, 0f, 0f, 1f); + return trans_inverse * MatrixTranspose(rot); + } + public static float4x4 MatrixTranspose(float4x4 m) + { + return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w); + } + public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf) + { + float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height + float w = h / aspect; // view space width + return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0); + } + public static float4x4 MatrixTranslation(float3 t) + { + return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1); + } + public static float4x4 MatrixRotationZ(float angle_radians) + { + float s = (float)Math.Sin(angle_radians); + float c = (float)Math.Cos(angle_radians); + return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); + } + public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up) + { + float4x4 m = new float4x4(); + m.w.w = 1.0f; + m.w.setxyz(eye); + m.z.setxyz(float3.normalize(eye - at)); + m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz()))); + m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz())); + return MatrixRigidInverse(m); + } + + public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v) + { + // builds a 4x4 transformation matrix based on orientation q and translation v + float qx2 = q.x * q.x; + float qy2 = q.y * q.y; + float qz2 = q.z * q.z; + + float qxqy = q.x * q.y; + float qxqz = q.x * q.z; + float qxqw = q.x * q.w; + float qyqz = q.y * q.z; + float qyqw = q.y * q.w; + float qzqw = q.z * q.w; + + return new float4x4( + 1 - 2 * (qy2 + qz2), + 2 * (qxqy + qzqw), + 2 * (qxqz - qyqw), + 0, + 2 * (qxqy - qzqw), + 1 - 2 * (qx2 + qz2), + 2 * (qyqz + qxqw), + 0, + 2 * (qxqz + qyqw), + 2 * (qyqz - qxqw), + 1 - 2 * (qx2 + qy2), + 0, + v.x, + v.y, + v.z, + 1.0f); + } + } +} -- cgit v1.1