diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs index ea373013eb..caadac6c93 100644 --- a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs @@ -26,192 +26,81 @@ */ using System; -using System.Collections.Generic; -using System.Diagnostics; using System.Globalization; +using System.Runtime.InteropServices; +using System.Runtime.CompilerServices; using OpenMetaverse; -using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModule.ubODEMeshing; -public class Vertex : IComparable +[StructLayout(LayoutKind.Sequential)] +public class Vertex : IComparable, IEquatable { - Vector3 vector; - - public float X - { - get { return vector.X; } - set { vector.X = value; } - } - - public float Y - { - get { return vector.Y; } - set { vector.Y = value; } - } - - public float Z - { - get { return vector.Z; } - set { vector.Z = value; } - } + public float X; + public float Y; + public float Z; + [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vertex(float x, float y, float z) { - vector.X = x; - vector.Y = y; - vector.Z = z; - } - - public Vertex normalize() - { - float tlength = vector.Length(); - if (tlength != 0f) - { - float mul = 1.0f / tlength; - return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul); - } - else - { - return new Vertex(0f, 0f, 0f); - } - } - - public Vertex cross(Vertex v) - { - return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X); - } - - // disable warning: mono compiler moans about overloading - // operators hiding base operator but should not according to C# - // language spec -#pragma warning disable 0108 - public static Vertex operator *(Vertex v, Quaternion q) - { - // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ - - Vertex v2 = new Vertex(0f, 0f, 0f); - - v2.X = q.W * q.W * v.X + - 2f * q.Y * q.W * v.Z - - 2f * q.Z * q.W * v.Y + - q.X * q.X * v.X + - 2f * q.Y * q.X * v.Y + - 2f * q.Z * q.X * v.Z - - q.Z * q.Z * v.X - - q.Y * q.Y * v.X; - - v2.Y = - 2f * q.X * q.Y * v.X + - q.Y * q.Y * v.Y + - 2f * q.Z * q.Y * v.Z + - 2f * q.W * q.Z * v.X - - q.Z * q.Z * v.Y + - q.W * q.W * v.Y - - 2f * q.X * q.W * v.Z - - q.X * q.X * v.Y; - - v2.Z = - 2f * q.X * q.Z * v.X + - 2f * q.Y * q.Z * v.Y + - q.Z * q.Z * v.Z - - 2f * q.W * q.Y * v.X - - q.Y * q.Y * v.Z + - 2f * q.W * q.X * v.Y - - q.X * q.X * v.Z + - q.W * q.W * v.Z; - - return v2; - } - - public static Vertex operator +(Vertex v1, Vertex v2) - { - return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); - } - - public static Vertex operator -(Vertex v1, Vertex v2) - { - return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); - } - - public static Vertex operator *(Vertex v1, Vertex v2) - { - return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z); - } - - public static Vertex operator +(Vertex v1, float am) - { - v1.X += am; - v1.Y += am; - v1.Z += am; - return v1; - } - - public static Vertex operator -(Vertex v1, float am) - { - v1.X -= am; - v1.Y -= am; - v1.Z -= am; - return v1; - } - - public static Vertex operator *(Vertex v1, float am) - { - v1.X *= am; - v1.Y *= am; - v1.Z *= am; - return v1; - } - - public static Vertex operator /(Vertex v1, float am) - { - if (am == 0f) - { - return new Vertex(0f,0f,0f); - } - float mul = 1.0f / am; - v1.X *= mul; - v1.Y *= mul; - v1.Z *= mul; - return v1; - } -#pragma warning restore 0108 - - - public float dot(Vertex v) - { - return X * v.X + Y * v.Y + Z * v.Z; + X = x; + Y = y; + Z = z; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vertex(Vector3 v) { - vector = v; + X = v.X; + Y = v.Y; + Z = v.Z; } + [MethodImpl(MethodImplOptions.AggressiveInlining)] public Vertex Clone() { return new Vertex(X, Y, Z); } - public static Vertex FromAngle(double angle) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public override int GetHashCode() { - return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f); + int hash = X.GetHashCode(); + hash = Utils.CombineHash(hash, Y.GetHashCode()); + hash = Utils.CombineHash(hash, Z.GetHashCode()); + return hash; } - public float Length() + public override bool Equals(object obj) { - return vector.Length(); + return (obj is Vertex) ? this == (Vertex)obj : false; } - public virtual bool Equals(Vertex v, float tolerance) + public bool Equals(Vertex other) { - Vertex diff = this - v; - float d = diff.Length(); - if (d < tolerance) - return true; - - return false; + return this == other; } + public bool Equals(Vertex v, float tolerance) + { + float x = X - v.X; + float y = Y - v.Y; + float z = Z - v.Z; + + double d = x * x + y * y + z * z; + double t = tolerance * tolerance; + return d < t; + } + + public static bool operator ==(Vertex value1, Vertex value2) + { + return value1.X == value2.X + && value1.Y == value2.Y + && value1.Z == value2.Z; + } + + public static bool operator !=(Vertex value1, Vertex value2) + { + return !(value1 == value2); + } public int CompareTo(Vertex other) { @@ -236,16 +125,6 @@ public class Vertex : IComparable return 0; } - public static bool operator >(Vertex me, Vertex other) - { - return me.CompareTo(other) > 0; - } - - public static bool operator <(Vertex me, Vertex other) - { - return me.CompareTo(other) < 0; - } - public String ToRaw() { // Why this stuff with the number formatter? @@ -299,37 +178,6 @@ public class Triangle return s1 + ";" + s2 + ";" + s3; } - public Vector3 getNormal() - { - // Vertices - - // Vectors for edges - Vector3 e1; - Vector3 e2; - - e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); - e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z); - - // Cross product for normal - Vector3 n = Vector3.Cross(e1, e2); - - // Length - float l = n.Length(); - - // Normalized "normal" - n = n/l; - - return n; - } - - public void invertNormal() - { - Vertex vt; - vt = v1; - v1 = v2; - v2 = vt; - } - // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and // debugging purposes public String ToStringRaw() diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs index 380cce2ef3..9792b06c9a 100644 --- a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs @@ -39,24 +39,6 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing { public class MeshBuildingData { - private class vertexcomp : IEqualityComparer - { - public bool Equals(Vertex v1, Vertex v2) - { - if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) - return true; - else - return false; - } - public int GetHashCode(Vertex v) - { - int a = v.X.GetHashCode(); - int b = v.Y.GetHashCode(); - int c = v.Z.GetHashCode(); - return (a << 16) ^ (b << 8) ^ c; - } - } - public Dictionary m_vertices; public List m_triangles; public float m_obbXmin; @@ -70,8 +52,7 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing public MeshBuildingData() { - vertexcomp vcomp = new vertexcomp(); - m_vertices = new Dictionary(vcomp); + m_vertices = new Dictionary(); m_triangles = new List(); m_centroid = Vector3.Zero; m_centroidDiv = 0; @@ -117,7 +98,7 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing m_obboffset = Vector3.Zero; } - public Mesh Scale(Vector3 scale) + public unsafe Mesh Scale(Vector3 scale) { if (m_verticesPtr == null || m_indicesPtr == null) return null; @@ -149,15 +130,21 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing result.m_obboffset.Z = m_obboffset.Z * z; result.vertices = new float[vertices.Length]; - int j = 0; - for (int i = 0; i < m_vertexCount; i++) + + fixed(float* dsts = result.vertices, srcs = vertices) { - result.vertices[j] = vertices[j] * x; - j++; - result.vertices[j] = vertices[j] * y; - j++; - result.vertices[j] = vertices[j] * z; - j++; + float* dst = dsts; + float* src = srcs; + float* end = srcs + vertices.Length; + while (src < end) + { + *dst = *src * x; + dst++; src++; + *dst = *src * y; + dst++; src++; + *dst = *src * z; + dst++; src++; + } } result.indexes = new int[indexes.Length]; diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs index 328afb1b26..8660407d83 100644 --- a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs @@ -172,54 +172,77 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing /// Size of entire object /// /// - private void AddSubMesh(OSDMap subMeshData, List coords, List faces) + private unsafe void AddSubMesh(OSDMap subMeshData, List coords, List faces) { // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap)); // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no // geometry for this submesh. - if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"])) + if (subMeshData.ContainsKey("NoGeometry")) return; - OpenMetaverse.Vector3 posMax; - OpenMetaverse.Vector3 posMin; - if (subMeshData.ContainsKey("PositionDomain")) + byte[] posBytes = subMeshData["Position"].AsBinary(); + if (posBytes == null || posBytes.Length == 0) + return; + byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); + if (triangleBytes == null || triangleBytes.Length == 0) + return; + + const float invMaxU16 = 1.0f / 65535f; + Vector3 posRange; + Vector3 posMin; + if(subMeshData.TryGetValue("PositionDomain", out OSD tmp)) { - posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); - posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + posRange = ((OSDMap)tmp)["Max"].AsVector3(); + posMin = ((OSDMap)tmp)["Min"].AsVector3(); + posRange = posRange - posMin; + posRange *= invMaxU16; } else { - posMax = new Vector3(0.5f, 0.5f, 0.5f); + posRange = new Vector3(invMaxU16, invMaxU16, invMaxU16); posMin = new Vector3(-0.5f, -0.5f, -0.5f); } - ushort faceIndexOffset = (ushort)coords.Count; + int faceIndexOffset = coords.Count; - byte[] posBytes = subMeshData["Position"].AsBinary(); - for (int i = 0; i < posBytes.Length; i += 6) - { - ushort uX = Utils.BytesToUInt16(posBytes, i); - ushort uY = Utils.BytesToUInt16(posBytes, i + 2); - ushort uZ = Utils.BytesToUInt16(posBytes, i + 4); + fixed (byte* ptrstart = posBytes) + { + byte* end = ptrstart + posBytes.Length; + byte* ptr = ptrstart; + while (ptr < end) + { + ushort uX = Utils.BytesToUInt16(ptr); + ptr += 2; + ushort uY = Utils.BytesToUInt16(ptr); + ptr += 2; + ushort uZ = Utils.BytesToUInt16(ptr); + ptr += 2; - Coord c = new Coord( - Utils.UInt16ToFloat(uX, posMin.X, posMax.X), - Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y), - Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z)); - - coords.Add(c); + coords.Add(new Coord( + uX * posRange.X + posMin.X, + uY * posRange.Y + posMin.Y, + uZ * posRange.Z + posMin.Z) + ); + } } - byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); - for (int i = 0; i < triangleBytes.Length; i += 6) + fixed (byte* ptrstart = triangleBytes) { - ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset); - ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset); - ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset); - Face f = new Face(v1, v2, v3); - faces.Add(f); + byte* end = ptrstart + triangleBytes.Length; + byte* ptr = ptrstart; + while (ptr < end) + { + int v1 = Utils.BytesToUInt16(ptr) + faceIndexOffset; + ptr += 2; + int v2 = Utils.BytesToUInt16(ptr) + faceIndexOffset; + ptr += 2; + int v3 = Utils.BytesToUInt16(ptr) + faceIndexOffset; + ptr += 2; + Face f = new Face(v1, v2, v3); + faces.Add(f); + } } } @@ -349,7 +372,7 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing /// Coords are added to this list by the method. /// Faces are added to this list by the method. /// true if coords and faces were successfully generated, false if not - private bool GenerateCoordsAndFacesFromPrimMeshData( + private unsafe bool GenerateCoordsAndFacesFromPrimMeshData( string primName, PrimitiveBaseShape primShape, out List coords, out List faces, bool convex) { // m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); @@ -479,8 +502,6 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing List vs = new List(); PHullResult hullr = new PHullResult(); float3 f3; - Coord c; - Face f; Vector3 range; Vector3 min; @@ -523,101 +544,91 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing } data = cmap["Positions"].AsBinary(); - int ptr = 0; - int vertsoffset = 0; - - if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point + fixed(byte* ptrstart = data) { - foreach (int hullsize in hsizes) + byte* ptr = ptrstart; + + int vertsoffset = 0; + + if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point { - for (i = 0; i < hullsize; i++ ) + foreach (int hullsize in hsizes) { - t1 = data[ptr++]; - t1 += data[ptr++] << 8; - t2 = data[ptr++]; - t2 += data[ptr++] << 8; - t3 = data[ptr++]; - t3 += data[ptr++] << 8; - - f3 = new float3((t1 * range.X + min.X), - (t2 * range.Y + min.Y), - (t3 * range.Z + min.Z)); - vs.Add(f3); - } - - if(hullsize <3) - { - vs.Clear(); - continue; - } - - if (hullsize <5) - { - foreach (float3 point in vs) + if (hullsize < 4) { - c.X = point.x; - c.Y = point.y; - c.Z = point.z; - coords.Add(c); - } - f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2); - faces.Add(f); + if (hullsize < 3) + { + ptr += 6 * hullsize; + continue; + } - if (hullsize == 4) + for (i = 0; i < hullsize; i++) + { + t1 = Utils.BytesToUInt16(ptr); ptr += 2; + t2 = Utils.BytesToUInt16(ptr); ptr += 2; + t3 = Utils.BytesToUInt16(ptr); ptr += 2; + + coords.Add(new Coord( + t1 * range.X + min.X, + t2 * range.Y + min.Y, + t3 * range.Z + min.Z) + ); + + } + + faces.Add(new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2)); + + vertsoffset += hullsize; + continue; + } + + for (i = 0; i < hullsize; i++) { - // not sure about orientation.. - f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3); - faces.Add(f); - f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1); - faces.Add(f); - f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1); - faces.Add(f); + t1 = Utils.BytesToUInt16(ptr); ptr += 2; + t2 = Utils.BytesToUInt16(ptr); ptr += 2; + t3 = Utils.BytesToUInt16(ptr); ptr += 2; + + f3 = new float3(t1 * range.X + min.X, + t2 * range.Y + min.Y, + t3 * range.Z + min.Z); + vs.Add(f3); } - vertsoffset += vs.Count; + + List indices; + if (!HullUtils.ComputeHull(vs, out indices)) + { + vs.Clear(); + continue; + } + + nverts = vs.Count; + nindexs = indices.Count; + + if (nindexs % 3 != 0) + { + vs.Clear(); + continue; + } + + for (i = 0; i < vs.Count; i++) + coords.Add(new Coord(vs[i].x, vs[i].y, vs[i].z)); + + for (i = 0; i < indices.Count; i += 3) + { + t1 = indices[i]; + if (t1 > nverts) + break; + t2 = indices[i + 1]; + if (t2 > nverts) + break; + t3 = indices[i + 2]; + if (t3 > nverts) + break; + faces.Add(new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3)); + } + vertsoffset += nverts; vs.Clear(); - continue; } - - List indices; - if (!HullUtils.ComputeHull(vs, out indices)) - { - vs.Clear(); - continue; - } - - nverts = vs.Count; - nindexs = indices.Count; - - if (nindexs % 3 != 0) - { - vs.Clear(); - continue; - } - - for (i = 0; i < nverts; i++) - { - c.X = vs[i].x; - c.Y = vs[i].y; - c.Z = vs[i].z; - coords.Add(c); - } - - for (i = 0; i < nindexs; i += 3) - { - t1 = indices[i]; - if (t1 > nverts) - break; - t2 = indices[i + 1]; - if (t2 > nverts) - break; - t3 = indices[i + 2]; - if (t3 > nverts) - break; - f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3); - faces.Add(f); - } - vertsoffset += nverts; - vs.Clear(); } } if (coords.Count > 0 && faces.Count > 0) @@ -631,55 +642,41 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing } vs.Clear(); - if (cmap.ContainsKey("BoundingVerts")) + if (cmap.TryGetValue("BoundingVerts", out OSD odata)) { - data = cmap["BoundingVerts"].AsBinary(); - - for (i = 0; i < data.Length; ) - { - t1 = data[i++]; - t1 += data[i++] << 8; - t2 = data[i++]; - t2 += data[i++] << 8; - t3 = data[i++]; - t3 += data[i++] << 8; - - f3 = new float3((t1 * range.X + min.X), - (t2 * range.Y + min.Y), - (t3 * range.Z + min.Z)); - vs.Add(f3); - } - - nverts = vs.Count; - - if (nverts < 3) + data = odata.AsBinary(); + if (data.Length < 3 * 6) { vs.Clear(); return false; } - if (nverts < 5) + fixed (byte* ptrstart = data) { - foreach (float3 point in vs) + byte* end = ptrstart + data.Length; + byte* ptr = ptrstart; + while(ptr < end) { - c.X = point.x; - c.Y = point.y; - c.Z = point.z; - coords.Add(c); - } + t1 = Utils.BytesToUInt16(ptr); ptr += 2; + t2 = Utils.BytesToUInt16(ptr); ptr += 2; + t3 = Utils.BytesToUInt16(ptr); ptr += 2; - f = new Face(0, 1, 2); - faces.Add(f); - - if (nverts == 4) - { - f = new Face(0, 2, 3); - faces.Add(f); - f = new Face(0, 3, 1); - faces.Add(f); - f = new Face( 3, 2, 1); - faces.Add(f); + f3 = new float3((t1 * range.X + min.X), + (t2 * range.Y + min.Y), + (t3 * range.Z + min.Z)); + vs.Add(f3); } + } + + nverts = vs.Count; + + if (nverts < 4) + { + for (i = 0; i < vs.Count; i++) + coords.Add(new Coord(vs[i].x, vs[i].y, vs[i].z)); + + faces.Add(new Face(0, 1, 2)); + vs.Clear(); return true; } @@ -693,14 +690,10 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing if (nindexs % 3 != 0) return false; - for (i = 0; i < nverts; i++) - { - c.X = vs[i].x; - c.Y = vs[i].y; - c.Z = vs[i].z; - coords.Add(c); - } - for (i = 0; i < nindexs; i += 3) + for (i = 0; i < vs.Count; i++) + coords.Add(new Coord(vs[i].x, vs[i].y, vs[i].z)); + + for (i = 0; i < indices.Count; i += 3) { t1 = indices[i]; if (t1 > nverts) @@ -711,15 +704,14 @@ namespace OpenSim.Region.PhysicsModule.ubODEMeshing t3 = indices[i + 2]; if (t3 > nverts) break; - f = new Face(t1, t2, t3); - faces.Add(f); + + faces.Add(new Face(t1, t2, t3)); } vs.Clear(); if (coords.Count > 0 && faces.Count > 0) return true; } - else - return false; + return false; } }