diff --git a/OpenMetaverse.Rendering.GPL/Face.cs b/OpenMetaverse.Rendering.GPL/Face.cs index 52531bb8..9282fcc1 100644 --- a/OpenMetaverse.Rendering.GPL/Face.cs +++ b/OpenMetaverse.Rendering.GPL/Face.cs @@ -87,7 +87,7 @@ namespace OpenMetaverse.Rendering baseVert.Normal = ((corners[1].Position - corners[0].Position) % (corners[2].Position - corners[1].Position)); - baseVert.Normal = Vector3.Norm(baseVert.Normal); + baseVert.Normal = Vector3.Normalize(baseVert.Normal); if ((face.Mask & FaceMask.Top) != 0) { @@ -227,12 +227,12 @@ namespace OpenMetaverse.Rendering face.Center, cuv, face.Vertices[0].Position, face.Vertices[0].TexCoord, face.Vertices[1].Position, face.Vertices[1].TexCoord); - binormal = Vector3.Norm(binormal); + binormal.Normalize(); Vector3 d0 = face.Center - face.Vertices[0].Position; Vector3 d1 = face.Center - face.Vertices[1].Position; Vector3 normal = ((face.Mask & FaceMask.Top) != 0) ? (d0 % d1) : (d1 % d0); - normal = Vector3.Norm(normal); + normal.Normalize(); // If not hollow and not open create a center point in the cap if ((face.Mask & FaceMask.Hollow) == 0 && (face.Mask & FaceMask.Open) == 0) @@ -518,18 +518,20 @@ namespace OpenMetaverse.Rendering } // Adjust normals based on wrapping and stitching - bool sBottomConverges = ( - Vector3.MagSquared( - face.Vertices[0].Position - - face.Vertices[face.NumS * (face.NumT - 2)].Position - ) < 0.000001f); - bool sTopConverges = ( - Vector3.MagSquared( - face.Vertices[face.NumS - 1].Position - - face.Vertices[face.NumS * (face.NumT - 2) + - face.NumS - 1].Position - ) < 0.000001f); - Primitive.SculptType sculptType = Primitive.SculptType.None; // TODO: Sculpt support + Vector3 test1 = + face.Vertices[0].Position - + face.Vertices[face.NumS * (face.NumT - 2)].Position; + + Vector3 test2 = + face.Vertices[face.NumS - 1].Position - + face.Vertices[face.NumS * (face.NumT - 2) + + face.NumS - 1].Position; + + bool sBottomConverges = (test1.LengthSquared() < 0.000001f); + bool sTopConverges = (test2.LengthSquared() < 0.000001f); + + // TODO: Sculpt support + Primitive.SculptType sculptType = Primitive.SculptType.None; if (sculptType == Primitive.SculptType.None) { @@ -606,8 +608,8 @@ namespace OpenMetaverse.Rendering for (i = 0; i < face.Vertices.Count; i++) { Vertex vertex = face.Vertices[i]; - vertex.Normal = Vector3.Norm(vertex.Normal); - vertex.Binormal = Vector3.Norm(vertex.Binormal); + vertex.Normal.Normalize(); + vertex.Binormal.Normalize(); face.Vertices[i] = vertex; } } diff --git a/OpenMetaverse.Rendering.GPL/GPLRenderer.cs b/OpenMetaverse.Rendering.GPL/GPLRenderer.cs index 495eaf0b..2414dd6c 100644 --- a/OpenMetaverse.Rendering.GPL/GPLRenderer.cs +++ b/OpenMetaverse.Rendering.GPL/GPLRenderer.cs @@ -777,14 +777,14 @@ namespace OpenMetaverse.Rendering { PathPoint point = new PathPoint(); - float t = Helpers.Lerp(prim.PathBegin, prim.PathEnd, (float)i * step); + float t = MathHelper.Lerp(prim.PathBegin, prim.PathEnd, (float)i * step); point.Position = new Vector3( - Helpers.Lerp(0, prim.PathShearX, t), - Helpers.Lerp(0, prim.PathShearY, t), + MathHelper.Lerp(0, prim.PathShearX, t), + MathHelper.Lerp(0, prim.PathShearY, t), t - 0.5f); - point.Rotation.SetQuaternion(Helpers.Lerp(F_PI * prim.PathTwistBegin, F_PI * prim.PathTwist, t), 0f, 0f, 1f); - point.Scale.X = Helpers.Lerp(startScale.X, endScale.X, t); - point.Scale.Y = Helpers.Lerp(startScale.Y, endScale.Y, t); + point.Rotation = Quaternion.CreateFromAxisAngle(MathHelper.Lerp(F_PI * prim.PathTwistBegin, F_PI * prim.PathTwist, t), 0f, 0f, 1f); + point.Scale.X = MathHelper.Lerp(startScale.X, endScale.X, t); + point.Scale.Y = MathHelper.Lerp(startScale.Y, endScale.Y, t); point.TexT = t; path.Points.Add(point); @@ -860,7 +860,7 @@ namespace OpenMetaverse.Rendering angStep = 2f * F_PI * tStep * angScale; // Scale to have size "match" scale. Compensates to get object to generally fill bounding box - int totalSides = Helpers.Round(sides / angScale); + int totalSides = MathHelper.Round(sides / angScale); if (totalSides < 8) scale = TABLE_SCALE[totalSides]; @@ -884,7 +884,7 @@ namespace OpenMetaverse.Rendering // Only use if it's not almost exactly on an edge if (tFraction < 0.9999f) { - Vector3 newPt = Helpers.Lerp(pt1, pt2, tFraction); + Vector3 newPt = Vector3.Lerp(pt1, pt2, tFraction); float ptX = newPt.X; if (ptX < profile.MinX) @@ -921,7 +921,7 @@ namespace OpenMetaverse.Rendering tFraction = (end - (t - tStep)) * sides; if (tFraction > 0.0001f) { - Vector3 newPt = Helpers.Lerp(pt1, pt2, tFraction); + Vector3 newPt = Vector3.Lerp(pt1, pt2, tFraction); float ptX = newPt.X; if (ptX < profile.MinX) @@ -1030,23 +1030,23 @@ namespace OpenMetaverse.Rendering float step = 1f / sides; float t = prim.PathBegin; ang = 2f * F_PI * revolutions * t; - s = (float)Math.Sin(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); - c = (float)Math.Cos(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); + s = (float)Math.Sin(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); + c = (float)Math.Cos(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); point = new PathPoint(); point.Position = new Vector3( - 0 + Helpers.Lerp(0, prim.PathShearX, s) + - 0 + Helpers.Lerp(-skew, skew, t) * 0.5f, - c + Helpers.Lerp(0, prim.PathShearY, s), + 0 + MathHelper.Lerp(0, prim.PathShearX, s) + + 0 + MathHelper.Lerp(-skew, skew, t) * 0.5f, + c + MathHelper.Lerp(0, prim.PathShearY, s), s); - point.Scale.X = holeX * Helpers.Lerp(taperXBegin, taperXEnd, t); - point.Scale.Y = holeY * Helpers.Lerp(taperYBegin, taperYEnd, t); + point.Scale.X = holeX * MathHelper.Lerp(taperXBegin, taperXEnd, t); + point.Scale.Y = holeY * MathHelper.Lerp(taperYBegin, taperYEnd, t); point.TexT = t; // Twist rotates the path along the x,y plane - twist.SetQuaternion(Helpers.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); + twist = Quaternion.CreateFromAxisAngle(MathHelper.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); // Rotate the point around the circle's center - qang.SetQuaternion(ang, pathAxis); + qang = Quaternion.CreateFromAxisAngle(pathAxis, ang); point.Rotation = twist * qang; path.Points.Add(point); @@ -1061,23 +1061,23 @@ namespace OpenMetaverse.Rendering while (t < prim.PathEnd) { ang = 2f * F_PI * revolutions * t; - c = (float)Math.Cos(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); - s = (float)Math.Sin(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); + c = (float)Math.Cos(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); + s = (float)Math.Sin(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); point.Position = new Vector3( - 0 + Helpers.Lerp(0, prim.PathShearX, s) + - 0 + Helpers.Lerp(-skew, skew, t) * 0.5f, - c + Helpers.Lerp(0, prim.PathShearY, s), + 0 + MathHelper.Lerp(0, prim.PathShearX, s) + + 0 + MathHelper.Lerp(-skew, skew, t) * 0.5f, + c + MathHelper.Lerp(0, prim.PathShearY, s), s); - point.Scale.X = holeX * Helpers.Lerp(taperXBegin, taperXEnd, t); - point.Scale.Y = holeY * Helpers.Lerp(taperYBegin, taperYEnd, t); + point.Scale.X = holeX * MathHelper.Lerp(taperXBegin, taperXEnd, t); + point.Scale.Y = holeY * MathHelper.Lerp(taperYBegin, taperYEnd, t); point.TexT = t; // Twist rotates the path along the x,y plane - twist.SetQuaternion(Helpers.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); + twist = Quaternion.CreateFromAxisAngle(MathHelper.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); // Rotate the point around the circle's center - qang.SetQuaternion(ang, pathAxis); + qang = Quaternion.CreateFromAxisAngle(pathAxis, ang); point.Rotation = twist * qang; path.Points.Add(point); @@ -1088,20 +1088,20 @@ namespace OpenMetaverse.Rendering t = prim.PathEnd; point = new PathPoint(); ang = 2f * F_PI * revolutions * t; - c = (float)Math.Cos(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); - s = (float)Math.Sin(ang) * Helpers.Lerp(radiusStart, radiusEnd, t); + c = (float)Math.Cos(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); + s = (float)Math.Sin(ang) * MathHelper.Lerp(radiusStart, radiusEnd, t); point.Position = new Vector3( - Helpers.Lerp(0, prim.PathShearX, s) + Helpers.Lerp(-skew, skew, t) * 0.5f, - c + Helpers.Lerp(0, prim.PathShearY, s), + MathHelper.Lerp(0, prim.PathShearX, s) + MathHelper.Lerp(-skew, skew, t) * 0.5f, + c + MathHelper.Lerp(0, prim.PathShearY, s), s); - point.Scale.X = holeX * Helpers.Lerp(taperXBegin, taperXEnd, t); - point.Scale.Y = holeY * Helpers.Lerp(taperYBegin, taperYEnd, t); + point.Scale.X = holeX * MathHelper.Lerp(taperXBegin, taperXEnd, t); + point.Scale.Y = holeY * MathHelper.Lerp(taperYBegin, taperYEnd, t); point.TexT = t; // Twist rotates the path along the x,y plane - twist.SetQuaternion(Helpers.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); - qang.SetQuaternion(ang, pathAxis); + twist = Quaternion.CreateFromAxisAngle(MathHelper.Lerp(twistBegin, twistEnd, t) * 2f * F_PI - F_PI, 0f, 0f, 1f); + qang = Quaternion.CreateFromAxisAngle(pathAxis, ang); point.Rotation = twist * qang; path.Points.Add(point); @@ -1197,7 +1197,7 @@ namespace OpenMetaverse.Rendering Vector3 d1 = p1 - pa; Vector3 d2 = p2 - pb; - if (Vector3.MagSquared(d1) < Vector3.MagSquared(d2)) + if (d1.LengthSquared() < d2.LengthSquared()) use_tri_1a2 = true; else use_tri_1a2 = false; diff --git a/OpenMetaverse.Rendering.GPL/Texture.cs b/OpenMetaverse.Rendering.GPL/Texture.cs index fe03a29b..cdfd3ec8 100644 --- a/OpenMetaverse.Rendering.GPL/Texture.cs +++ b/OpenMetaverse.Rendering.GPL/Texture.cs @@ -86,7 +86,7 @@ namespace OpenMetaverse.Rendering Vector3 vec) { Vector3 binormal; - float d = Vector3.Dot(vertex.Normal, Vector3.Fwd); + float d = Vector3.Dot(vertex.Normal, Vector3.UnitX); if (d >= 0.5f || d <= -0.5f) { binormal = new Vector3(0f, 1f, 0f); diff --git a/OpenMetaverse/AgentManagerMovement.cs b/OpenMetaverse/AgentManagerMovement.cs index ddd372f5..2b4f7f29 100644 --- a/OpenMetaverse/AgentManagerMovement.cs +++ b/OpenMetaverse/AgentManagerMovement.cs @@ -504,9 +504,8 @@ namespace OpenMetaverse if (Client.Settings.SEND_AGENT_UPDATES) { Vector3 myPos = Client.Self.SimPosition; - Vector3 forward = new Vector3(1, 0, 0); - Vector3 offset = Vector3.Norm(target - myPos); - Quaternion newRot = Vector3.RotBetween(forward, offset); + Vector3 offset = Vector3.Normalize(target - myPos); + Quaternion newRot = Vector3.RotationBetween(Vector3.UnitX, offset); BodyRotation = newRot; HeadRotation = newRot; diff --git a/OpenMetaverse/BitPack.cs b/OpenMetaverse/BitPack.cs index fe9c67ec..0a9b2b19 100644 --- a/OpenMetaverse/BitPack.cs +++ b/OpenMetaverse/BitPack.cs @@ -134,7 +134,7 @@ namespace OpenMetaverse max = 1 << intBits; - float fixedVal = Helpers.Clamp(data, (float)min, (float)max); + float fixedVal = MathHelper.Clamp(data, (float)min, (float)max); if (isSigned) fixedVal += max; fixedVal *= 1 << fracBits; diff --git a/OpenMetaverse/CapsToPacket.cs b/OpenMetaverse/CapsToPacket.cs index 46985e36..788d004a 100644 --- a/OpenMetaverse/CapsToPacket.cs +++ b/OpenMetaverse/CapsToPacket.cs @@ -38,10 +38,10 @@ namespace OpenMetaverse.Packets public static string ToXmlString(Packet packet) { - return LLSDParser.SerializeXmlString(ToLLSD(packet)); + return LLSDParser.SerializeXmlString(GetLLSD(packet)); } - public static LLSD ToLLSD(Packet packet) + public static LLSD GetLLSD(Packet packet) { LLSDMap body = new LLSDMap(); Type type = packet.GetType(); @@ -80,7 +80,7 @@ namespace OpenMetaverse.Packets public static byte[] ToBinary(Packet packet) { - return LLSDParser.SerializeBinary(ToLLSD(packet)); + return LLSDParser.SerializeBinary(GetLLSD(packet)); } public static Packet FromXmlString(string xml) diff --git a/OpenMetaverse/CoordinateFrame.cs b/OpenMetaverse/CoordinateFrame.cs index e34c48a5..c2504cca 100644 --- a/OpenMetaverse/CoordinateFrame.cs +++ b/OpenMetaverse/CoordinateFrame.cs @@ -117,12 +117,12 @@ namespace OpenMetaverse throw new ArgumentException("Non-finite in CoordinateFrame constructor"); } - public CoordinateFrame(Vector3 origin, Matrix3 rotation) + public CoordinateFrame(Vector3 origin, Matrix4 rotation) { this.origin = origin; - xAxis = rotation[0]; - yAxis = rotation[1]; - zAxis = rotation[2]; + xAxis = rotation.AtAxis; + yAxis = rotation.LeftAxis; + zAxis = rotation.UpAxis; if (!IsFinite()) throw new ArgumentException("Non-finite in CoordinateFrame constructor"); @@ -130,12 +130,12 @@ namespace OpenMetaverse public CoordinateFrame(Vector3 origin, Quaternion rotation) { - Matrix3 m = rotation.ToMatrix3(); + Matrix4 m = Matrix4.CreateFromQuaternion(rotation); this.origin = origin; - xAxis = m[0]; - yAxis = m[1]; - zAxis = m[2]; + xAxis = m.AtAxis; + yAxis = m.LeftAxis; + zAxis = m.UpAxis; if (!IsFinite()) throw new ArgumentException("Non-finite in CoordinateFrame constructor"); @@ -154,20 +154,20 @@ namespace OpenMetaverse public void Rotate(float angle, Vector3 rotationAxis) { - Quaternion q = new Quaternion(angle, rotationAxis); + Quaternion q = Quaternion.CreateFromAxisAngle(rotationAxis, angle); Rotate(q); } public void Rotate(Quaternion q) { - Matrix3 m = q.ToMatrix3(); + Matrix4 m = Matrix4.CreateFromQuaternion(q); Rotate(m); } - public void Rotate(Matrix3 m) + public void Rotate(Matrix4 m) { - xAxis = Vector3.Rot(xAxis, m); - yAxis = Vector3.Rot(yAxis, m); + xAxis = Vector3.Transform(xAxis, m); + yAxis = Vector3.Transform(yAxis, m); Orthonormalize(); @@ -177,8 +177,8 @@ namespace OpenMetaverse public void Roll(float angle) { - Quaternion q = new Quaternion(angle, xAxis); - Matrix3 m = q.ToMatrix3(); + Quaternion q = Quaternion.CreateFromAxisAngle(xAxis, angle); + Matrix4 m = Matrix4.CreateFromQuaternion(q); Rotate(m); if (!yAxis.IsFinite() || !zAxis.IsFinite()) @@ -187,8 +187,8 @@ namespace OpenMetaverse public void Pitch(float angle) { - Quaternion q = new Quaternion(angle, yAxis); - Matrix3 m = q.ToMatrix3(); + Quaternion q = Quaternion.CreateFromAxisAngle(yAxis, angle); + Matrix4 m = Matrix4.CreateFromQuaternion(q); Rotate(m); if (!xAxis.IsFinite() || !zAxis.IsFinite()) @@ -197,8 +197,8 @@ namespace OpenMetaverse public void Yaw(float angle) { - Quaternion q = new Quaternion(angle, zAxis); - Matrix3 m = q.ToMatrix3(); + Quaternion q = Quaternion.CreateFromAxisAngle(zAxis, angle); + Matrix4 m = Matrix4.CreateFromQuaternion(q); Rotate(m); if (!xAxis.IsFinite() || !yAxis.IsFinite()) @@ -223,10 +223,10 @@ namespace OpenMetaverse { // Prevent left from being zero at.X += 0.01f; - at = Vector3.Norm(at); + at.Normalize(); left = Vector3.Cross(upDirection, at); } - left = Vector3.Norm(left); + left.Normalize(); xAxis = at; yAxis = left; @@ -256,7 +256,7 @@ namespace OpenMetaverse { this.origin = origin; Vector3 at = new Vector3(target - origin); - at = Vector3.Norm(at); + at.Normalize(); LookDirection(at, upDirection); } @@ -274,9 +274,9 @@ namespace OpenMetaverse protected void Orthonormalize() { // Make sure the axis are orthagonal and normalized - xAxis = Vector3.Norm(xAxis); + xAxis.Normalize(); yAxis -= xAxis * (xAxis * yAxis); - yAxis = Vector3.Norm(yAxis); + yAxis.Normalize(); zAxis = Vector3.Cross(xAxis, yAxis); } } diff --git a/OpenMetaverse/Helpers.cs b/OpenMetaverse/Helpers.cs index d42253e3..bc4188c4 100644 --- a/OpenMetaverse/Helpers.cs +++ b/OpenMetaverse/Helpers.cs @@ -133,7 +133,7 @@ namespace OpenMetaverse /// public static short TEOffsetShort(float offset) { - offset = Clamp(offset, -1.0f, 1.0f); + offset = MathHelper.Clamp(offset, -1.0f, 1.0f); offset *= 32767.0f; return (short)Math.Round(offset); } @@ -408,7 +408,7 @@ namespace OpenMetaverse /// A single byte representing the original float value public static byte FloatToByte(float val, float lower, float upper) { - val = Clamp(val, lower, upper); + val = MathHelper.Clamp(val, lower, upper); // Normalize the value val -= lower; val /= (upper - lower); @@ -503,18 +503,6 @@ namespace OpenMetaverse return (uint)((bytes[3] << 24) + (bytes[2] << 16) + (bytes[1] << 8) + bytes[0]); } - /// - /// Clamp a given value between a range - /// - /// Value to clamp - /// Minimum allowable value - /// Maximum allowable value - /// A value inclusively between lower and upper - public static float Clamp(float val, float lower, float upper) - { - return Math.Min(Math.Max(val, lower), upper); - } - /// /// Convert a variable length UTF8 byte array to a string /// @@ -786,51 +774,6 @@ namespace OpenMetaverse rhs = temp; } - /// - /// Test if a single precision float is a finite number - /// - public static bool IsFinite(float value) - { - return !(Single.IsNaN(value) || Single.IsInfinity(value)); - } - - /// - /// - /// - /// - /// - public static int Round(float val) - { - return (int)Math.Floor(val + 0.5f); - } - - /// - /// - /// - /// - /// - /// - /// - public static float Lerp(float a, float b, float u) - { - return a + ((b - a) * u); - } - - /// - /// - /// - /// - /// - /// - /// - public static Vector3 Lerp(Vector3 a, Vector3 b, float u) - { - return new Vector3( - a.X + (b.X - a.X) * u, - a.Y + (b.Y - a.Y) * u, - a.Z + (b.Z - a.Z) * u); - } - /// /// Decode a zerocoded byte array, used to decompress packets marked /// with the zerocoded flag @@ -883,7 +826,7 @@ namespace OpenMetaverse return (int)zerolen; } - catch (Exception e) + catch (Exception) { Logger.Log(String.Format("Zerodecoding error: i={0}, srclen={1}, bodylen={2}, zerolen={3}\n{4}", i, srclen, bodylen, zerolen, FieldToHexString(src, srclen, null)), LogLevel.Error); @@ -1165,7 +1108,7 @@ namespace OpenMetaverse StructuredData.LLSDMap map = new OpenMetaverse.StructuredData.LLSDMap(prims.Count); for (int i = 0; i < prims.Count; i++) - map.Add(prims[i].LocalID.ToString(), prims[i].ToLLSD()); + map.Add(prims[i].LocalID.ToString(), prims[i].GetLLSD()); return map; } diff --git a/OpenMetaverse/InternalDictionary.cs b/OpenMetaverse/InternalDictionary.cs index 037d7283..2fc11d23 100644 --- a/OpenMetaverse/InternalDictionary.cs +++ b/OpenMetaverse/InternalDictionary.cs @@ -162,7 +162,7 @@ namespace OpenMetaverse /// List<Primitive> prims = Client.Network.CurrentSim.ObjectsPrimitives.FindAll( /// delegate(Primitive prim) { /// Vector3 pos = prim.Position; - /// return ((prim.ParentID == 0) && (pos != Vector3.Zero) && (Vector3.Dist(pos, location) < radius)); + /// return ((prim.ParentID == 0) && (pos != Vector3.Zero) && (Vector3.Distance(pos, location) < radius)); /// } /// ); /// diff --git a/OpenMetaverse/Login.cs b/OpenMetaverse/Login.cs index 2ef9f0ca..9a8d9421 100644 --- a/OpenMetaverse/Login.cs +++ b/OpenMetaverse/Login.cs @@ -557,7 +557,9 @@ namespace OpenMetaverse // Override SSL authentication mechanisms. DO NOT convert this to the // .NET 2.0 preferred method, the equivalent function in Mono has a // different name and it will break compatibility! +#pragma warning disable 0618 ServicePointManager.CertificatePolicy = new AcceptAllCertificatePolicy(); +#pragma warning restore 0618 // TODO: At some point, maybe we should check the cert? // Create the CAPS login structure diff --git a/OpenMetaverse/MathHelper.cs b/OpenMetaverse/MathHelper.cs new file mode 100644 index 00000000..12b5bb3f --- /dev/null +++ b/OpenMetaverse/MathHelper.cs @@ -0,0 +1,172 @@ +using System; + +namespace OpenMetaverse +{ + public static class MathHelper + { + public const float E = (float)Math.E; + public const float LOG10E = 0.4342945f; + public const float LOG2E = 1.442695f; + public const float PI = (float)Math.PI; + public const float TWO_PI = (float)(Math.PI * 2.0d); + public const float PI_OVER_TWO = (float)(Math.PI / 2.0d); + public const float PI_OVER_FOUR = (float)(Math.PI / 4.0d); + + /// + /// Clamp a given value between a range + /// + /// Value to clamp + /// Minimum allowable value + /// Maximum allowable value + /// A value inclusively between lower and upper + public static float Clamp(float value, float min, float max) + { + // First we check to see if we're greater than the max + value = (value > max) ? max : value; + + // Then we check to see if we're less than the min. + value = (value < min) ? min : value; + + // There's no check to see if min > max. + return value; + } + + /// + /// Clamp a given value between a range + /// + /// Value to clamp + /// Minimum allowable value + /// Maximum allowable value + /// A value inclusively between lower and upper + public static double Clamp(double value, double min, double max) + { + // First we check to see if we're greater than the max + value = (value > max) ? max : value; + + // Then we check to see if we're less than the min. + value = (value < min) ? min : value; + + // There's no check to see if min > max. + return value; + } + + /// + /// Round a floating-point value to the nearest integer + /// + /// Floating point number to round + /// Integer + public static int Round(float val) + { + return (int)Math.Floor(val + 0.5f); + } + + /// + /// Test if a single precision float is a finite number + /// + public static bool IsFinite(float value) + { + return !(Single.IsNaN(value) || Single.IsInfinity(value)); + } + + /// + /// Test if a double precision float is a finite number + /// + public static bool IsFinite(double value) + { + return !(Double.IsNaN(value) || Double.IsInfinity(value)); + } + + /// + /// Get the distance between two floating-point values + /// + /// First value + /// Second value + /// The distance between the two values + public static float Distance(float value1, float value2) + { + return Math.Abs(value1 - value2); + } + + public static float Hermite(float value1, float tangent1, float value2, float tangent2, float amount) + { + // All transformed to double not to lose precission + // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity + double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result; + double sCubed = s * s * s; + double sSquared = s * s; + + if (amount == 0f) + result = value1; + else if (amount == 1f) + result = value2; + else + result = (2d * v1 - 2d * v2 + t2 + t1) * sCubed + + (3d * v2 - 3d * v1 - 2d * t1 - t2) * sSquared + + t1 * s + v1; + return (float)result; + } + + public static double Hermite(double value1, double tangent1, double value2, double tangent2, double amount) + { + // All transformed to double not to lose precission + // Otherwise, for high numbers of param:amount the result is NaN instead of Infinity + double v1 = value1, v2 = value2, t1 = tangent1, t2 = tangent2, s = amount, result; + double sCubed = s * s * s; + double sSquared = s * s; + + if (amount == 0d) + result = value1; + else if (amount == 1f) + result = value2; + else + result = (2d * v1 - 2d * v2 + t2 + t1) * sCubed + + (3d * v2 - 3d * v1 - 2d * t1 - t2) * sSquared + + t1 * s + v1; + return result; + } + + public static float Lerp(float value1, float value2, float amount) + { + return value1 + (value2 - value1) * amount; + } + + public static double Lerp(double value1, double value2, double amount) + { + return value1 + (value2 - value1) * amount; + } + + public static float SmoothStep(float value1, float value2, float amount) + { + // It is expected that 0 < amount < 1 + // If amount < 0, return value1 + // If amount > 1, return value2 + float result = MathHelper.Clamp(amount, 0f, 1f); + return MathHelper.Hermite(value1, 0f, value2, 0f, result); + } + + public static double SmoothStep(double value1, double value2, double amount) + { + // It is expected that 0 < amount < 1 + // If amount < 0, return value1 + // If amount > 1, return value2 + double result = MathHelper.Clamp(amount, 0f, 1f); + return MathHelper.Hermite(value1, 0f, value2, 0f, result); + } + + public static float ToDegrees(float radians) + { + // This method uses double precission internally, + // though it returns single float + // Factor = 180 / pi + return (float)(radians * 57.295779513082320876798154814105); + } + + public static float ToRadians(float degrees) + { + // This method uses double precission internally, + // though it returns single float + // Factor = pi / 180 + return (float)(degrees * 0.017453292519943295769236907684886); + } + } +} diff --git a/OpenMetaverse/ObjectManager.cs b/OpenMetaverse/ObjectManager.cs index db08f001..3d5ad9d2 100644 --- a/OpenMetaverse/ObjectManager.cs +++ b/OpenMetaverse/ObjectManager.cs @@ -2690,8 +2690,8 @@ namespace OpenMetaverse // no acceleration if (avatar.Acceleration != Vector3.Zero && avatar.Velocity == Vector3.Zero) { - avatar.Position += (avatar.Velocity + (0.5f * (adjSeconds - HAVOK_TIMESTEP)) * - avatar.Acceleration) * adjSeconds; + avatar.Position += (avatar.Velocity + avatar.Acceleration * + (0.5f * (adjSeconds - HAVOK_TIMESTEP))) * adjSeconds; avatar.Velocity += avatar.Acceleration * adjSeconds; } #endregion Linear Motion @@ -2706,14 +2706,14 @@ namespace OpenMetaverse { #region Angular Velocity Vector3 angVel = prim.AngularVelocity; - float omega = Vector3.MagSquared(angVel); + float omega = angVel.LengthSquared(); if (omega > 0.00001f) { omega = (float)Math.Sqrt(omega); float angle = omega * adjSeconds; angVel *= 1.0f / omega; - Quaternion dQ = new Quaternion(angle, angVel); + Quaternion dQ = Quaternion.CreateFromAxisAngle(angVel, angle); prim.Rotation *= dQ; } @@ -2724,8 +2724,8 @@ namespace OpenMetaverse // no acceleration if (prim.Acceleration != Vector3.Zero && prim.Velocity == Vector3.Zero) { - prim.Position += (prim.Velocity + (0.5f * (adjSeconds - HAVOK_TIMESTEP)) * - prim.Acceleration) * adjSeconds; + prim.Position += (prim.Velocity + prim.Acceleration * + (0.5f * (adjSeconds - HAVOK_TIMESTEP))) * adjSeconds; prim.Velocity += prim.Acceleration * adjSeconds; } #endregion Linear Motion diff --git a/OpenMetaverse/Prims.cs b/OpenMetaverse/Prims.cs index af19059c..87c8f8b8 100644 --- a/OpenMetaverse/Prims.cs +++ b/OpenMetaverse/Prims.cs @@ -285,7 +285,7 @@ namespace OpenMetaverse /// /// /// - public LLSD ToLLSD() + public LLSD GetLLSD() { LLSDMap map = new LLSDMap(); @@ -294,7 +294,7 @@ namespace OpenMetaverse map["air_friction"] = LLSD.FromReal(Drag); map["wind_sensitivity"] = LLSD.FromReal(Wind); map["tension"] = LLSD.FromReal(Tension); - map["user_force"] = Force.ToLLSD(); + map["user_force"] = Force.GetLLSD(); return map; } @@ -382,11 +382,11 @@ namespace OpenMetaverse return data; } - public LLSD ToLLSD() + public LLSD GetLLSD() { LLSDMap map = new LLSDMap(); - map["color"] = Color.ToLLSD(); + map["color"] = Color.GetLLSD(); map["intensity"] = LLSD.FromReal(Intensity); map["radius"] = LLSD.FromReal(Radius); map["cutoff"] = LLSD.FromReal(Cutoff); @@ -456,7 +456,7 @@ namespace OpenMetaverse return data; } - public LLSD ToLLSD() + public LLSD GetLLSD() { LLSDMap map = new LLSDMap(); @@ -537,7 +537,7 @@ namespace OpenMetaverse Data.PCode, Data.Material); } - public LLSD ToLLSD() + public LLSD GetLLSD() { LLSDMap path = new LLSDMap(14); path["begin"] = LLSD.FromReal(Data.PathBegin); @@ -571,19 +571,19 @@ namespace OpenMetaverse prim["description"] = LLSD.FromString(Properties.Description); prim["phantom"] = LLSD.FromBoolean(((Flags & ObjectFlags.Phantom) != 0)); prim["physical"] = LLSD.FromBoolean(((Flags & ObjectFlags.Physics) != 0)); - prim["position"] = Position.ToLLSD(); - prim["rotation"] = Rotation.ToLLSD(); - prim["scale"] = Scale.ToLLSD(); + prim["position"] = Position.GetLLSD(); + prim["rotation"] = Rotation.GetLLSD(); + prim["scale"] = Scale.GetLLSD(); prim["material"] = LLSD.FromInteger((int)Data.Material); prim["shadows"] = LLSD.FromBoolean(((Flags & ObjectFlags.CastShadows) != 0)); - prim["textures"] = Textures.ToLLSD(); + prim["textures"] = Textures.GetLLSD(); prim["volume"] = volume; if (ParentID != 0) prim["parentid"] = LLSD.FromInteger(ParentID); - prim["light"] = Light.ToLLSD(); - prim["flex"] = Flexible.ToLLSD(); - prim["sculpt"] = Sculpt.ToLLSD(); + prim["light"] = Light.GetLLSD(); + prim["flex"] = Flexible.GetLLSD(); + prim["sculpt"] = Sculpt.GetLLSD(); return prim; } diff --git a/OpenMetaverse/TextureEntry.cs b/OpenMetaverse/TextureEntry.cs index 5675ed83..e7e68780 100644 --- a/OpenMetaverse/TextureEntry.cs +++ b/OpenMetaverse/TextureEntry.cs @@ -431,11 +431,11 @@ namespace OpenMetaverse hasAttribute = TextureAttributes.None; } - public LLSD ToLLSD(int faceNumber) + public LLSD GetLLSD(int faceNumber) { LLSDMap tex = new LLSDMap(10); if (faceNumber >= 0) tex["face_number"] = LLSD.FromInteger(faceNumber); - tex["colors"] = RGBA.ToLLSD(); + tex["colors"] = RGBA.GetLLSD(); tex["scales"] = LLSD.FromReal(RepeatU); tex["scalet"] = LLSD.FromReal(RepeatV); tex["offsets"] = LLSD.FromReal(OffsetU); @@ -596,17 +596,17 @@ namespace OpenMetaverse /// /// /// - public LLSD ToLLSD() + public LLSD GetLLSD() { LLSDArray array = new LLSDArray(); // Always add default texture - array.Add(DefaultTexture.ToLLSD(-1)); + array.Add(DefaultTexture.GetLLSD(-1)); for (int i = 0; i < MAX_FACES; i++) { if (FaceTextures[i] != null) - array.Add(FaceTextures[i].ToLLSD(i)); + array.Add(FaceTextures[i].GetLLSD(i)); } return array; diff --git a/OpenMetaverse/Types/Color4.cs b/OpenMetaverse/Types/Color4.cs new file mode 100644 index 00000000..8d4ef432 --- /dev/null +++ b/OpenMetaverse/Types/Color4.cs @@ -0,0 +1,368 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + /// + /// An 8-bit color structure including an alpha channel + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Color4 : IComparable, IEquatable + { + /// Red + public float R; + /// Green + public float G; + /// Blue + public float B; + /// Alpha + public float A; + + #region Constructors + + /// + /// + /// + /// + /// + /// + /// + public Color4(byte r, byte g, byte b, byte a) + { + const float quanta = 1.0f / 255.0f; + + R = (float)r * quanta; + G = (float)g * quanta; + B = (float)b * quanta; + A = (float)a * quanta; + } + + public Color4(float r, float g, float b, float a) + { + // Quick check to see if someone is doing something obviously wrong + // like using float values from 0.0 - 255.0 + if (r > 1f || g > 1f || b > 1f || a > 1f) + Logger.Log( + String.Format("Attempting to initialize Color4 with out of range values <{0},{1},{2},{3}>", + r, g, b, a), Helpers.LogLevel.Warning); + + // Valid range is from 0.0 to 1.0 + R = MathHelper.Clamp(r, 0f, 1f); + G = MathHelper.Clamp(g, 0f, 1f); + B = MathHelper.Clamp(b, 0f, 1f); + A = MathHelper.Clamp(a, 0f, 1f); + } + + /// + /// Builds a color from a byte array + /// + /// Byte array containing a 16 byte color + /// Beginning position in the byte array + public Color4(byte[] byteArray, int pos, bool inverted) + { + R = G = B = A = 0f; + FromBytes(byteArray, pos, inverted); + } + + /// + /// Returns the raw bytes for this vector + /// + /// Byte array containing a 16 byte color + /// Beginning position in the byte array + /// True if the byte array stores inverted values, + /// otherwise false. For example the color black (fully opaque) inverted + /// would be 0xFF 0xFF 0xFF 0x00 + /// True if the alpha value is inverted in + /// addition to whatever the inverted parameter is. Setting inverted true + /// and alphaInverted true will flip the alpha value back to non-inverted, + /// but keep the other color bytes inverted + /// A 16 byte array containing R, G, B, and A + public Color4(byte[] byteArray, int pos, bool inverted, bool alphaInverted) + { + R = G = B = A = 0f; + FromBytes(byteArray, pos, inverted, alphaInverted); + } + + /// + /// Copy constructor + /// + /// Color to copy + public Color4(Color4 color) + { + R = color.R; + G = color.G; + B = color.B; + A = color.A; + } + + #endregion Constructors + + #region Public Methods + + /// + /// IComparable.CompareTo implementation + /// + /// Sorting ends up like this: |--Grayscale--||--Color--|. + /// Alpha is only used when the colors are otherwise equivalent + public int CompareTo(Color4 color) + { + float thisHue = GetHue(); + float thatHue = color.GetHue(); + + if (thisHue < 0f && thatHue < 0f) + { + // Both monochromatic + if (R == color.R) + { + // Monochromatic and equal, compare alpha + return A.CompareTo(color.A); + } + else + { + // Compare lightness + return R.CompareTo(R); + } + } + else + { + if (thisHue == thatHue) + { + // RGB is equal, compare alpha + return A.CompareTo(color.A); + } + else + { + // Compare hues + return thisHue.CompareTo(thatHue); + } + } + } + + public void FromBytes(byte[] byteArray, int pos, bool inverted) + { + const float quanta = 1.0f / 255.0f; + + if (inverted) + { + R = (float)(255 - byteArray[pos]) * quanta; + G = (float)(255 - byteArray[pos + 1]) * quanta; + B = (float)(255 - byteArray[pos + 2]) * quanta; + A = (float)(255 - byteArray[pos + 3]) * quanta; + } + else + { + R = (float)byteArray[pos] * quanta; + G = (float)byteArray[pos + 1] * quanta; + B = (float)byteArray[pos + 2] * quanta; + A = (float)byteArray[pos + 3] * quanta; + } + } + + /// + /// Builds a color from a byte array + /// + /// Byte array containing a 16 byte color + /// Beginning position in the byte array + /// True if the byte array stores inverted values, + /// otherwise false. For example the color black (fully opaque) inverted + /// would be 0xFF 0xFF 0xFF 0x00 + /// True if the alpha value is inverted in + /// addition to whatever the inverted parameter is. Setting inverted true + /// and alphaInverted true will flip the alpha value back to non-inverted, + /// but keep the other color bytes inverted + public void FromBytes(byte[] byteArray, int pos, bool inverted, bool alphaInverted) + { + FromBytes(byteArray, pos, inverted); + + if (alphaInverted) + A = 1.0f - A; + } + + public byte[] GetBytes() + { + return GetBytes(false); + } + + /// + /// + /// + /// + public byte[] GetBytes(bool inverted) + { + byte[] byteArray = new byte[4]; + + byteArray[0] = Helpers.FloatToByte(R, 0f, 1f); + byteArray[1] = Helpers.FloatToByte(G, 0f, 1f); + byteArray[2] = Helpers.FloatToByte(B, 0f, 1f); + byteArray[3] = Helpers.FloatToByte(A, 0f, 1f); + + if (inverted) + { + byteArray[0] = (byte)(255 - byteArray[0]); + byteArray[1] = (byte)(255 - byteArray[1]); + byteArray[2] = (byte)(255 - byteArray[2]); + byteArray[3] = (byte)(255 - byteArray[3]); + } + + return byteArray; + } + + public byte[] GetFloatBytes() + { + byte[] bytes = new byte[16]; + Buffer.BlockCopy(BitConverter.GetBytes(R), 0, bytes, 0, 4); + Buffer.BlockCopy(BitConverter.GetBytes(G), 0, bytes, 4, 4); + Buffer.BlockCopy(BitConverter.GetBytes(B), 0, bytes, 8, 4); + Buffer.BlockCopy(BitConverter.GetBytes(A), 0, bytes, 12, 4); + return bytes; + } + + public float GetHue() + { + const float HUE_MAX = 360f; + + float max = Math.Max(Math.Max(R, G), B); + float min = Math.Min(Math.Min(R, B), B); + + if (max == min) + { + // Achromatic, hue is undefined + return -1f; + } + else + { + if (R == max) + { + float bDelta = (((max - B) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + float gDelta = (((max - G) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + return bDelta - gDelta; + } + else if (G == max) + { + float rDelta = (((max - R) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + float bDelta = (((max - B) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + return (HUE_MAX / 3f) + rDelta - bDelta; + } + else // B == max + { + float gDelta = (((max - G) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + float rDelta = (((max - R) * (HUE_MAX / 6f)) + ((max - min) / 2f)) / (max - min); + return ((2f * HUE_MAX) / 3f) + gDelta - rDelta; + } + } + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(R)); + array.Add(LLSD.FromReal(G)); + array.Add(LLSD.FromReal(B)); + array.Add(LLSD.FromReal(A)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 4) + { + R = (float)array[0].AsReal(); + G = (float)array[1].AsReal(); + B = (float)array[2].AsReal(); + A = (float)array[3].AsReal(); + + return; + } + } + + this = Color4.Black; + } + + #endregion Public Methods + + #region Static Methods + + #endregion Static Methods + + #region Overrides + + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}, {3}>", R, G, B, A); + } + + public string ToRGBString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}>", R, G, B); + } + + public override bool Equals(object obj) + { + return (obj is Color4) ? this == (Color4)obj : false; + } + + public bool Equals(Color4 other) + { + return this == other; + } + + public override int GetHashCode() + { + return R.GetHashCode() ^ G.GetHashCode() ^ B.GetHashCode() ^ A.GetHashCode(); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Color4 lhs, Color4 rhs) + { + return (lhs.R == rhs.R) && (lhs.G == rhs.G) && (lhs.B == rhs.B) && (lhs.A == rhs.A); + } + + public static bool operator !=(Color4 lhs, Color4 rhs) + { + return !(lhs == rhs); + } + + #endregion Operators + + /// A Color4 with zero RGB values and full alpha (1.0) + public readonly static Color4 Black = new Color4(0f, 0f, 0f, 1f); + + /// A Color4 with full RGB values (1.0) and full alpha (1.0) + public readonly static Color4 White = new Color4(1f, 1f, 1f, 1f); + } +} diff --git a/OpenMetaverse/Types/Matrix4.cs b/OpenMetaverse/Types/Matrix4.cs new file mode 100644 index 00000000..a65a0352 --- /dev/null +++ b/OpenMetaverse/Types/Matrix4.cs @@ -0,0 +1,1078 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; + +namespace OpenMetaverse +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 : IEquatable + { + public float M11, M12, M13, M14; + public float M21, M22, M23, M24; + public float M31, M32, M33, M34; + public float M41, M42, M43, M44; + + #region Properties + + public Vector3 AtAxis + { + get + { + return new Vector3(M11, M21, M31); + } + set + { + M11 = value.X; + M21 = value.Y; + M31 = value.Z; + } + } + + public Vector3 LeftAxis + { + get + { + return new Vector3(M12, M22, M32); + } + set + { + M12 = value.X; + M22 = value.Y; + M32 = value.Z; + } + } + + public Vector3 UpAxis + { + get + { + return new Vector3(M13, M23, M33); + } + set + { + M13 = value.X; + M23 = value.Y; + M33 = value.Z; + } + } + + #endregion Properties + + #region Constructors + + public Matrix4( + float m11, float m12, float m13, float m14, + float m21, float m22, float m23, float m24, + float m31, float m32, float m33, float m34, + float m41, float m42, float m43, float m44) + { + M11 = m11; + M12 = m12; + M13 = m13; + M14 = m14; + + M21 = m21; + M22 = m22; + M23 = m23; + M24 = m24; + + M31 = m31; + M32 = m32; + M33 = m33; + M34 = m34; + + M41 = m41; + M42 = m42; + M43 = m43; + M44 = m44; + } + + public Matrix4(float roll, float pitch, float yaw) + { + this = CreateFromEulers(roll, pitch, yaw); + } + + public Matrix4(Matrix4 m) + { + M11 = m.M11; + M12 = m.M12; + M13 = m.M13; + M14 = m.M14; + + M21 = m.M21; + M22 = m.M22; + M23 = m.M23; + M24 = m.M24; + + M31 = m.M31; + M32 = m.M32; + M33 = m.M33; + M34 = m.M34; + + M41 = m.M41; + M42 = m.M42; + M43 = m.M43; + M44 = m.M44; + } + + #endregion Constructors + + #region Public Methods + + public float Determinant() + { + return + M14 * M23 * M32 * M41 - M13 * M24 * M32 * M41 - M14 * M22 * M33 * M41 + M12 * M24 * M33 * M41 + + M13 * M22 * M34 * M41 - M12 * M23 * M34 * M41 - M14 * M23 * M31 * M42 + M13 * M24 * M31 * M42 + + M14 * M21 * M33 * M42 - M11 * M24 * M33 * M42 - M13 * M21 * M34 * M42 + M11 * M23 * M34 * M42 + + M14 * M22 * M31 * M43 - M12 * M24 * M31 * M43 - M14 * M21 * M32 * M43 + M11 * M24 * M32 * M43 + + M12 * M21 * M34 * M43 - M11 * M22 * M34 * M43 - M13 * M22 * M31 * M44 + M12 * M23 * M31 * M44 + + M13 * M21 * M32 * M44 - M11 * M23 * M32 * M44 - M12 * M21 * M33 * M44 + M11 * M22 * M33 * M44; + } + + public float Trace() + { + return M11 + M22 + M33 + M44; + } + + /// + /// Convert this matrix to euler rotations + /// + /// X euler angle + /// Y euler angle + /// Z euler angle + public void GetEulerAngles(out float roll, out float pitch, out float yaw) + { + double angleX, angleY, angleZ; + double cx, cy, cz; // cosines + double sx, sz; // sines + + angleY = Math.Asin(MathHelper.Clamp(M13, -1f, 1f)); + cy = Math.Cos(angleY); + + if (Math.Abs(cy) > 0.005f) + { + // No gimbal lock + cx = M33 / cy; + sx = (-M23) / cy; + + angleX = (float)Math.Atan2(sx, cx); + + cz = M11 / cy; + sz = (-M12) / cy; + + angleZ = (float)Math.Atan2(sz, cz); + } + else + { + // Gimbal lock + angleX = 0; + + cz = M22; + sz = M21; + + angleZ = Math.Atan2(sz, cz); + } + + // Return only positive angles in [0,360] + if (angleX < 0) angleX += 360d; + if (angleY < 0) angleY += 360d; + if (angleZ < 0) angleZ += 360d; + + roll = (float)angleX; + pitch = (float)angleY; + yaw = (float)angleZ; + } + + /// + /// Convert this matrix to a quaternion rotation + /// + /// A quaternion representation of this rotation matrix + public Quaternion GetQuaternion() + { + Quaternion quat = new Quaternion(); + float trace = Trace() + 1f; + + if (trace > Single.Epsilon) + { + float s = 0.5f / (float)Math.Sqrt(trace); + + quat.X = (M32 - M23) * s; + quat.Y = (M13 - M31) * s; + quat.Z = (M21 - M12) * s; + quat.W = 0.25f / s; + } + else + { + if (M11 > M22 && M11 > M33) + { + float s = 2.0f * (float)Math.Sqrt(1.0f + M11 - M22 - M33); + + quat.X = 0.25f * s; + quat.Y = (M12 + M21) / s; + quat.Z = (M13 + M31) / s; + quat.W = (M23 - M32) / s; + } + else if (M22 > M33) + { + float s = 2.0f * (float)Math.Sqrt(1.0f + M22 - M11 - M33); + + quat.X = (M12 + M21) / s; + quat.Y = 0.25f * s; + quat.Z = (M23 + M32) / s; + quat.W = (M13 - M31) / s; + } + else + { + float s = 2.0f * (float)Math.Sqrt(1.0f + M33 - M11 - M22); + + quat.X = (M13 + M31) / s; + quat.Y = (M23 + M32) / s; + quat.Z = 0.25f * s; + quat.W = (M12 - M21) / s; + } + } + + return quat; + } + + #endregion Public Methods + + #region Static Methods + + public static Matrix4 Add(Matrix4 matrix1, Matrix4 matrix2) + { + Matrix4 matrix; + matrix.M11 = matrix1.M11 + matrix2.M11; + matrix.M12 = matrix1.M12 + matrix2.M12; + matrix.M13 = matrix1.M13 + matrix2.M13; + matrix.M14 = matrix1.M14 + matrix2.M14; + + matrix.M21 = matrix1.M21 + matrix2.M21; + matrix.M22 = matrix1.M22 + matrix2.M22; + matrix.M23 = matrix1.M23 + matrix2.M23; + matrix.M24 = matrix1.M24 + matrix2.M24; + + matrix.M31 = matrix1.M31 + matrix2.M31; + matrix.M32 = matrix1.M32 + matrix2.M32; + matrix.M33 = matrix1.M33 + matrix2.M33; + matrix.M34 = matrix1.M34 + matrix2.M34; + + matrix.M41 = matrix1.M41 + matrix2.M41; + matrix.M42 = matrix1.M42 + matrix2.M42; + matrix.M43 = matrix1.M43 + matrix2.M43; + matrix.M44 = matrix1.M44 + matrix2.M44; + return matrix; + } + + public static Matrix4 CreateFromAxisAngle(Vector3 axis, float angle) + { + Matrix4 matrix = new Matrix4(); + + float x = axis.X; + float y = axis.Y; + float z = axis.Z; + float sin = (float)Math.Sin(angle); + float cos = (float)Math.Cos(angle); + float xx = x * x; + float yy = y * y; + float zz = z * z; + float xy = x * y; + float xz = x * z; + float yz = y * z; + + matrix.M11 = xx + (cos * (1f - xx)); + matrix.M12 = (xy - (cos * xy)) + (sin * z); + matrix.M13 = (xz - (cos * xz)) - (sin * y); + //matrix.M14 = 0f; + + matrix.M21 = (xy - (cos * xy)) - (sin * z); + matrix.M22 = yy + (cos * (1f - yy)); + matrix.M23 = (yz - (cos * yz)) + (sin * x); + //matrix.M24 = 0f; + + matrix.M31 = (xz - (cos * xz)) + (sin * y); + matrix.M32 = (yz - (cos * yz)) - (sin * x); + matrix.M33 = zz + (cos * (1f - zz)); + //matrix.M34 = 0f; + + //matrix.M41 = matrix.M42 = matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + /// + /// Construct a matrix from euler rotation values in radians + /// + /// X euler angle in radians + /// Y euler angle in radians + /// Z euler angle in radians + public static Matrix4 CreateFromEulers(float roll, float pitch, float yaw) + { + Matrix4 m; + + float a, b, c, d, e, f; + float ad, bd; + + a = (float)Math.Cos(roll); + b = (float)Math.Sin(roll); + c = (float)Math.Cos(pitch); + d = (float)Math.Sin(pitch); + e = (float)Math.Cos(yaw); + f = (float)Math.Sin(yaw); + + ad = a * d; + bd = b * d; + + m.M11 = c * e; + m.M12 = -c * f; + m.M13 = d; + m.M14 = 0f; + + m.M21 = bd * e + a * f; + m.M22 = -bd * f + a * e; + m.M23 = -b * c; + m.M24 = 0f; + + m.M31 = -ad * e + b * f; + m.M32 = ad * f + b * e; + m.M33 = a * c; + m.M34 = 0f; + + m.M41 = m.M42 = m.M43 = 0f; + m.M44 = 1f; + + return m; + } + + public static Matrix4 CreateFromQuaternion(Quaternion quaternion) + { + Matrix4 matrix; + + float xx = quaternion.X * quaternion.X; + float yy = quaternion.Y * quaternion.Y; + float zz = quaternion.Z * quaternion.Z; + float xy = quaternion.X * quaternion.Y; + float zw = quaternion.Z * quaternion.W; + float zx = quaternion.Z * quaternion.X; + float yw = quaternion.Y * quaternion.W; + float yz = quaternion.Y * quaternion.Z; + float xw = quaternion.X * quaternion.W; + + matrix.M11 = 1f - (2f * (yy + zz)); + matrix.M12 = 2f * (xy + zw); + matrix.M13 = 2f * (zx - yw); + matrix.M14 = 0f; + + matrix.M21 = 2f * (xy - zw); + matrix.M22 = 1f - (2f * (zz + xx)); + matrix.M23 = 2f * (yz + xw); + matrix.M24 = 0f; + + matrix.M31 = 2f * (zx + yw); + matrix.M32 = 2f * (yz - xw); + matrix.M33 = 1f - (2f * (yy + xx)); + matrix.M34 = 0f; + + matrix.M41 = matrix.M42 = matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector) + { + Matrix4 matrix; + + Vector3 z = Vector3.Normalize(cameraPosition - cameraTarget); + Vector3 x = Vector3.Normalize(Vector3.Cross(cameraUpVector, z)); + Vector3 y = Vector3.Cross(z, x); + + matrix.M11 = x.X; + matrix.M12 = y.X; + matrix.M13 = z.X; + matrix.M14 = 0f; + + matrix.M21 = x.Y; + matrix.M22 = y.Y; + matrix.M23 = z.Y; + matrix.M24 = 0f; + + matrix.M31 = x.Z; + matrix.M32 = y.Z; + matrix.M33 = z.Z; + matrix.M34 = 0f; + + matrix.M41 = -Vector3.Dot(x, cameraPosition); + matrix.M42 = -Vector3.Dot(y, cameraPosition); + matrix.M43 = -Vector3.Dot(z, cameraPosition); + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateRotationX(float radians) + { + Matrix4 matrix; + + float cos = (float)Math.Cos(radians); + float sin = (float)Math.Sin(radians); + + matrix.M11 = 1f; + matrix.M12 = 0f; + matrix.M13 = 0f; + matrix.M14 = 0f; + + matrix.M21 = 0f; + matrix.M22 = cos; + matrix.M23 = sin; + matrix.M24 = 0f; + + matrix.M31 = 0f; + matrix.M32 = -sin; + matrix.M33 = cos; + matrix.M34 = 0f; + + matrix.M41 = 0f; + matrix.M42 = 0f; + matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateRotationY(float radians) + { + Matrix4 matrix; + + float cos = (float)Math.Cos(radians); + float sin = (float)Math.Sin(radians); + + matrix.M11 = cos; + matrix.M12 = 0f; + matrix.M13 = -sin; + matrix.M14 = 0f; + + matrix.M21 = 0f; + matrix.M22 = 1f; + matrix.M23 = 0f; + matrix.M24 = 0f; + + matrix.M31 = sin; + matrix.M32 = 0f; + matrix.M33 = cos; + matrix.M34 = 0f; + + matrix.M41 = 0f; + matrix.M42 = 0f; + matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateRotationZ(float radians) + { + Matrix4 matrix; + + float cos = (float)Math.Cos(radians); + float sin = (float)Math.Sin(radians); + + matrix.M11 = cos; + matrix.M12 = sin; + matrix.M13 = 0f; + matrix.M14 = 0f; + + matrix.M21 = -sin; + matrix.M22 = cos; + matrix.M23 = 0f; + matrix.M24 = 0f; + + matrix.M31 = 0f; + matrix.M32 = 0f; + matrix.M33 = 1f; + matrix.M34 = 0f; + + matrix.M41 = 0f; + matrix.M42 = 0f; + matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateScale(Vector3 scale) + { + Matrix4 matrix; + + matrix.M11 = scale.X; + matrix.M12 = 0f; + matrix.M13 = 0f; + matrix.M14 = 0f; + + matrix.M21 = 0f; + matrix.M22 = scale.Y; + matrix.M23 = 0f; + matrix.M24 = 0f; + + matrix.M31 = 0f; + matrix.M32 = 0f; + matrix.M33 = scale.Z; + matrix.M34 = 0f; + + matrix.M41 = 0f; + matrix.M42 = 0f; + matrix.M43 = 0f; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateTranslation(Vector3 position) + { + Matrix4 matrix; + + matrix.M11 = 1f; + matrix.M12 = 0f; + matrix.M13 = 0f; + matrix.M14 = 0f; + + matrix.M21 = 0f; + matrix.M22 = 1f; + matrix.M23 = 0f; + matrix.M24 = 0f; + + matrix.M31 = 0f; + matrix.M32 = 0f; + matrix.M33 = 1f; + matrix.M34 = 0f; + + matrix.M41 = position.X; + matrix.M42 = position.Y; + matrix.M43 = position.Z; + matrix.M44 = 1f; + + return matrix; + } + + public static Matrix4 CreateWorld(Vector3 position, Vector3 forward, Vector3 up) + { + Matrix4 result; + + // Normalize forward vector + forward.Normalize(); + + // Calculate right vector + Vector3 right = Vector3.Cross(forward, up); + right.Normalize(); + + // Recalculate up vector + up = Vector3.Cross(right, forward); + up.Normalize(); + + result.M11 = right.X; + result.M12 = right.Y; + result.M13 = right.Z; + result.M14 = 0.0f; + + result.M21 = up.X; + result.M22 = up.Y; + result.M23 = up.Z; + result.M24 = 0.0f; + + result.M31 = -forward.X; + result.M32 = -forward.Y; + result.M33 = -forward.Z; + result.M34 = 0.0f; + + result.M41 = position.X; + result.M42 = position.Y; + result.M43 = position.Z; + result.M44 = 1.0f; + + return result; + } + + public static Matrix4 Divide(Matrix4 matrix1, Matrix4 matrix2) + { + Matrix4 matrix; + + matrix.M11 = matrix1.M11 / matrix2.M11; + matrix.M12 = matrix1.M12 / matrix2.M12; + matrix.M13 = matrix1.M13 / matrix2.M13; + matrix.M14 = matrix1.M14 / matrix2.M14; + + matrix.M21 = matrix1.M21 / matrix2.M21; + matrix.M22 = matrix1.M22 / matrix2.M22; + matrix.M23 = matrix1.M23 / matrix2.M23; + matrix.M24 = matrix1.M24 / matrix2.M24; + + matrix.M31 = matrix1.M31 / matrix2.M31; + matrix.M32 = matrix1.M32 / matrix2.M32; + matrix.M33 = matrix1.M33 / matrix2.M33; + matrix.M34 = matrix1.M34 / matrix2.M34; + + matrix.M41 = matrix1.M41 / matrix2.M41; + matrix.M42 = matrix1.M42 / matrix2.M42; + matrix.M43 = matrix1.M43 / matrix2.M43; + matrix.M44 = matrix1.M44 / matrix2.M44; + + return matrix; + } + + public static Matrix4 Divide(Matrix4 matrix1, float divider) + { + Matrix4 matrix; + + float oodivider = 1f / divider; + matrix.M11 = matrix1.M11 * oodivider; + matrix.M12 = matrix1.M12 * oodivider; + matrix.M13 = matrix1.M13 * oodivider; + matrix.M14 = matrix1.M14 * oodivider; + + matrix.M21 = matrix1.M21 * oodivider; + matrix.M22 = matrix1.M22 * oodivider; + matrix.M23 = matrix1.M23 * oodivider; + matrix.M24 = matrix1.M24 * oodivider; + + matrix.M31 = matrix1.M31 * oodivider; + matrix.M32 = matrix1.M32 * oodivider; + matrix.M33 = matrix1.M33 * oodivider; + matrix.M34 = matrix1.M34 * oodivider; + + matrix.M41 = matrix1.M41 * oodivider; + matrix.M42 = matrix1.M42 * oodivider; + matrix.M43 = matrix1.M43 * oodivider; + matrix.M44 = matrix1.M44 * oodivider; + + return matrix; + } + + public static Matrix4 Lerp(Matrix4 matrix1, Matrix4 matrix2, float amount) + { + Matrix4 matrix; + + matrix.M11 = matrix1.M11 + ((matrix2.M11 - matrix1.M11) * amount); + matrix.M12 = matrix1.M12 + ((matrix2.M12 - matrix1.M12) * amount); + matrix.M13 = matrix1.M13 + ((matrix2.M13 - matrix1.M13) * amount); + matrix.M14 = matrix1.M14 + ((matrix2.M14 - matrix1.M14) * amount); + + matrix.M21 = matrix1.M21 + ((matrix2.M21 - matrix1.M21) * amount); + matrix.M22 = matrix1.M22 + ((matrix2.M22 - matrix1.M22) * amount); + matrix.M23 = matrix1.M23 + ((matrix2.M23 - matrix1.M23) * amount); + matrix.M24 = matrix1.M24 + ((matrix2.M24 - matrix1.M24) * amount); + + matrix.M31 = matrix1.M31 + ((matrix2.M31 - matrix1.M31) * amount); + matrix.M32 = matrix1.M32 + ((matrix2.M32 - matrix1.M32) * amount); + matrix.M33 = matrix1.M33 + ((matrix2.M33 - matrix1.M33) * amount); + matrix.M34 = matrix1.M34 + ((matrix2.M34 - matrix1.M34) * amount); + + matrix.M41 = matrix1.M41 + ((matrix2.M41 - matrix1.M41) * amount); + matrix.M42 = matrix1.M42 + ((matrix2.M42 - matrix1.M42) * amount); + matrix.M43 = matrix1.M43 + ((matrix2.M43 - matrix1.M43) * amount); + matrix.M44 = matrix1.M44 + ((matrix2.M44 - matrix1.M44) * amount); + + return matrix; + } + + public static Matrix4 Multiply(Matrix4 matrix1, Matrix4 matrix2) + { + return new Matrix4( + matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31 + matrix1.M14 * matrix2.M41, + matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32 + matrix1.M14 * matrix2.M42, + matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33 + matrix1.M14 * matrix2.M43, + matrix1.M11 * matrix2.M14 + matrix1.M12 * matrix2.M24 + matrix1.M13 * matrix2.M34 + matrix1.M14 * matrix2.M44, + + matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31 + matrix1.M24 * matrix2.M41, + matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32 + matrix1.M24 * matrix2.M42, + matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33 + matrix1.M24 * matrix2.M43, + matrix1.M21 * matrix2.M14 + matrix1.M22 * matrix2.M24 + matrix1.M23 * matrix2.M34 + matrix1.M24 * matrix2.M44, + + matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31 + matrix1.M34 * matrix2.M41, + matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32 + matrix1.M34 * matrix2.M42, + matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33 + matrix1.M34 * matrix2.M43, + matrix1.M31 * matrix2.M14 + matrix1.M32 * matrix2.M24 + matrix1.M33 * matrix2.M34 + matrix1.M34 * matrix2.M44, + + matrix1.M41 * matrix2.M11 + matrix1.M42 * matrix2.M21 + matrix1.M43 * matrix2.M31 + matrix1.M44 * matrix2.M41, + matrix1.M41 * matrix2.M12 + matrix1.M42 * matrix2.M22 + matrix1.M43 * matrix2.M32 + matrix1.M44 * matrix2.M42, + matrix1.M41 * matrix2.M13 + matrix1.M42 * matrix2.M23 + matrix1.M43 * matrix2.M33 + matrix1.M44 * matrix2.M43, + matrix1.M41 * matrix2.M14 + matrix1.M42 * matrix2.M24 + matrix1.M43 * matrix2.M34 + matrix1.M44 * matrix2.M44 + ); + } + + public static Matrix4 Multiply(Matrix4 matrix1, float scaleFactor) + { + Matrix4 matrix; + matrix.M11 = matrix1.M11 * scaleFactor; + matrix.M12 = matrix1.M12 * scaleFactor; + matrix.M13 = matrix1.M13 * scaleFactor; + matrix.M14 = matrix1.M14 * scaleFactor; + + matrix.M21 = matrix1.M21 * scaleFactor; + matrix.M22 = matrix1.M22 * scaleFactor; + matrix.M23 = matrix1.M23 * scaleFactor; + matrix.M24 = matrix1.M24 * scaleFactor; + + matrix.M31 = matrix1.M31 * scaleFactor; + matrix.M32 = matrix1.M32 * scaleFactor; + matrix.M33 = matrix1.M33 * scaleFactor; + matrix.M34 = matrix1.M34 * scaleFactor; + + matrix.M41 = matrix1.M41 * scaleFactor; + matrix.M42 = matrix1.M42 * scaleFactor; + matrix.M43 = matrix1.M43 * scaleFactor; + matrix.M44 = matrix1.M44 * scaleFactor; + return matrix; + } + + public static Matrix4 Negate(Matrix4 matrix) + { + Matrix4 result; + result.M11 = -matrix.M11; + result.M12 = -matrix.M12; + result.M13 = -matrix.M13; + result.M14 = -matrix.M14; + + result.M21 = -matrix.M21; + result.M22 = -matrix.M22; + result.M23 = -matrix.M23; + result.M24 = -matrix.M24; + + result.M31 = -matrix.M31; + result.M32 = -matrix.M32; + result.M33 = -matrix.M33; + result.M34 = -matrix.M34; + + result.M41 = -matrix.M41; + result.M42 = -matrix.M42; + result.M43 = -matrix.M43; + result.M44 = -matrix.M44; + return result; + } + + public static Matrix4 Subtract(Matrix4 matrix1, Matrix4 matrix2) + { + Matrix4 matrix; + matrix.M11 = matrix1.M11 - matrix2.M11; + matrix.M12 = matrix1.M12 - matrix2.M12; + matrix.M13 = matrix1.M13 - matrix2.M13; + matrix.M14 = matrix1.M14 - matrix2.M14; + + matrix.M21 = matrix1.M21 - matrix2.M21; + matrix.M22 = matrix1.M22 - matrix2.M22; + matrix.M23 = matrix1.M23 - matrix2.M23; + matrix.M24 = matrix1.M24 - matrix2.M24; + + matrix.M31 = matrix1.M31 - matrix2.M31; + matrix.M32 = matrix1.M32 - matrix2.M32; + matrix.M33 = matrix1.M33 - matrix2.M33; + matrix.M34 = matrix1.M34 - matrix2.M34; + + matrix.M41 = matrix1.M41 - matrix2.M41; + matrix.M42 = matrix1.M42 - matrix2.M42; + matrix.M43 = matrix1.M43 - matrix2.M43; + matrix.M44 = matrix1.M44 - matrix2.M44; + return matrix; + } + + public static Matrix4 Transform(Matrix4 value, Quaternion rotation) + { + Matrix4 matrix; + + float x2 = rotation.X + rotation.X; + float y2 = rotation.Y + rotation.Y; + float z2 = rotation.Z + rotation.Z; + + float a = (1f - rotation.Y * y2) - rotation.Z * z2; + float b = rotation.X * y2 - rotation.W * z2; + float c = rotation.X * z2 + rotation.W * y2; + float d = rotation.X * y2 + rotation.W * z2; + float e = (1f - rotation.X * x2) - rotation.Z * z2; + float f = rotation.Y * z2 - rotation.W * x2; + float g = rotation.X * z2 - rotation.W * y2; + float h = rotation.Y * z2 + rotation.W * x2; + float i = (1f - rotation.X * x2) - rotation.Y * y2; + + matrix.M11 = ((value.M11 * a) + (value.M12 * b)) + (value.M13 * c); + matrix.M12 = ((value.M11 * d) + (value.M12 * e)) + (value.M13 * f); + matrix.M13 = ((value.M11 * g) + (value.M12 * h)) + (value.M13 * i); + matrix.M14 = value.M14; + + matrix.M21 = ((value.M21 * a) + (value.M22 * b)) + (value.M23 * c); + matrix.M22 = ((value.M21 * d) + (value.M22 * e)) + (value.M23 * f); + matrix.M23 = ((value.M21 * g) + (value.M22 * h)) + (value.M23 * i); + matrix.M24 = value.M24; + + matrix.M31 = ((value.M31 * a) + (value.M32 * b)) + (value.M33 * c); + matrix.M32 = ((value.M31 * d) + (value.M32 * e)) + (value.M33 * f); + matrix.M33 = ((value.M31 * g) + (value.M32 * h)) + (value.M33 * i); + matrix.M34 = value.M34; + + matrix.M41 = ((value.M41 * a) + (value.M42 * b)) + (value.M43 * c); + matrix.M42 = ((value.M41 * d) + (value.M42 * e)) + (value.M43 * f); + matrix.M43 = ((value.M41 * g) + (value.M42 * h)) + (value.M43 * i); + matrix.M44 = value.M44; + + return matrix; + } + + public static Matrix4 Transpose(Matrix4 matrix) + { + Matrix4 result; + + result.M11 = matrix.M11; + result.M12 = matrix.M21; + result.M13 = matrix.M31; + result.M14 = matrix.M41; + + result.M21 = matrix.M12; + result.M22 = matrix.M22; + result.M23 = matrix.M32; + result.M24 = matrix.M42; + + result.M31 = matrix.M13; + result.M32 = matrix.M23; + result.M33 = matrix.M33; + result.M34 = matrix.M43; + + result.M41 = matrix.M14; + result.M42 = matrix.M24; + result.M43 = matrix.M34; + result.M44 = matrix.M44; + + return result; + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Matrix4) ? this == (Matrix4)obj : false; + } + + public bool Equals(Matrix4 other) + { + return this == other; + } + + public override int GetHashCode() + { + return + M11.GetHashCode() ^ M12.GetHashCode() ^ M13.GetHashCode() ^ M14.GetHashCode() ^ + M21.GetHashCode() ^ M22.GetHashCode() ^ M23.GetHashCode() ^ M24.GetHashCode() ^ + M31.GetHashCode() ^ M32.GetHashCode() ^ M33.GetHashCode() ^ M34.GetHashCode() ^ + M41.GetHashCode() ^ M42.GetHashCode() ^ M43.GetHashCode() ^ M44.GetHashCode(); + } + + /// + /// Get a formatted string representation of the vector + /// + /// A string representation of the vector + public override string ToString() + { + return string.Format(Helpers.EnUsCulture, + "|{0}, {1}, {2}, {3}|\n|{4}, {5}, {6}, {7}|\n|{8}, {9}, {10}, {11}|\n|{12}, {13}, {14}, {15}|", + M11, M12, M13, M14, M21, M22, M23, M24, M31, M32, M33, M34, M41, M42, M43, M44); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Matrix4 left, Matrix4 right) + { + return left.Equals(right); + } + + public static bool operator !=(Matrix4 left, Matrix4 right) + { + return !left.Equals(right); + } + + public static Matrix4 operator +(Matrix4 left, Matrix4 right) + { + return Add(left, right); + } + + public static Matrix4 operator -(Matrix4 matrix) + { + return Negate(matrix); + } + + public static Matrix4 operator -(Matrix4 left, Matrix4 right) + { + return Subtract(left, right); + } + + public static Matrix4 operator *(Matrix4 left, Matrix4 right) + { + return Multiply(left, right); + } + + public static Matrix4 operator *(Matrix4 left, float scalar) + { + return Multiply(left, scalar); + } + + public static Matrix4 operator /(Matrix4 left, Matrix4 right) + { + return Divide(left, right); + } + + public static Matrix4 operator /(Matrix4 matrix, float divider) + { + return Divide(matrix, divider); + } + + public Vector4 this[int row] + { + get + { + switch (row) + { + case 0: + return new Vector4(M11, M12, M13, M14); + case 1: + return new Vector4(M21, M22, M23, M24); + case 2: + return new Vector4(M31, M32, M33, M34); + case 3: + return new Vector4(M41, M42, M43, M44); + default: + throw new IndexOutOfRangeException("Matrix4 row index must be from 0-3"); + } + } + set + { + switch (row) + { + case 0: + M11 = value.X; + M12 = value.Y; + M13 = value.Z; + M14 = value.W; + break; + case 1: + M21 = value.X; + M22 = value.Y; + M23 = value.Z; + M24 = value.W; + break; + case 2: + M31 = value.X; + M32 = value.Y; + M33 = value.Z; + M34 = value.W; + break; + case 3: + M41 = value.X; + M42 = value.Y; + M43 = value.Z; + M44 = value.W; + break; + default: + throw new IndexOutOfRangeException("Matrix4 row index must be from 0-3"); + } + } + } + + public float this[int row, int column] + { + get + { + switch (row) + { + case 0: + switch (column) + { + case 0: + return M11; + case 1: + return M12; + case 2: + return M13; + case 3: + return M14; + default: + throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3"); + } + case 1: + switch (column) + { + case 0: + return M21; + case 1: + return M22; + case 2: + return M23; + case 3: + return M24; + default: + throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3"); + } + case 2: + switch (column) + { + case 0: + return M31; + case 1: + return M32; + case 2: + return M33; + case 3: + return M34; + default: + throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3"); + } + case 3: + switch (column) + { + case 0: + return M41; + case 1: + return M42; + case 2: + return M43; + case 3: + return M44; + default: + throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3"); + } + default: + throw new IndexOutOfRangeException("Matrix4 row and column values must be from 0-3"); + } + } + } + + #endregion Operators + } +} diff --git a/OpenMetaverse/Types/Quaternion.cs b/OpenMetaverse/Types/Quaternion.cs new file mode 100644 index 00000000..a7a537fc --- /dev/null +++ b/OpenMetaverse/Types/Quaternion.cs @@ -0,0 +1,775 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using System.Globalization; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion : IEquatable + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + /// W value + public float W; + + // Used for little to big endian conversion on big endian architectures + private byte[] conversionBuffer; + + #region Constructors + + public Quaternion(float x, float y, float z, float w) + { + conversionBuffer = null; + X = x; + Y = y; + Z = z; + W = w; + } + + public Quaternion(Vector3 vectorPart, float scalarPart) + { + conversionBuffer = null; + X = vectorPart.X; + Y = vectorPart.Y; + Z = vectorPart.Z; + W = scalarPart; + } + + /// + /// Build a quaternion from normalized float values + /// + /// X value from -1.0 to 1.0 + /// Y value from -1.0 to 1.0 + /// Z value from -1.0 to 1.0 + public Quaternion(float x, float y, float z) + { + conversionBuffer = null; + X = x; + Y = y; + Z = z; + + float xyzsum = 1 - X * X - Y * Y - Z * Z; + W = (xyzsum > 0) ? (float)Math.Sqrt(xyzsum) : 0; + } + + /// + /// Constructor, builds a quaternion object from a byte array + /// + /// Byte array containing four four-byte floats + /// Offset in the byte array to start reading at + /// Whether the source data is normalized or + /// not. If this is true 12 bytes will be read, otherwise 16 bytes will + /// be read. + public Quaternion(byte[] byteArray, int pos, bool normalized) + { + conversionBuffer = null; + X = Y = Z = W = 0; + FromBytes(byteArray, pos, normalized); + } + + public Quaternion(Quaternion q) + { + conversionBuffer = null; + X = q.X; + Y = q.Y; + Z = q.Z; + W = q.W; + } + + #endregion Constructors + + #region Public Methods + + public float Length() + { + return (float)Math.Sqrt(W * W + X * X + Y * Y + Z * Z); + } + + public float LengthSquared() + { + return (W * W + X * X + Y * Y + Z * Z); + } + + /// + /// Normalizes the quaternion + /// + public void Normalize() + { + this = Normalize(this); + } + + /// + /// Builds a quaternion object from a byte array + /// + /// The source byte array + /// Offset in the byte array to start reading at + /// Whether the source data is normalized or + /// not. If this is true 12 bytes will be read, otherwise 16 bytes will + /// be read. + public void FromBytes(byte[] byteArray, int pos, bool normalized) + { + if (!normalized) + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[16]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 16); + + Array.Reverse(conversionBuffer, 0, 4); + Array.Reverse(conversionBuffer, 4, 4); + Array.Reverse(conversionBuffer, 8, 4); + Array.Reverse(conversionBuffer, 12, 4); + + X = BitConverter.ToSingle(conversionBuffer, 0); + Y = BitConverter.ToSingle(conversionBuffer, 4); + Z = BitConverter.ToSingle(conversionBuffer, 8); + W = BitConverter.ToSingle(conversionBuffer, 12); + } + else + { + // Little endian architecture + X = BitConverter.ToSingle(byteArray, pos); + Y = BitConverter.ToSingle(byteArray, pos + 4); + Z = BitConverter.ToSingle(byteArray, pos + 8); + W = BitConverter.ToSingle(byteArray, pos + 12); + } + } + else + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[16]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 12); + + Array.Reverse(conversionBuffer, 0, 4); + Array.Reverse(conversionBuffer, 4, 4); + Array.Reverse(conversionBuffer, 8, 4); + + X = BitConverter.ToSingle(conversionBuffer, 0); + Y = BitConverter.ToSingle(conversionBuffer, 4); + Z = BitConverter.ToSingle(conversionBuffer, 8); + } + else + { + // Little endian architecture + X = BitConverter.ToSingle(byteArray, pos); + Y = BitConverter.ToSingle(byteArray, pos + 4); + Z = BitConverter.ToSingle(byteArray, pos + 8); + } + + float xyzsum = 1f - X * X - Y * Y - Z * Z; + W = (xyzsum > 0f) ? (float)Math.Sqrt(xyzsum) : 0f; + } + } + + /// + /// Normalize this quaternion and serialize it to a byte array + /// + /// A 12 byte array containing normalized X, Y, and Z floating + /// point values in order using little endian byte ordering + public byte[] GetBytes() + { + byte[] bytes = new byte[12]; + float norm; + + norm = (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); + + if (norm != 0f) + { + norm = 1f / norm; + + float x, y, z; + if (W >= 0f) + { + x = X; y = Y; z = Z; + } + else + { + x = -X; y = -Y; z = -Z; + } + + Buffer.BlockCopy(BitConverter.GetBytes(norm * x), 0, bytes, 0, 4); + Buffer.BlockCopy(BitConverter.GetBytes(norm * y), 0, bytes, 4, 4); + Buffer.BlockCopy(BitConverter.GetBytes(norm * z), 0, bytes, 8, 4); + + if (!BitConverter.IsLittleEndian) + { + Array.Reverse(bytes, 0, 4); + Array.Reverse(bytes, 4, 4); + Array.Reverse(bytes, 8, 4); + } + } + else + { + throw new InvalidOperationException(String.Format( + "Quaternion {0} normalized to zero", ToString())); + } + + return bytes; + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(X)); + array.Add(LLSD.FromReal(Y)); + array.Add(LLSD.FromReal(Z)); + array.Add(LLSD.FromReal(W)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 4) + { + X = (float)array[0].AsReal(); + Y = (float)array[1].AsReal(); + Z = (float)array[2].AsReal(); + W = (float)array[3].AsReal(); + + return; + } + } + + this = Quaternion.Identity; + } + + /// + /// Convert this quaternion to euler angles + /// + /// X euler angle + /// Y euler angle + /// Z euler angle + public void GetEulerAngles(out float roll, out float pitch, out float yaw) + { + float sqx = X * X; + float sqy = Y * Y; + float sqz = Z * Z; + float sqw = W * W; + + // Unit will be a correction factor if the quaternion is not normalized + float unit = sqx + sqy + sqz + sqw; + double test = X * Y + Z * W; + + if (test > 0.499f * unit) + { + // Singularity at north pole + yaw = 2f * (float)Math.Atan2(X, W); + pitch = (float)Math.PI / 2f; + roll = 0f; + } + else if (test < -0.499f * unit) + { + // Singularity at south pole + yaw = -2f * (float)Math.Atan2(X, W); + pitch = -(float)Math.PI / 2f; + roll = 0f; + } + else + { + yaw = (float)Math.Atan2(2f * Y * W - 2f * X * Z, sqx - sqy - sqz + sqw); + pitch = (float)Math.Asin(2f * test / unit); + roll = (float)Math.Atan2(2f * X * W - 2f * Y * Z, -sqx + sqy - sqz + sqw); + } + } + + #endregion Public Methods + + #region Static Methods + + public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2) + { + quaternion1.X += quaternion2.X; + quaternion1.Y += quaternion2.Y; + quaternion1.Z += quaternion2.Z; + quaternion1.W += quaternion2.W; + return quaternion1; + } + + /// + /// Returns the conjugate (spatial inverse) of a quaternion + /// + private static Quaternion Conjugate(Quaternion quaternion) + { + quaternion.X = -quaternion.X; + quaternion.Y = -quaternion.Y; + quaternion.Z = -quaternion.Z; + return quaternion; + } + + /// + /// Build a quaternion from an axis and an angle of rotation around + /// that axis + /// + public static Quaternion CreateFromAxisAngle(float axisX, float axisY, float axisZ, float angle) + { + Vector3 axis = new Vector3(axisX, axisY, axisZ); + return CreateFromAxisAngle(axis, angle); + } + + /// + /// Build a quaternion from an axis and an angle of rotation around + /// that axis + /// + /// Axis of rotation + /// Angle of rotation + public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle) + { + Quaternion q; + axis = Vector3.Normalize(axis); + + angle *= 0.5f; + float c = (float)Math.Cos(angle); + float s = (float)Math.Sin(angle); + + q.conversionBuffer = null; + q.X = axis.X * s; + q.Y = axis.Y * s; + q.Z = axis.Z * s; + q.W = c; + + return Quaternion.Normalize(q); + } + + /// + /// Creates a quaternion from a vector containing roll, pitch, and yaw + /// in radians + /// + /// Vector representation of the euler angles in + /// radians + /// Quaternion representation of the euler angles + public static Quaternion CreateFromEulers(Vector3 eulers) + { + return CreateFromEulers(eulers.X, eulers.Y, eulers.Z); + } + + /// + /// Creates a quaternion from roll, pitch, and yaw euler angles in + /// radians + /// + /// X angle in radians + /// Y angle in radians + /// Z angle in radians + /// Quaternion representation of the euler angles + public static Quaternion CreateFromEulers(float roll, float pitch, float yaw) + { + if (roll > Helpers.TWO_PI || pitch > Helpers.TWO_PI || yaw > Helpers.TWO_PI) + throw new ArgumentException("Euler angles must be in radians"); + + double atCos = Math.Cos(roll / 2f); + double atSin = Math.Sin(roll / 2f); + double leftCos = Math.Cos(pitch / 2f); + double leftSin = Math.Sin(pitch / 2f); + double upCos = Math.Cos(yaw / 2f); + double upSin = Math.Sin(yaw / 2f); + double atLeftCos = atCos * leftCos; + double atLeftSin = atSin * leftSin; + return new Quaternion( + (float)(atSin * leftCos * upCos + atCos * leftSin * upSin), + (float)(atCos * leftSin * upCos - atSin * leftCos * upSin), + (float)(atLeftCos * upSin + atLeftSin * upCos), + (float)(atLeftCos * upCos - atLeftSin * upSin) + ); + } + + public static Quaternion CreateFromRotationMatrix(Matrix4 m) + { + Quaternion quat; + quat.conversionBuffer = null; + + float trace = m.Trace(); + + if (trace > Single.Epsilon) + { + float s = (float)Math.Sqrt(trace + 1f); + quat.W = s * 0.5f; + s = 0.5f / s; + quat.X = (m.M23 - m.M32) * s; + quat.Y = (m.M31 - m.M13) * s; + quat.Z = (m.M12 - m.M21) * s; + } + else + { + if (m.M11 > m.M22 && m.M11 > m.M33) + { + float s = (float)Math.Sqrt(1f + m.M11 - m.M22 - m.M33); + quat.X = 0.5f * s; + s = 0.5f / s; + quat.Y = (m.M12 + m.M21) * s; + quat.Z = (m.M13 + m.M31) * s; + quat.W = (m.M23 - m.M32) * s; + } + else if (m.M22 > m.M33) + { + float s = (float)Math.Sqrt(1f + m.M22 - m.M11 - m.M33); + quat.Y = 0.5f * s; + s = 0.5f / s; + quat.X = (m.M21 + m.M12) * s; + quat.Z = (m.M32 + m.M23) * s; + quat.W = (m.M31 - m.M13) * s; + } + else + { + float s = (float)Math.Sqrt(1f + m.M33 - m.M11 - m.M22); + quat.Z = 0.5f * s; + s = 0.5f / s; + quat.X = (m.M31 + m.M13) * s; + quat.Y = (m.M32 + m.M23) * s; + quat.W = (m.M12 - m.M21) * s; + } + } + + return quat; + } + + public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2) + { + float x = quaternion1.X; + float y = quaternion1.Y; + float z = quaternion1.Z; + float w = quaternion1.W; + + float q2lensq = quaternion2.LengthSquared(); //num14 + float ooq2lensq = 1f / q2lensq; + float x2 = -quaternion2.X * ooq2lensq; + float y2 = -quaternion2.Y * ooq2lensq; + float z2 = -quaternion2.Z * ooq2lensq; + float w2 = quaternion2.W * ooq2lensq; + + return new Quaternion( + ((x * w2) + (x2 * w)) + (y * z2) - (z * y2), + ((y * w2) + (y2 * w)) + (z * x2) - (x * z2), + ((z * w2) + (z2 * w)) + (x * y2) - (y * x2), + (w * w2) - ((x * x2) + (y * y2)) + (z * z2)); + } + + public static float Dot(Quaternion q1, Quaternion q2) + { + return (q1.X * q2.X) + (q1.Y * q2.Y) + (q1.Z * q2.Z) + (q1.W * q2.W); + } + + /// + /// Conjugates and renormalizes a vector + /// + public static Quaternion Inverse(Quaternion quaternion) + { + float norm = quaternion.LengthSquared(); + + if (norm == 0f) + { + quaternion.X = quaternion.Y = quaternion.Z = quaternion.W = 0f; + } + else + { + float oonorm = 1f / norm; + quaternion = Conjugate(quaternion); + + quaternion.X *= oonorm; + quaternion.Y *= oonorm; + quaternion.Z *= oonorm; + quaternion.W *= oonorm; + } + + return quaternion; + } + + /// + /// Spherical linear interpolation between two quaternions + /// + public static Quaternion Slerp(Quaternion q1, Quaternion q2, float amount) + { + float angle = Dot(q1, q2); + + if (angle < 0f) + { + q1 *= -1f; + angle *= -1f; + } + + float scale; + float invscale; + + if ((angle + 1f) > 0.05f) + { + if ((1f - angle) >= 0.05f) + { + // slerp + float theta = (float)Math.Acos(angle); + float invsintheta = 1f / (float)Math.Sin(theta); + scale = (float)Math.Sin(theta * (1f - amount)) * invsintheta; + invscale = (float)Math.Sin(theta * amount) * invsintheta; + } + else + { + // lerp + scale = 1f - amount; + invscale = amount; + } + } + else + { + q2.X = -q1.Y; + q2.Y = q1.X; + q2.Z = -q1.W; + q2.W = q1.Z; + + scale = (float)Math.Sin(MathHelper.PI * (0.5f - amount)); + invscale = (float)Math.Sin(MathHelper.PI * amount); + } + + return (q1 * scale) + (q2 * invscale); + } + + public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2) + { + quaternion1.X -= quaternion2.X; + quaternion1.Y -= quaternion2.Y; + quaternion1.Z -= quaternion2.Z; + quaternion1.W -= quaternion2.W; + return quaternion1; + } + + public static Quaternion Multiply(Quaternion q1, Quaternion q2) + { + return new Quaternion( + (q1.W * q2.X) + (q1.X * q2.W) + (q1.Y * q2.Z) - (q1.Z * q2.Y), + (q1.W * q2.Y) - (q1.X * q2.Z) + (q1.Y * q2.W) + (q1.Z * q2.X), + (q1.W * q2.Z) + (q1.X * q2.Y) - (q1.Y * q2.X) + (q1.Z * q2.W), + (q1.W * q2.W) - (q1.X * q2.X) - (q1.Y * q2.Y) - (q1.Z * q2.Z) + ); + } + + public static Quaternion Multiply(Quaternion quaternion, float scaleFactor) + { + quaternion.X *= scaleFactor; + quaternion.Y *= scaleFactor; + quaternion.Z *= scaleFactor; + quaternion.W *= scaleFactor; + return quaternion; + } + + public static Quaternion Negate(Quaternion quaternion) + { + quaternion.X = -quaternion.X; + quaternion.Y = -quaternion.Y; + quaternion.Z = -quaternion.Z; + quaternion.W = -quaternion.W; + return quaternion; + } + + public static Quaternion Normalize(Quaternion q) + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = q.Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1f / mag; + q.X *= oomag; + q.Y *= oomag; + q.Z *= oomag; + q.W *= oomag; + } + else + { + q.X = 0f; + q.Y = 0f; + q.Z = 0f; + q.W = 1f; + } + + return q; + } + + public static Quaternion Parse(string val) + { + char[] splitChar = { ',' }; + string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(splitChar); + if (split.Length == 3) + { + return new Quaternion( + float.Parse(split[0].Trim(), Helpers.EnUsCulture), + float.Parse(split[1].Trim(), Helpers.EnUsCulture), + float.Parse(split[2].Trim(), Helpers.EnUsCulture)); + } + else + { + return new Quaternion( + float.Parse(split[0].Trim(), Helpers.EnUsCulture), + float.Parse(split[1].Trim(), Helpers.EnUsCulture), + float.Parse(split[2].Trim(), Helpers.EnUsCulture), + float.Parse(split[3].Trim(), Helpers.EnUsCulture)); + } + } + + public static bool TryParse(string val, out Quaternion result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = new Quaternion(); + return false; + } + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Quaternion) ? this == (Quaternion)obj : false; + } + + public bool Equals(Quaternion other) + { + return W == other.W + && X == other.X + && Y == other.Y + && Z == other.Z; + } + + public override int GetHashCode() + { + return (X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode()); + } + + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}, {3}>", X, Y, Z, W); + } + + /// + /// Get a string representation of the quaternion elements with up to three + /// decimal digits and separated by spaces only + /// + /// Raw string representation of the quaternion + public string ToRawString() + { + CultureInfo enUs = new CultureInfo("en-us"); + enUs.NumberFormat.NumberDecimalDigits = 3; + + return String.Format(enUs, "{0} {1} {2} {3}", X, Y, Z, W); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2) + { + return quaternion1.W == quaternion2.W + && quaternion1.X == quaternion2.X + && quaternion1.Y == quaternion2.Y + && quaternion1.Z == quaternion2.Z; + } + + public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2) + { + return !(quaternion1 == quaternion2); + } + + public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2) + { + quaternion1.X += quaternion2.X; + quaternion1.Y += quaternion2.Y; + quaternion1.Z += quaternion2.Z; + quaternion1.W += quaternion2.W; + return quaternion1; + } + + public static Quaternion operator -(Quaternion quaternion) + { + quaternion.X = -quaternion.X; + quaternion.Y = -quaternion.Y; + quaternion.Z = -quaternion.Z; + quaternion.W = -quaternion.W; + return quaternion; + } + + public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2) + { + quaternion1.X -= quaternion2.X; + quaternion1.Y -= quaternion2.Y; + quaternion1.Z -= quaternion2.Z; + quaternion1.W -= quaternion2.W; + return quaternion1; + } + + public static Quaternion operator *(Quaternion q1, Quaternion q2) + { + return new Quaternion( + (q1.W * q2.X) + (q1.X * q2.W) + (q1.Y * q2.Z) - (q1.Z * q2.Y), + (q1.W * q2.Y) - (q1.X * q2.Z) + (q1.Y * q2.W) + (q1.Z * q2.X), + (q1.W * q2.Z) + (q1.X * q2.Y) - (q1.Y * q2.X) + (q1.Z * q2.W), + (q1.W * q2.W) - (q1.X * q2.X) - (q1.Y * q2.Y) - (q1.Z * q2.Z) + ); + } + + public static Quaternion operator *(Quaternion quaternion, float scaleFactor) + { + quaternion.X *= scaleFactor; + quaternion.Y *= scaleFactor; + quaternion.Z *= scaleFactor; + quaternion.W *= scaleFactor; + return quaternion; + } + + public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2) + { + return Divide(quaternion1, quaternion2); + } + + #endregion Operators + + /// A quaternion with a value of 0,0,0,1 + public readonly static Quaternion Identity = new Quaternion(0f, 0f, 0f, 1f); + } +} diff --git a/OpenMetaverse/Types/UUID.cs b/OpenMetaverse/Types/UUID.cs new file mode 100644 index 00000000..14a005ec --- /dev/null +++ b/OpenMetaverse/Types/UUID.cs @@ -0,0 +1,344 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; + +namespace OpenMetaverse +{ + /// + /// A 128-bit Universally Unique Identifier, used throughout the Second + /// Life networking protocol + /// + [Serializable] + public struct UUID : IComparable, IEquatable + { + /// The System.Guid object this struct wraps around + public Guid Guid; + + #region Constructors + + /// + /// Constructor that takes a string UUID representation + /// + /// A string representation of a UUID, case + /// insensitive and can either be hyphenated or non-hyphenated + /// UUID("11f8aa9c-b071-4242-836b-13b7abe0d489") + public UUID(string val) + { + if (String.IsNullOrEmpty(val)) + Guid = new Guid(); + else + Guid = new Guid(val); + } + + /// + /// Constructor that takes a System.Guid object + /// + /// A Guid object that contains the unique identifier + /// to be represented by this UUID + public UUID(Guid val) + { + Guid = val; + } + + /// + /// Constructor that takes a byte array containing a UUID + /// + /// Byte array containing a 16 byte UUID + /// Beginning offset in the array + public UUID(byte[] source, int pos) + { + Guid = UUID.Zero.Guid; + FromBytes(source, pos); + } + + /// + /// Constructor that takes an unsigned 64-bit unsigned integer to + /// convert to a UUID + /// + /// 64-bit unsigned integer to convert to a UUID + public UUID(ulong val) + { + Guid = new Guid(0, 0, 0, BitConverter.GetBytes(val)); + } + + /// + /// Copy constructor + /// + /// UUID to copy + public UUID(UUID val) + { + Guid = val.Guid; + } + + #endregion Constructors + + #region Public Methods + + /// + /// IComparable.CompareTo implementation + /// + public int CompareTo(UUID id) + { + return Guid.CompareTo(id.Guid); + } + + /// + /// + /// + /// + /// + public void FromBytes(byte[] source, int pos) + { + Guid = new Guid( + (source[pos + 0] << 24) | (source[pos + 1] << 16) | (source[pos + 2] << 8) | source[pos + 3], + (short)((source[pos + 4] << 8) | source[pos + 5]), + (short)((source[pos + 6] << 8) | source[pos + 7]), + source[pos + 8], source[pos + 9], source[pos + 10], source[pos + 11], + source[pos + 12], source[pos + 13], source[pos + 14], source[pos + 15]); + } + + /// + /// Returns a copy of the raw bytes for this UUID + /// + /// A 16 byte array containing this UUID + public byte[] GetBytes() + { + byte[] bytes = Guid.ToByteArray(); + byte[] output = new byte[16]; + output[0] = bytes[3]; + output[1] = bytes[2]; + output[2] = bytes[1]; + output[3] = bytes[0]; + output[4] = bytes[5]; + output[5] = bytes[4]; + output[6] = bytes[7]; + output[7] = bytes[6]; + Buffer.BlockCopy(bytes, 8, output, 8, 8); + + return output; + } + + /// + /// Calculate an LLCRC (cyclic redundancy check) for this UUID + /// + /// The CRC checksum for this UUID + public uint CRC() + { + uint retval = 0; + byte[] bytes = GetBytes(); + + retval += (uint)((bytes[3] << 24) + (bytes[2] << 16) + (bytes[1] << 8) + bytes[0]); + retval += (uint)((bytes[7] << 24) + (bytes[6] << 16) + (bytes[5] << 8) + bytes[4]); + retval += (uint)((bytes[11] << 24) + (bytes[10] << 16) + (bytes[9] << 8) + bytes[8]); + retval += (uint)((bytes[15] << 24) + (bytes[14] << 16) + (bytes[13] << 8) + bytes[12]); + + return retval; + } + + /// + /// Create a 64-bit integer representation of the first half of this UUID + /// + /// An integer created from the first eight bytes of this UUID + public ulong GetULong() + { + return Helpers.BytesToUInt64(Guid.ToByteArray()); + } + + #endregion Public Methods + + #region Static Methods + + /// + /// Generate a UUID from a string + /// + /// A string representation of a UUID, case + /// insensitive and can either be hyphenated or non-hyphenated + /// UUID.Parse("11f8aa9c-b071-4242-836b-13b7abe0d489") + public static UUID Parse(string val) + { + return new UUID(val); + } + + /// + /// Generate a UUID from a string + /// + /// A string representation of a UUID, case + /// insensitive and can either be hyphenated or non-hyphenated + /// Will contain the parsed UUID if successful, + /// otherwise null + /// True if the string was successfully parse, otherwise false + /// UUID.TryParse("11f8aa9c-b071-4242-836b-13b7abe0d489", result) + public static bool TryParse(string val, out UUID result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = UUID.Zero; + return false; + } + } + + /// + /// Combine two UUIDs together by taking the MD5 hash of a byte array + /// containing both UUIDs + /// + /// First UUID to combine + /// Second UUID to combine + /// The UUID product of the combination + public static UUID Combine(UUID first, UUID second) + { + // Construct the buffer that MD5ed + byte[] input = new byte[32]; + Buffer.BlockCopy(first.GetBytes(), 0, input, 0, 16); + Buffer.BlockCopy(second.GetBytes(), 0, input, 16, 16); + + return new UUID(Helpers.MD5Builder.ComputeHash(input), 0); + } + + /// + /// + /// + /// + public static UUID Random() + { + return new UUID(Guid.NewGuid()); + } + + #endregion Static Methods + + #region Overrides + + /// + /// Return a hash code for this UUID, used by .NET for hash tables + /// + /// An integer composed of all the UUID bytes XORed together + public override int GetHashCode() + { + return Guid.GetHashCode(); + } + + /// + /// Comparison function + /// + /// An object to compare to this UUID + /// True if the object is a UUID and both UUIDs are equal + public override bool Equals(object o) + { + if (!(o is UUID)) return false; + + UUID uuid = (UUID)o; + return Guid == uuid.Guid; + } + + /// + /// Comparison function + /// + /// UUID to compare to + /// True if the UUIDs are equal, otherwise false + public bool Equals(UUID uuid) + { + return Guid == uuid.Guid; + } + + /// + /// Get a hyphenated string representation of this UUID + /// + /// A string representation of this UUID, lowercase and + /// with hyphens + /// 11f8aa9c-b071-4242-836b-13b7abe0d489 + public override string ToString() + { + return Guid.ToString(); + } + + #endregion Overrides + + #region Operators + + /// + /// Equals operator + /// + /// First UUID for comparison + /// Second UUID for comparison + /// True if the UUIDs are byte for byte equal, otherwise false + public static bool operator ==(UUID lhs, UUID rhs) + { + return lhs.Guid == rhs.Guid; + } + + /// + /// Not equals operator + /// + /// First UUID for comparison + /// Second UUID for comparison + /// True if the UUIDs are not equal, otherwise true + public static bool operator !=(UUID lhs, UUID rhs) + { + return !(lhs == rhs); + } + + /// + /// XOR operator + /// + /// First UUID + /// Second UUID + /// A UUID that is a XOR combination of the two input UUIDs + public static UUID operator ^(UUID lhs, UUID rhs) + { + byte[] lhsbytes = lhs.GetBytes(); + byte[] rhsbytes = rhs.GetBytes(); + byte[] output = new byte[16]; + + for (int i = 0; i < 16; i++) + { + output[i] = (byte)(lhsbytes[i] ^ rhsbytes[i]); + } + + return new UUID(output, 0); + } + + /// + /// String typecasting operator + /// + /// A UUID in string form. Case insensitive, + /// hyphenated or non-hyphenated + /// A UUID built from the string representation + public static implicit operator UUID(string val) + { + return new UUID(val); + } + + #endregion Operators + + /// An UUID with a value of all zeroes + public static readonly UUID Zero = new UUID(); + } +} diff --git a/OpenMetaverse/Types/Vector2.cs b/OpenMetaverse/Types/Vector2.cs new file mode 100644 index 00000000..6067222e --- /dev/null +++ b/OpenMetaverse/Types/Vector2.cs @@ -0,0 +1,474 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using System.Globalization; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + /// + /// A two-dimensional vector with floating-point values + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector2 : IComparable, IEquatable + { + /// X value + public float X; + /// Y value + public float Y; + + // Used for little to big endian conversion on big endian architectures + private byte[] conversionBuffer; + + #region Constructors + + public Vector2(float x, float y) + { + conversionBuffer = null; + X = x; + Y = y; + } + + public Vector2(float value) + { + conversionBuffer = null; + X = value; + Y = value; + } + + public Vector2(Vector2 vector) + { + conversionBuffer = null; + X = vector.X; + Y = vector.Y; + } + + #endregion Constructors + + #region Public Methods + + /// + /// Test if this vector is equal to another vector, within a given + /// tolerance range + /// + /// Vector to test against + /// The acceptable magnitude of difference + /// between the two vectors + /// True if the magnitude of difference between the two vectors + /// is less than the given tolerance, otherwise false + public bool ApproxEquals(Vector2 vec, float tolerance) + { + Vector2 diff = this - vec; + return (diff.Length() <= tolerance); + } + + /// + /// Test if this vector is composed of all finite numbers + /// + public bool IsFinite() + { + return MathHelper.IsFinite(X) && MathHelper.IsFinite(Y); + } + + /// + /// IComparable.CompareTo implementation + /// + public int CompareTo(Vector2 vector) + { + return Length().CompareTo(vector.Length()); + } + + /// + /// Builds a vector from a byte array + /// + /// Byte array containing two four-byte floats + /// Beginning position in the byte array + public void FromBytes(byte[] byteArray, int pos) + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[8]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 8); + + Array.Reverse(conversionBuffer, 0, 4); + Array.Reverse(conversionBuffer, 4, 4); + + X = BitConverter.ToSingle(conversionBuffer, 0); + Y = BitConverter.ToSingle(conversionBuffer, 4); + } + else + { + // Little endian architecture + X = BitConverter.ToSingle(byteArray, pos); + Y = BitConverter.ToSingle(byteArray, pos + 4); + } + } + + /// + /// Returns the raw bytes for this vector + /// + /// An eight-byte array containing X and Y + public byte[] GetBytes() + { + byte[] byteArray = new byte[8]; + + Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 4); + Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 4, 4); + + if (!BitConverter.IsLittleEndian) + { + Array.Reverse(byteArray, 0, 4); + Array.Reverse(byteArray, 4, 4); + } + + return byteArray; + } + + public float Length() + { + return (float)Math.Sqrt(DistanceSquared(this, Zero)); + } + + public float LengthSquared() + { + return DistanceSquared(this, Zero); + } + + public void Normalize() + { + this = Normalize(this); + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(X)); + array.Add(LLSD.FromReal(Y)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 2) + { + X = (float)array[0].AsReal(); + Y = (float)array[1].AsReal(); + + return; + } + } + + this = Vector2.Zero; + } + + #endregion Public Methods + + #region Static Methods + + public static Vector2 Add(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + public static Vector2 Clamp(Vector2 value1, Vector2 min, Vector2 max) + { + return new Vector2( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y)); + } + + public static float Distance(Vector2 value1, Vector2 value2) + { + return (float)Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static float DistanceSquared(Vector2 value1, Vector2 value2) + { + return DistanceSquared(value1, value2); + } + + public static Vector2 Divide(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + public static Vector2 Divide(Vector2 value1, float divider) + { + float factor = 1 / divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + public static float Dot(Vector2 value1, Vector2 value2) + { + return value1.X * value2.X + value1.Y * value2.Y; + } + + public static Vector2 Lerp(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount)); + } + + public static Vector2 Max(Vector2 value1, Vector2 value2) + { + return new Vector2( + Math.Max(value1.X, value2.X), + Math.Max(value1.Y, value2.Y)); + } + + public static Vector2 Min(Vector2 value1, Vector2 value2) + { + return new Vector2( + Math.Min(value1.X, value2.X), + Math.Min(value1.Y, value2.Y)); + } + + public static Vector2 Multiply(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + public static Vector2 Multiply(Vector2 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + return value1; + } + + public static Vector2 Negate(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + public static Vector2 Normalize(Vector2 value) + { + float factor = DistanceSquared(value, Zero); + factor = 1f / (float)Math.Sqrt(factor); + value.X *= factor; + value.Y *= factor; + return value; + } + + /// + /// Parse a vector from a string + /// + /// A string representation of a 2D vector, enclosed + /// in arrow brackets and separated by commas + public static Vector3 Parse(string val) + { + char[] splitChar = { ',' }; + string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(splitChar); + return new Vector3( + float.Parse(split[0].Trim(), Helpers.EnUsCulture), + float.Parse(split[1].Trim(), Helpers.EnUsCulture), + float.Parse(split[2].Trim(), Helpers.EnUsCulture)); + } + + public static bool TryParse(string val, out Vector3 result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = Vector3.Zero; + return false; + } + } + + /// + /// Interpolates between two vectors using a cubic equation + /// + public static Vector2 SmoothStep(Vector2 value1, Vector2 value2, float amount) + { + return new Vector2( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount)); + } + + public static Vector2 Subtract(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + public static Vector2 Transform(Vector2 position, Matrix4 matrix) + { + position.X = (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41; + position.Y = (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42; + return position; + } + + public static Vector2 TransformNormal(Vector2 position, Matrix4 matrix) + { + position.X = (position.X * matrix.M11) + (position.Y * matrix.M21); + position.Y = (position.X * matrix.M12) + (position.Y * matrix.M22); + return position; + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Vector2) ? this == ((Vector2)obj) : false; + } + + public bool Equals(Vector2 other) + { + return this == other; + } + + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode(); + } + + /// + /// Get a formatted string representation of the vector + /// + /// A string representation of the vector + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}>", X, Y); + } + + /// + /// Get a string representation of the vector elements with up to three + /// decimal digits and separated by spaces only + /// + /// Raw string representation of the vector + public string ToRawString() + { + CultureInfo enUs = new CultureInfo("en-us"); + enUs.NumberFormat.NumberDecimalDigits = 3; + + return String.Format(enUs, "{0} {1}", X, Y); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Vector2 value1, Vector2 value2) + { + return value1.X == value2.X && value1.Y == value2.Y; + } + + public static bool operator !=(Vector2 value1, Vector2 value2) + { + return value1.X != value2.X || value1.Y != value2.Y; + } + + public static Vector2 operator +(Vector2 value1, Vector2 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + return value1; + } + + public static Vector2 operator -(Vector2 value) + { + value.X = -value.X; + value.Y = -value.Y; + return value; + } + + public static Vector2 operator -(Vector2 value1, Vector2 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + return value1; + } + + public static Vector2 operator *(Vector2 value1, Vector2 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + return value1; + } + + + public static Vector2 operator *(Vector2 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + return value; + } + + public static Vector2 operator /(Vector2 value1, Vector2 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + return value1; + } + + + public static Vector2 operator /(Vector2 value1, float divider) + { + float factor = 1 / divider; + value1.X *= factor; + value1.Y *= factor; + return value1; + } + + #endregion Operators + + /// A vector with a value of 0,0 + public readonly static Vector2 Zero = new Vector2(); + /// A vector with a value of 1,1 + public readonly static Vector2 One = new Vector2(1f, 1f); + /// A vector with a value of 1,0 + public readonly static Vector2 UnitX = new Vector2(1f, 0f); + /// A vector with a value of 0,1 + public readonly static Vector2 UnitY = new Vector2(0f, 1f); + } +} diff --git a/OpenMetaverse/Types/Vector3.cs b/OpenMetaverse/Types/Vector3.cs new file mode 100644 index 00000000..c61e7024 --- /dev/null +++ b/OpenMetaverse/Types/Vector3.cs @@ -0,0 +1,620 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using System.Globalization; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + /// + /// A three-dimensional vector with floating-point values + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 : IComparable, IEquatable + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + + // Used for little to big endian conversion on big endian architectures + private byte[] conversionBuffer; + + #region Constructors + + public Vector3(float x, float y, float z) + { + conversionBuffer = null; + X = x; + Y = y; + Z = z; + } + + public Vector3(float value) + { + conversionBuffer = null; + X = value; + Y = value; + Z = value; + } + + public Vector3(Vector2 value, float z) + { + conversionBuffer = null; + X = value.X; + Y = value.Y; + Z = z; + } + + public Vector3(Vector3d vector) + { + conversionBuffer = null; + X = (float)vector.X; + Y = (float)vector.Y; + Z = (float)vector.Z; + } + + /// + /// Constructor, builds a vector from a byte array + /// + /// Byte array containing three four-byte floats + /// Beginning position in the byte array + public Vector3(byte[] byteArray, int pos) + { + conversionBuffer = null; + X = Y = Z = 0f; + FromBytes(byteArray, pos); + } + + public Vector3(Vector3 vector) + { + conversionBuffer = null; + X = vector.X; + Y = vector.Y; + Z = vector.Z; + } + + #endregion Constructors + + #region Public Methods + + public float Length() + { + return (float)Math.Sqrt(DistanceSquared(this, Zero)); + } + + public float LengthSquared() + { + return DistanceSquared(this, Zero); + } + + public void Normalize() + { + this = Normalize(this); + } + + /// + /// Test if this vector is equal to another vector, within a given + /// tolerance range + /// + /// Vector to test against + /// The acceptable magnitude of difference + /// between the two vectors + /// True if the magnitude of difference between the two vectors + /// is less than the given tolerance, otherwise false + public bool ApproxEquals(Vector3 vec, float tolerance) + { + Vector3 diff = this - vec; + return (diff.Length() <= tolerance); + } + + /// + /// IComparable.CompareTo implementation + /// + public int CompareTo(Vector3 vector) + { + return Length().CompareTo(vector.Length()); + } + + /// + /// Test if this vector is composed of all finite numbers + /// + public bool IsFinite() + { + return (MathHelper.IsFinite(X) && MathHelper.IsFinite(Y) && MathHelper.IsFinite(Z)); + } + + /// + /// Builds a vector from a byte array + /// + /// Byte array containing a 12 byte vector + /// Beginning position in the byte array + public void FromBytes(byte[] byteArray, int pos) + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[12]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 12); + + Array.Reverse(conversionBuffer, 0, 4); + Array.Reverse(conversionBuffer, 4, 4); + Array.Reverse(conversionBuffer, 8, 4); + + X = BitConverter.ToSingle(conversionBuffer, 0); + Y = BitConverter.ToSingle(conversionBuffer, 4); + Z = BitConverter.ToSingle(conversionBuffer, 8); + } + else + { + // Little endian architecture + X = BitConverter.ToSingle(byteArray, pos); + Y = BitConverter.ToSingle(byteArray, pos + 4); + Z = BitConverter.ToSingle(byteArray, pos + 8); + } + } + + /// + /// Returns the raw bytes for this vector + /// + /// A 12 byte array containing X, Y, and Z + public byte[] GetBytes() + { + byte[] byteArray = new byte[12]; + + Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 4); + Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 4, 4); + Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, byteArray, 8, 4); + + if (!BitConverter.IsLittleEndian) + { + Array.Reverse(byteArray, 0, 4); + Array.Reverse(byteArray, 4, 4); + Array.Reverse(byteArray, 8, 4); + } + + return byteArray; + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(X)); + array.Add(LLSD.FromReal(Y)); + array.Add(LLSD.FromReal(Z)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 3) + { + X = (float)array[0].AsReal(); + Y = (float)array[1].AsReal(); + Z = (float)array[2].AsReal(); + + return; + } + } + + this = Vector3.Zero; + } + + #endregion Public Methods + + #region Static Methods + + public static Vector3 Add(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max) + { + return new Vector3( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z)); + } + + public static Vector3 Cross(Vector3 value1, Vector3 value2) + { + return new Vector3( + value1.Y * value2.Z - value2.Y * value1.Z, + value1.Z * value2.X - value2.Z * value1.X, + value1.X * value2.Y - value2.X * value1.Y); + } + + public static float Distance(Vector3 value1, Vector3 value2) + { + return (float)Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static float DistanceSquared(Vector3 value1, Vector3 value2) + { + return + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z); + } + + public static Vector3 Divide(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3 Divide(Vector3 value1, float value2) + { + float factor = 1f / value2; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + public static float Dot(Vector3 value1, Vector3 value2) + { + return value1.X * value2.X + value1.Y * value2.Y + value1.Z * value2.Z; + } + + public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount)); + } + + public static Vector3 Max(Vector3 value1, Vector3 value2) + { + return new Vector3( + Math.Max(value1.X, value2.X), + Math.Max(value1.Y, value2.Y), + Math.Max(value1.Z, value2.Z)); + } + + public static Vector3 Min(Vector3 value1, Vector3 value2) + { + return new Vector3( + Math.Min(value1.X, value2.X), + Math.Min(value1.Y, value2.Y), + Math.Min(value1.Z, value2.Z)); + } + + public static Vector3 Multiply(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3 Multiply(Vector3 value1, float scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector3 Negate(Vector3 value) + { + value.X = -value.X; + value.Y = -value.Y; + value.Z = -value.Z; + return value; + } + + public static Vector3 Normalize(Vector3 value) + { + float factor = Distance(value, Zero); + factor = 1f / factor; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + /// + /// Parse a vector from a string + /// + /// A string representation of a 3D vector, enclosed + /// in arrow brackets and separated by commas + public static Vector3 Parse(string val) + { + char[] splitChar = { ',' }; + string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(splitChar); + return new Vector3( + Single.Parse(split[0].Trim(), Helpers.EnUsCulture), + Single.Parse(split[1].Trim(), Helpers.EnUsCulture), + Single.Parse(split[2].Trim(), Helpers.EnUsCulture)); + } + + public static bool TryParse(string val, out Vector3 result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = Vector3.Zero; + return false; + } + } + + /// + /// Calculate the rotation between two vectors + /// + /// Normalized directional vector (such as 1,0,0 for forward facing) + /// Normalized target vector + public static Quaternion RotationBetween(Vector3 a, Vector3 b) + { + float dotProduct = Dot(a, b); + Vector3 crossProduct = Cross(a, b); + float magProduct = a.Length() * b.Length(); + double angle = Math.Acos(dotProduct / magProduct); + Vector3 axis = Normalize(crossProduct); + float s = (float)Math.Sin(angle / 2d); + + return new Quaternion( + axis.X * s, + axis.Y * s, + axis.Z * s, + (float)Math.Cos(angle / 2d)); + } + + /// + /// Interpolates between two vectors using a cubic equation + /// + public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount) + { + return new Vector3( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount)); + } + + public static Vector3 Subtract(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector3 Transform(Vector3 position, Matrix4 matrix) + { + return new Vector3( + (position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43); + } + + public static Vector3 TransformNormal(Vector3 position, Matrix4 matrix) + { + return new Vector3( + (position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31), + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32), + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33)); + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Vector3) ? this == (Vector3)obj : false; + } + + public bool Equals(Vector3 other) + { + return this == other; + } + + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode(); + } + + /// + /// Get a formatted string representation of the vector + /// + /// A string representation of the vector + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}>", X, Y, Z); + } + + /// + /// Get a string representation of the vector elements with up to three + /// decimal digits and separated by spaces only + /// + /// Raw string representation of the vector + public string ToRawString() + { + CultureInfo enUs = new CultureInfo("en-us"); + enUs.NumberFormat.NumberDecimalDigits = 3; + + return String.Format(enUs, "{0} {1} {2}", X, Y, Z); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Vector3 value1, Vector3 value2) + { + return value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vector3 value1, Vector3 value2) + { + return !(value1 == value2); + } + + public static Vector3 operator +(Vector3 value1, Vector3 value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector3 operator -(Vector3 value) + { + value.X = -value.X; + value.Y = -value.Y; + value.Z = -value.Z; + return value; + } + + public static Vector3 operator -(Vector3 value1, Vector3 value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector3 operator *(Vector3 value1, Vector3 value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3 operator *(Vector3 value, float scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + public static Vector3 operator *(Vector3 vec, Quaternion rot) + { + Vector3 vec2; + + vec2.X = + rot.W * rot.W * vec.X + + 2f * rot.Y * rot.W * vec.Z - + 2f * rot.Z * rot.W * vec.Y + + rot.X * rot.X * vec.X + + 2f * rot.Y * rot.X * vec.Y + + 2f * rot.Z * rot.X * vec.Z - + rot.Z * rot.Z * vec.X - + rot.Y * rot.Y * vec.X; + + vec2.Y = + 2f * rot.X * rot.Y * vec.X + + rot.Y * rot.Y * vec.Y + + 2f * rot.Z * rot.Y * vec.Z + + 2f * rot.W * rot.Z * vec.X - + rot.Z * rot.Z * vec.Y + + rot.W * rot.W * vec.Y - + 2f * rot.X * rot.W * vec.Z - + rot.X * rot.X * vec.Y; + + vec2.Z = + 2f * rot.X * rot.Z * vec.X + + 2f * rot.Y * rot.Z * vec.Y + + rot.Z * rot.Z * vec.Z - + 2f * rot.W * rot.Y * vec.X - + rot.Y * rot.Y * vec.Z + + 2f * rot.W * rot.X * vec.Y - + rot.X * rot.X * vec.Z + + rot.W * rot.W * vec.Z; + + return vec; + } + + public static Vector3 operator *(Vector3 vector, Matrix4 matrix) + { + return Transform(vector, matrix); + } + + public static Vector3 operator /(Vector3 value1, Vector3 value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3 operator /(Vector3 value, float divider) + { + float factor = 1f / divider; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + /// + /// Cross product between two vectors + /// + public static Vector3 operator %(Vector3 value1, Vector3 value2) + { + return Cross(value1, value2); + } + + #endregion Operators + + /// A vector with a value of 0,0,0 + public readonly static Vector3 Zero = new Vector3(); + /// A vector with a value of 1,1,1 + public readonly static Vector3 One = new Vector3(1f, 1f, 1f); + /// A unit vector facing forward (X axis), value 1,0,0 + public readonly static Vector3 UnitX = new Vector3(1f, 0f, 0f); + /// A unit vector facing left (Y axis), value 0,1,0 + public readonly static Vector3 UnitY = new Vector3(0f, 1f, 0f); + /// A unit vector facing up (Z axis), value 0,0,1 + public readonly static Vector3 UnitZ = new Vector3(0f, 0f, 1f); + } +} diff --git a/OpenMetaverse/Types/Vector3d.cs b/OpenMetaverse/Types/Vector3d.cs new file mode 100644 index 00000000..39f44c82 --- /dev/null +++ b/OpenMetaverse/Types/Vector3d.cs @@ -0,0 +1,533 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using System.Globalization; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + /// + /// A three-dimensional vector with doubleing-point values + /// + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector3d : IComparable, IEquatable + { + /// X value + public double X; + /// Y value + public double Y; + /// Z value + public double Z; + + // Used for little to big endian conversion on big endian architectures + private byte[] conversionBuffer; + + #region Constructors + + public Vector3d(double x, double y, double z) + { + conversionBuffer = null; + X = x; + Y = y; + Z = z; + } + + public Vector3d(double value) + { + conversionBuffer = null; + X = value; + Y = value; + Z = value; + } + + /// + /// Constructor, builds a vector from a byte array + /// + /// Byte array containing three eight-byte doubles + /// Beginning position in the byte array + public Vector3d(byte[] byteArray, int pos) + { + conversionBuffer = null; + X = Y = Z = 0d; + FromBytes(byteArray, pos); + } + + public Vector3d(Vector3 vector) + { + conversionBuffer = null; + X = vector.X; + Y = vector.Y; + Z = vector.Z; + } + + public Vector3d(Vector3d vector) + { + conversionBuffer = null; + X = vector.X; + Y = vector.Y; + Z = vector.Z; + } + + #endregion Constructors + + #region Public Methods + + public double Length() + { + return Math.Sqrt(DistanceSquared(this, Zero)); + } + + public double LengthSquared() + { + return DistanceSquared(this, Zero); + } + + public void Normalize() + { + this = Normalize(this); + } + + /// + /// Test if this vector is equal to another vector, within a given + /// tolerance range + /// + /// Vector to test against + /// The acceptable magnitude of difference + /// between the two vectors + /// True if the magnitude of difference between the two vectors + /// is less than the given tolerance, otherwise false + public bool ApproxEquals(Vector3d vec, double tolerance) + { + Vector3d diff = this - vec; + return (diff.Length() <= tolerance); + } + + /// + /// IComparable.CompareTo implementation + /// + public int CompareTo(Vector3d vector) + { + return this.Length().CompareTo(vector.Length()); + } + + /// + /// Test if this vector is composed of all finite numbers + /// + public bool IsFinite() + { + return (MathHelper.IsFinite(X) && MathHelper.IsFinite(Y) && MathHelper.IsFinite(Z)); + } + + /// + /// Builds a vector from a byte array + /// + /// Byte array containing a 24 byte vector + /// Beginning position in the byte array + public void FromBytes(byte[] byteArray, int pos) + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[24]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 24); + + Array.Reverse(conversionBuffer, 0, 8); + Array.Reverse(conversionBuffer, 8, 8); + Array.Reverse(conversionBuffer, 16, 8); + + X = BitConverter.ToDouble(conversionBuffer, 0); + Y = BitConverter.ToDouble(conversionBuffer, 8); + Z = BitConverter.ToDouble(conversionBuffer, 16); + } + else + { + // Little endian architecture + X = BitConverter.ToDouble(byteArray, pos); + Y = BitConverter.ToDouble(byteArray, pos + 8); + Z = BitConverter.ToDouble(byteArray, pos + 16); + } + } + + /// + /// Returns the raw bytes for this vector + /// + /// A 24 byte array containing X, Y, and Z + public byte[] GetBytes() + { + byte[] byteArray = new byte[24]; + + Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 8); + Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 8, 8); + Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, byteArray, 16, 8); + + if (!BitConverter.IsLittleEndian) + { + Array.Reverse(byteArray, 0, 8); + Array.Reverse(byteArray, 8, 8); + Array.Reverse(byteArray, 16, 8); + } + + return byteArray; + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(X)); + array.Add(LLSD.FromReal(Y)); + array.Add(LLSD.FromReal(Z)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 3) + { + X = array[0].AsReal(); + Y = array[1].AsReal(); + Z = array[2].AsReal(); + + return; + } + } + + this = Vector3d.Zero; + } + + #endregion Public Methods + + #region Static Methods + + public static Vector3d Add(Vector3d value1, Vector3d value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector3d Clamp(Vector3d value1, Vector3d min, Vector3d max) + { + return new Vector3d( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z)); + } + + public static Vector3d Cross(Vector3d value1, Vector3d value2) + { + return new Vector3d( + value1.Y * value2.Z - value2.Y * value1.Z, + value1.Z * value2.X - value2.Z * value1.X, + value1.X * value2.Y - value2.X * value1.Y); + } + + public static double Distance(Vector3d value1, Vector3d value2) + { + return Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static double DistanceSquared(Vector3d value1, Vector3d value2) + { + return + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z); + } + + public static Vector3d Divide(Vector3d value1, Vector3d value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3d Divide(Vector3d value1, double value2) + { + double factor = 1d / value2; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + public static double Dot(Vector3d value1, Vector3d value2) + { + return value1.X * value2.X + value1.Y * value2.Y + value1.Z * value2.Z; + } + + public static Vector3d Lerp(Vector3d value1, Vector3d value2, double amount) + { + return new Vector3d( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount)); + } + + public static Vector3d Max(Vector3d value1, Vector3d value2) + { + return new Vector3d( + Math.Max(value1.X, value2.X), + Math.Max(value1.Y, value2.Y), + Math.Max(value1.Z, value2.Z)); + } + + public static Vector3d Min(Vector3d value1, Vector3d value2) + { + return new Vector3d( + Math.Min(value1.X, value2.X), + Math.Min(value1.Y, value2.Y), + Math.Min(value1.Z, value2.Z)); + } + + public static Vector3d Multiply(Vector3d value1, Vector3d value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3d Multiply(Vector3d value1, double scaleFactor) + { + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector3d Negate(Vector3d value) + { + value.X = -value.X; + value.Y = -value.Y; + value.Z = -value.Z; + return value; + } + + public static Vector3d Normalize(Vector3d value) + { + double factor = Distance(value, Zero); + factor = 1d / factor; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + /// + /// Parse a vector from a string + /// + /// A string representation of a 3D vector, enclosed + /// in arrow brackets and separated by commas + public static Vector3d Parse(string val) + { + char[] splitChar = { ',' }; + string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(splitChar); + return new Vector3d( + Double.Parse(split[0].Trim(), Helpers.EnUsCulture), + Double.Parse(split[1].Trim(), Helpers.EnUsCulture), + Double.Parse(split[2].Trim(), Helpers.EnUsCulture)); + } + + public static bool TryParse(string val, out Vector3d result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = Vector3d.Zero; + return false; + } + } + + /// + /// Interpolates between two vectors using a cubic equation + /// + public static Vector3d SmoothStep(Vector3d value1, Vector3d value2, double amount) + { + return new Vector3d( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount)); + } + + public static Vector3d Subtract(Vector3d value1, Vector3d value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Vector3d) ? this == (Vector3d)obj : false; + } + + public bool Equals(Vector3d other) + { + return this == other; + } + + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode(); + } + + /// + /// Get a formatted string representation of the vector + /// + /// A string representation of the vector + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}>", X, Y, Z); + } + + /// + /// Get a string representation of the vector elements with up to three + /// decimal digits and separated by spaces only + /// + /// Raw string representation of the vector + public string ToRawString() + { + CultureInfo enUs = new CultureInfo("en-us"); + enUs.NumberFormat.NumberDecimalDigits = 3; + + return String.Format(enUs, "{0} {1} {2}", X, Y, Z); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Vector3d value1, Vector3d value2) + { + return value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vector3d value1, Vector3d value2) + { + return !(value1 == value2); + } + + public static Vector3d operator +(Vector3d value1, Vector3d value2) + { + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector3d operator -(Vector3d value) + { + value.X = -value.X; + value.Y = -value.Y; + value.Z = -value.Z; + return value; + } + + public static Vector3d operator -(Vector3d value1, Vector3d value2) + { + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector3d operator *(Vector3d value1, Vector3d value2) + { + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector3d operator *(Vector3d value, double scaleFactor) + { + value.X *= scaleFactor; + value.Y *= scaleFactor; + value.Z *= scaleFactor; + return value; + } + + public static Vector3d operator /(Vector3d value1, Vector3d value2) + { + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector3d operator /(Vector3d value, double divider) + { + double factor = 1d / divider; + value.X *= factor; + value.Y *= factor; + value.Z *= factor; + return value; + } + + /// + /// Cross product between two vectors + /// + public static Vector3d operator %(Vector3d value1, Vector3d value2) + { + return Cross(value1, value2); + } + + #endregion Operators + + /// A vector with a value of 0,0,0 + public readonly static Vector3d Zero = new Vector3d(); + /// A vector with a value of 1,1,1 + public readonly static Vector3d One = new Vector3d(); + /// A unit vector facing forward (X axis), value of 1,0,0 + public readonly static Vector3d UnitX = new Vector3d(1d, 0d, 0d); + /// A unit vector facing left (Y axis), value of 0,1,0 + public readonly static Vector3d UnitY = new Vector3d(0d, 1d, 0d); + /// A unit vector facing up (Z axis), value of 0,0,1 + public readonly static Vector3d UnitZ = new Vector3d(0d, 0d, 1d); + } +} diff --git a/OpenMetaverse/Types/Vector4.cs b/OpenMetaverse/Types/Vector4.cs new file mode 100644 index 00000000..9b1fc71e --- /dev/null +++ b/OpenMetaverse/Types/Vector4.cs @@ -0,0 +1,576 @@ +/* + * Copyright (c) 2008, openmetaverse.org + * All rights reserved. + * + * - Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * - Neither the name of the openmetaverse.org nor the names + * of its contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Runtime.InteropServices; +using System.Globalization; +using OpenMetaverse.StructuredData; + +namespace OpenMetaverse +{ + [Serializable] + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 : IComparable, IEquatable + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + /// W value + public float W; + + // Used for little to big endian conversion on big endian architectures + private byte[] conversionBuffer; + + #region Constructors + + public Vector4(float x, float y, float z, float w) + { + conversionBuffer = null; + X = x; + Y = y; + Z = z; + W = w; + } + + public Vector4(Vector2 value, float z, float w) + { + conversionBuffer = null; + X = value.X; + Y = value.Y; + Z = z; + W = w; + } + + public Vector4(Vector3 value, float w) + { + conversionBuffer = null; + X = value.X; + Y = value.Y; + Z = value.Z; + W = w; + } + + public Vector4(float value) + { + conversionBuffer = null; + X = value; + Y = value; + Z = value; + W = value; + } + + /// + /// Constructor, builds a vector from a byte array + /// + /// Byte array containing four four-byte floats + /// Beginning position in the byte array + public Vector4(byte[] byteArray, int pos) + { + conversionBuffer = null; + X = Y = Z = W = 0f; + FromBytes(byteArray, pos); + } + + public Vector4(Vector4 value) + { + conversionBuffer = null; + X = value.X; + Y = value.Y; + Z = value.Z; + W = value.W; + } + + #endregion Constructors + + #region Public Methods + + public float Length() + { + return (float)Math.Sqrt(DistanceSquared(this, Zero)); + } + + public float LengthSquared() + { + return DistanceSquared(this, Zero); + } + + public void Normalize() + { + this = Normalize(this); + } + + /// + /// Test if this vector is equal to another vector, within a given + /// tolerance range + /// + /// Vector to test against + /// The acceptable magnitude of difference + /// between the two vectors + /// True if the magnitude of difference between the two vectors + /// is less than the given tolerance, otherwise false + public bool ApproxEquals(Vector4 vec, float tolerance) + { + Vector4 diff = this - vec; + return (diff.Length() <= tolerance); + } + + /// + /// IComparable.CompareTo implementation + /// + public int CompareTo(Vector4 vector) + { + return Length().CompareTo(vector.Length()); + } + + /// + /// Test if this vector is composed of all finite numbers + /// + public bool IsFinite() + { + return (MathHelper.IsFinite(X) && MathHelper.IsFinite(Y) && MathHelper.IsFinite(Z) && MathHelper.IsFinite(W)); + } + + /// + /// Builds a vector from a byte array + /// + /// Byte array containing a 16 byte vector + /// Beginning position in the byte array + public void FromBytes(byte[] byteArray, int pos) + { + if (!BitConverter.IsLittleEndian) + { + // Big endian architecture + if (conversionBuffer == null) + conversionBuffer = new byte[16]; + + Buffer.BlockCopy(byteArray, pos, conversionBuffer, 0, 16); + + Array.Reverse(conversionBuffer, 0, 4); + Array.Reverse(conversionBuffer, 4, 4); + Array.Reverse(conversionBuffer, 8, 4); + Array.Reverse(conversionBuffer, 12, 4); + + X = BitConverter.ToSingle(conversionBuffer, 0); + Y = BitConverter.ToSingle(conversionBuffer, 4); + Z = BitConverter.ToSingle(conversionBuffer, 8); + W = BitConverter.ToSingle(conversionBuffer, 12); + } + else + { + // Little endian architecture + X = BitConverter.ToSingle(byteArray, pos); + Y = BitConverter.ToSingle(byteArray, pos + 4); + Z = BitConverter.ToSingle(byteArray, pos + 8); + W = BitConverter.ToSingle(byteArray, pos + 12); + } + } + + /// + /// Returns the raw bytes for this vector + /// + /// A 16 byte array containing X, Y, Z, and W + public byte[] GetBytes() + { + byte[] byteArray = new byte[16]; + + Buffer.BlockCopy(BitConverter.GetBytes(X), 0, byteArray, 0, 4); + Buffer.BlockCopy(BitConverter.GetBytes(Y), 0, byteArray, 4, 4); + Buffer.BlockCopy(BitConverter.GetBytes(Z), 0, byteArray, 8, 4); + Buffer.BlockCopy(BitConverter.GetBytes(W), 0, byteArray, 12, 4); + + if (!BitConverter.IsLittleEndian) + { + Array.Reverse(byteArray, 0, 4); + Array.Reverse(byteArray, 4, 4); + Array.Reverse(byteArray, 8, 4); + Array.Reverse(byteArray, 12, 4); + } + + return byteArray; + } + + public LLSD GetLLSD() + { + LLSDArray array = new LLSDArray(); + array.Add(LLSD.FromReal(X)); + array.Add(LLSD.FromReal(Y)); + array.Add(LLSD.FromReal(Z)); + array.Add(LLSD.FromReal(W)); + return array; + } + + public void FromLLSD(LLSD llsd) + { + if (llsd.Type == LLSDType.Array) + { + LLSDArray array = (LLSDArray)llsd; + + if (array.Count == 4) + { + X = (float)array[0].AsReal(); + Y = (float)array[1].AsReal(); + Z = (float)array[2].AsReal(); + W = (float)array[3].AsReal(); + + return; + } + } + + this = Vector4.Zero; + } + + #endregion Public Methods + + #region Static Methods + + public static Vector4 Add(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max) + { + return new Vector4( + MathHelper.Clamp(value1.X, min.X, max.X), + MathHelper.Clamp(value1.Y, min.Y, max.Y), + MathHelper.Clamp(value1.Z, min.Z, max.Z), + MathHelper.Clamp(value1.W, min.W, max.W)); + } + + public static float Distance(Vector4 value1, Vector4 value2) + { + return (float)Math.Sqrt(DistanceSquared(value1, value2)); + } + + public static float DistanceSquared(Vector4 value1, Vector4 value2) + { + return + (value1.W - value2.W) * (value1.W - value2.W) + + (value1.X - value2.X) * (value1.X - value2.X) + + (value1.Y - value2.Y) * (value1.Y - value2.Y) + + (value1.Z - value2.Z) * (value1.Z - value2.Z); + } + + public static Vector4 Divide(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector4 Divide(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + public static float Dot(Vector4 vector1, Vector4 vector2) + { + return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W; + } + + public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.Lerp(value1.X, value2.X, amount), + MathHelper.Lerp(value1.Y, value2.Y, amount), + MathHelper.Lerp(value1.Z, value2.Z, amount), + MathHelper.Lerp(value1.W, value2.W, amount)); + } + + public static Vector4 Max(Vector4 value1, Vector4 value2) + { + return new Vector4( + Math.Max(value1.X, value2.X), + Math.Max(value1.Y, value2.Y), + Math.Max(value1.Z, value2.Z), + Math.Max(value1.W, value2.W)); + } + + public static Vector4 Min(Vector4 value1, Vector4 value2) + { + return new Vector4( + Math.Min(value1.X, value2.X), + Math.Min(value1.Y, value2.Y), + Math.Min(value1.Z, value2.Z), + Math.Min(value1.W, value2.W)); + } + + public static Vector4 Multiply(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector4 Multiply(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 Negate(Vector4 value) + { + value.X = -value.X; + value.Y = -value.Y; + value.Z = -value.Z; + value.W = -value.W; + return value; + } + + public static Vector4 Normalize(Vector4 vector) + { + float factor = DistanceSquared(vector, Zero); + factor = 1f / (float)Math.Sqrt(factor); + + vector.W *= factor; + vector.X *= factor; + vector.Y *= factor; + vector.Z *= factor; + return vector; + } + + public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount) + { + return new Vector4( + MathHelper.SmoothStep(value1.X, value2.X, amount), + MathHelper.SmoothStep(value1.Y, value2.Y, amount), + MathHelper.SmoothStep(value1.Z, value2.Z, amount), + MathHelper.SmoothStep(value1.W, value2.W, amount)); + } + + public static Vector4 Subtract(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector4 Transform(Vector2 position, Matrix4 matrix) + { + return new Vector4( + (position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44); + } + + public static Vector4 Transform(Vector3 position, Matrix4 matrix) + { + return new Vector4( + (position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41, + (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42, + (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43, + (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44); + } + + public static Vector4 Transform(Vector4 vector, Matrix4 matrix) + { + return new Vector4( + (vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41), + (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42), + (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43), + (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44)); + } + + public static Vector4 Parse(string val) + { + char[] splitChar = { ',' }; + string[] split = val.Replace("<", String.Empty).Replace(">", String.Empty).Split(splitChar); + return new Vector4( + float.Parse(split[0].Trim(), Helpers.EnUsCulture), + float.Parse(split[1].Trim(), Helpers.EnUsCulture), + float.Parse(split[2].Trim(), Helpers.EnUsCulture), + float.Parse(split[3].Trim(), Helpers.EnUsCulture)); + } + + public static bool TryParse(string val, out Vector4 result) + { + try + { + result = Parse(val); + return true; + } + catch (Exception) + { + result = new Vector4(); + return false; + } + } + + #endregion Static Methods + + #region Overrides + + public override bool Equals(object obj) + { + return (obj is Vector4) ? this == (Vector4)obj : false; + } + + public bool Equals(Vector4 other) + { + return W == other.W + && X == other.X + && Y == other.Y + && Z == other.Z; + } + + public override int GetHashCode() + { + return X.GetHashCode() ^ Y.GetHashCode() ^ Z.GetHashCode() ^ W.GetHashCode(); + } + + public override string ToString() + { + return String.Format(Helpers.EnUsCulture, "<{0}, {1}, {2}, {3}>", X, Y, Z, W); + } + + /// + /// Get a string representation of the vector elements with up to three + /// decimal digits and separated by spaces only + /// + /// Raw string representation of the vector + public string ToRawString() + { + CultureInfo enUs = new CultureInfo("en-us"); + enUs.NumberFormat.NumberDecimalDigits = 3; + + return String.Format(enUs, "{0} {1} {2} {3}", X, Y, Z, W); + } + + #endregion Overrides + + #region Operators + + public static bool operator ==(Vector4 value1, Vector4 value2) + { + return value1.W == value2.W + && value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vector4 value1, Vector4 value2) + { + return !(value1 == value2); + } + + public static Vector4 operator +(Vector4 value1, Vector4 value2) + { + value1.W += value2.W; + value1.X += value2.X; + value1.Y += value2.Y; + value1.Z += value2.Z; + return value1; + } + + public static Vector4 operator -(Vector4 value) + { + return new Vector4(-value.X, -value.Y, -value.Z, -value.W); + } + + public static Vector4 operator -(Vector4 value1, Vector4 value2) + { + value1.W -= value2.W; + value1.X -= value2.X; + value1.Y -= value2.Y; + value1.Z -= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, Vector4 value2) + { + value1.W *= value2.W; + value1.X *= value2.X; + value1.Y *= value2.Y; + value1.Z *= value2.Z; + return value1; + } + + public static Vector4 operator *(Vector4 value1, float scaleFactor) + { + value1.W *= scaleFactor; + value1.X *= scaleFactor; + value1.Y *= scaleFactor; + value1.Z *= scaleFactor; + return value1; + } + + public static Vector4 operator /(Vector4 value1, Vector4 value2) + { + value1.W /= value2.W; + value1.X /= value2.X; + value1.Y /= value2.Y; + value1.Z /= value2.Z; + return value1; + } + + public static Vector4 operator /(Vector4 value1, float divider) + { + float factor = 1f / divider; + value1.W *= factor; + value1.X *= factor; + value1.Y *= factor; + value1.Z *= factor; + return value1; + } + + #endregion Operators + + /// A vector with a value of 0,0,0,0 + public readonly static Vector4 Zero = new Vector4(); + /// A vector with a value of 1,1,1,1 + public readonly static Vector4 One = new Vector4(1f, 1f, 1f, 1f); + /// A vector with a value of 1,0,0,0 + public readonly static Vector4 UnitX = new Vector4(1f, 0f, 0f, 0f); + /// A vector with a value of 0,1,0,0 + public readonly static Vector4 UnitY = new Vector4(0f, 1f, 0f, 0f); + /// A vector with a value of 0,0,1,0 + public readonly static Vector4 UnitZ = new Vector4(0f, 0f, 1f, 0f); + /// A vector with a value of 0,0,0,1 + public readonly static Vector4 UnitW = new Vector4(0f, 0f, 0f, 1f); + } +} diff --git a/Programs/PrimWorkshop/frmBrowser.cs b/Programs/PrimWorkshop/frmBrowser.cs index 42532675..9e8190be 100644 --- a/Programs/PrimWorkshop/frmBrowser.cs +++ b/Programs/PrimWorkshop/frmBrowser.cs @@ -1725,11 +1725,11 @@ StartRender: // Calculate the distance to move to/away float dist = (float)(e.Delta / 120) * 10.0f; - if (Vector3.Dist(Camera.Position, Camera.FocalPoint) > dist) + if (Vector3.Distance(Camera.Position, Camera.FocalPoint) > dist) { // Move closer or further away from the focal point Vector3 toFocal = Camera.FocalPoint - Camera.Position; - toFocal = Vector3.Norm(toFocal); + toFocal.Normalize(); toFocal = toFocal * dist; diff --git a/Programs/PrimWorkshop/frmPrimWorkshop.cs b/Programs/PrimWorkshop/frmPrimWorkshop.cs index c9b1b5d6..1d3fcd9e 100644 --- a/Programs/PrimWorkshop/frmPrimWorkshop.cs +++ b/Programs/PrimWorkshop/frmPrimWorkshop.cs @@ -146,7 +146,7 @@ namespace PrimWorkshop // Using euler angles because I have no clue what I'm doing float roll, pitch, yaw; - Matrix3 rotation = prim.Rotation.ToMatrix3(); + Matrix4 rotation = Matrix4.CreateFromQuaternion(prim.Rotation); rotation.GetEulerAngles(out roll, out pitch, out yaw); Gl.glRotatef(roll * 57.2957795f, 1f, 0f, 0f); diff --git a/Programs/examples/TestClient/Commands/Movement/FollowCommand.cs b/Programs/examples/TestClient/Commands/Movement/FollowCommand.cs index fb37d002..cb9053bc 100644 --- a/Programs/examples/TestClient/Commands/Movement/FollowCommand.cs +++ b/Programs/examples/TestClient/Commands/Movement/FollowCommand.cs @@ -124,7 +124,7 @@ namespace OpenMetaverse.TestClient if (Client.Network.Simulators[i] == Client.Network.CurrentSim) { - distance = Vector3.Dist(targetAv.Position, Client.Self.SimPosition); + distance = Vector3.Distance(targetAv.Position, Client.Self.SimPosition); } else { diff --git a/Programs/examples/TestClient/Commands/Movement/SitCommand.cs b/Programs/examples/TestClient/Commands/Movement/SitCommand.cs index bed676af..cae0926b 100644 --- a/Programs/examples/TestClient/Commands/Movement/SitCommand.cs +++ b/Programs/examples/TestClient/Commands/Movement/SitCommand.cs @@ -23,7 +23,7 @@ namespace OpenMetaverse.TestClient Client.Network.CurrentSim.ObjectsPrimitives.ForEach( delegate(Primitive prim) { - float distance = Vector3.Dist(Client.Self.SimPosition, prim.Position); + float distance = Vector3.Distance(Client.Self.SimPosition, prim.Position); if (closest == null || distance < closestDistance) { diff --git a/Programs/examples/TestClient/Commands/Prims/ExportParticlesCommand.cs b/Programs/examples/TestClient/Commands/Prims/ExportParticlesCommand.cs index e52ee1b5..68ea0ed1 100644 --- a/Programs/examples/TestClient/Commands/Prims/ExportParticlesCommand.cs +++ b/Programs/examples/TestClient/Commands/Prims/ExportParticlesCommand.cs @@ -88,8 +88,8 @@ namespace OpenMetaverse.TestClient lsl.Append(" PSYS_PART_START_ALPHA, " + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartStartColor.A) + "," + Environment.NewLine); lsl.Append(" PSYS_PART_END_ALPHA, " + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartEndColor.A) + "," + Environment.NewLine); - lsl.Append(" PSYS_PART_START_COLOR, " + exportPrim.ParticleSys.PartStartColor.ToStringRGB() + "," + Environment.NewLine); - lsl.Append(" PSYS_PART_END_COLOR, " + exportPrim.ParticleSys.PartEndColor.ToStringRGB() + "," + Environment.NewLine); + lsl.Append(" PSYS_PART_START_COLOR, " + exportPrim.ParticleSys.PartStartColor.ToRGBString() + "," + Environment.NewLine); + lsl.Append(" PSYS_PART_END_COLOR, " + exportPrim.ParticleSys.PartEndColor.ToRGBString() + "," + Environment.NewLine); lsl.Append(" PSYS_PART_START_SCALE, <" + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartStartScaleX) + ", " + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartStartScaleY) + ", 0>, " + Environment.NewLine); lsl.Append(" PSYS_PART_END_SCALE, <" + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartEndScaleX) + ", " + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartEndScaleY) + ", 0>, " + Environment.NewLine); lsl.Append(" PSYS_PART_MAX_AGE, " + String.Format("{0:0.00000}", exportPrim.ParticleSys.PartMaxAge) + "," + Environment.NewLine); diff --git a/Programs/examples/TestClient/Commands/Prims/FindObjectsCommand.cs b/Programs/examples/TestClient/Commands/Prims/FindObjectsCommand.cs index e6f22ad1..4ea85b78 100644 --- a/Programs/examples/TestClient/Commands/Prims/FindObjectsCommand.cs +++ b/Programs/examples/TestClient/Commands/Prims/FindObjectsCommand.cs @@ -37,7 +37,7 @@ namespace OpenMetaverse.TestClient List prims = Client.Network.CurrentSim.ObjectsPrimitives.FindAll( delegate(Primitive prim) { Vector3 pos = prim.Position; - return ((prim.ParentID == 0) && (pos != Vector3.Zero) && (Vector3.Dist(pos, location) < radius)); + return ((prim.ParentID == 0) && (pos != Vector3.Zero) && (Vector3.Distance(pos, location) < radius)); } );