diff options
Diffstat (limited to 'OpenSim/Region/Physics/Meshing/HelperTypes.cs')
-rw-r--r-- | OpenSim/Region/Physics/Meshing/HelperTypes.cs | 114 |
1 files changed, 36 insertions, 78 deletions
diff --git a/OpenSim/Region/Physics/Meshing/HelperTypes.cs b/OpenSim/Region/Physics/Meshing/HelperTypes.cs index 418baf5..816b6bb 100644 --- a/OpenSim/Region/Physics/Meshing/HelperTypes.cs +++ b/OpenSim/Region/Physics/Meshing/HelperTypes.cs | |||
@@ -70,11 +70,6 @@ public class Quaternion | |||
70 | return c; | 70 | return c; |
71 | } | 71 | } |
72 | 72 | ||
73 | |||
74 | public Matrix4 computeMatrix() | ||
75 | { | ||
76 | return new Matrix4(this); | ||
77 | } | ||
78 | public void normalize() | 73 | public void normalize() |
79 | { | 74 | { |
80 | //float mag = length(); | 75 | //float mag = length(); |
@@ -95,77 +90,8 @@ public class Quaternion | |||
95 | return (float)Math.Sqrt(w * w + x * x + y * y + z * z); | 90 | return (float)Math.Sqrt(w * w + x * x + y * y + z * z); |
96 | } | 91 | } |
97 | } | 92 | } |
98 | public class Matrix4 | 93 | |
99 | { | 94 | |
100 | public float m00 = 0; | ||
101 | public float m01 = 0; | ||
102 | public float m02 = 0; | ||
103 | public float m03 = 0; | ||
104 | public float m10 = 0; | ||
105 | public float m11 = 0; | ||
106 | public float m12 = 0; | ||
107 | public float m13 = 0; | ||
108 | public float m20 = 0; | ||
109 | public float m21 = 0; | ||
110 | public float m22 = 0; | ||
111 | public float m23 = 0; | ||
112 | public float m30 = 0; | ||
113 | public float m31 = 0; | ||
114 | public float m32 = 0; | ||
115 | public float m33 = 1; | ||
116 | |||
117 | public Matrix4(float m001, float m011, float m021, float m031, float m101, float m111, float m121, float m131, float m201, float m211, float m221, float m231, float m301, float m311, float m321, float m331) | ||
118 | { | ||
119 | m00 = m001; | ||
120 | m01 = m011; | ||
121 | m02 = m021; | ||
122 | m03 = m031; | ||
123 | m10 = m101; | ||
124 | m11 = m111; | ||
125 | m12 = m121; | ||
126 | m13 = m131; | ||
127 | m20 = m201; | ||
128 | m21 = m211; | ||
129 | m22 = m221; | ||
130 | m23 = m231; | ||
131 | m30 = m301; | ||
132 | m31 = m311; | ||
133 | m32 = m321; | ||
134 | m33 = m331; | ||
135 | } | ||
136 | public Matrix4() | ||
137 | { | ||
138 | } | ||
139 | public Matrix4(Quaternion r) | ||
140 | { | ||
141 | m00 = 1 - (2 * (r.y * r.y)) - (2 * (r.z * r.z)); | ||
142 | m01 = (r.x * r.y * 2) - (r.w * r.z * 2); | ||
143 | m02 = (r.x * r.z * 2) + (r.w * r.y * 2); | ||
144 | m03 = 0f; | ||
145 | m10 = (r.x * r.y * 2) + (r.w * r.z * 2); | ||
146 | m11 = 1 - (2 * (r.x * r.x)) - (2 * (r.z * r.z)); | ||
147 | m12 = (r.y * r.z * 2) - (r.w * r.x * 2); | ||
148 | m13 = 0f; | ||
149 | m20 = (r.x * r.z * 2) - (r.w * r.y * 2); | ||
150 | m21 = (r.y * r.z * 2) - (r.w * r.x * 2); | ||
151 | m22 = 1 - (2 * (r.x * r.x)) - (2 * (r.y * r.y)); | ||
152 | m23 = 0f; | ||
153 | m30 = 0f; | ||
154 | m31 = 0f; | ||
155 | m32 = 0f; | ||
156 | m33 = 1f; | ||
157 | } | ||
158 | public Vertex transform(Vertex o) | ||
159 | { | ||
160 | Vertex r = new Vertex(0,0,0); | ||
161 | // w value implicitly 1 therefore the last + m3x actually represents (m3x * o.W) = m3x | ||
162 | // in calculating the dot product. | ||
163 | r.X = (m00 * o.X) + (m10 * o.Y) + (m20 * o.Z) + m30; | ||
164 | r.Y = (m01 * o.X) + (m11 * o.Y) + (m21 * o.Z) + m31; | ||
165 | r.Z = (m02 * o.X) + (m12 * o.Y) + (m22 * o.Z) + m32; | ||
166 | return r; | ||
167 | } | ||
168 | } | ||
169 | 95 | ||
170 | public class Vertex : PhysicsVector, IComparable<Vertex> | 96 | public class Vertex : PhysicsVector, IComparable<Vertex> |
171 | { | 97 | { |
@@ -199,8 +125,40 @@ public class Vertex : PhysicsVector, IComparable<Vertex> | |||
199 | #pragma warning disable 0108 | 125 | #pragma warning disable 0108 |
200 | public static Vertex operator *(Vertex v, Quaternion q) | 126 | public static Vertex operator *(Vertex v, Quaternion q) |
201 | { | 127 | { |
202 | Matrix4 tm = q.computeMatrix(); | 128 | // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ |
203 | return tm.transform(v); | 129 | |
130 | Vertex v2 = new Vertex(0f, 0f, 0f); | ||
131 | |||
132 | v2.X = q.w * q.w * v.X + | ||
133 | 2f * q.y * q.w * v.Z - | ||
134 | 2f * q.z * q.w * v.Y + | ||
135 | q.x * q.x * v.X + | ||
136 | 2f * q.y * q.x * v.Y + | ||
137 | 2f * q.z * q.x * v.Z - | ||
138 | q.z * q.z * v.X - | ||
139 | q.y * q.y * v.X; | ||
140 | |||
141 | v2.Y = | ||
142 | 2f * q.x * q.y * v.X + | ||
143 | q.y * q.y * v.Y + | ||
144 | 2f * q.z * q.y * v.Z + | ||
145 | 2f * q.w * q.z * v.X - | ||
146 | q.z * q.z * v.Y + | ||
147 | q.w * q.w * v.Y - | ||
148 | 2f * q.x * q.w * v.Z - | ||
149 | q.x * q.x * v.Y; | ||
150 | |||
151 | v2.Z = | ||
152 | 2f * q.x * q.z * v.X + | ||
153 | 2f * q.y * q.z * v.Y + | ||
154 | q.z * q.z * v.Z - | ||
155 | 2f * q.w * q.y * v.X - | ||
156 | q.y * q.y * v.Z + | ||
157 | 2f * q.w * q.x * v.Y - | ||
158 | q.x * q.x * v.Z + | ||
159 | q.w * q.w * v.Z; | ||
160 | |||
161 | return v2; | ||
204 | } | 162 | } |
205 | 163 | ||
206 | public static Vertex operator +(Vertex v1, Vertex v2) | 164 | public static Vertex operator +(Vertex v1, Vertex v2) |