From e9a56d5e194b9b80f8adf5d1ce5d9ab3f5f7bfdd Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 12 Sep 2015 21:38:26 +0100 Subject: rename Ubit physics modules --- .../PhysicsModules/UbitMeshing/HelperTypes.cs | 340 -- OpenSim/Region/PhysicsModules/UbitMeshing/Mesh.cs | 601 --- .../PhysicsModules/UbitMeshing/Meshmerizer.cs | 1458 -------- .../PhysicsModules/UbitMeshing/PrimMesher.cs | 1708 --------- .../UbitMeshing/Properties/AssemblyInfo.cs | 36 - .../Region/PhysicsModules/UbitMeshing/SculptMap.cs | 244 -- .../PhysicsModules/UbitMeshing/SculptMesh.cs | 220 -- .../Region/PhysicsModules/UbitOde/ODECharacter.cs | 1847 --------- .../Region/PhysicsModules/UbitOde/ODEDynamics.cs | 1096 ------ .../Region/PhysicsModules/UbitOde/ODEMeshWorker.cs | 933 ----- OpenSim/Region/PhysicsModules/UbitOde/ODEModule.cs | 97 - OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs | 3901 -------------------- .../UbitOde/ODERayCastRequestManager.cs | 683 ---- .../Region/PhysicsModules/UbitOde/ODESitAvatar.cs | 356 -- OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs | 2025 ---------- OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs | 2818 -------------- .../UbitOde/Properties/AssemblyInfo.cs | 61 - OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs | 2024 ++++++++++ .../Region/PhysicsModules/ubOde/ODECharacter.cs | 1847 +++++++++ OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | 1096 ++++++ .../Region/PhysicsModules/ubOde/ODEMeshWorker.cs | 933 +++++ OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs | 97 + OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs | 3896 +++++++++++++++++++ .../ubOde/ODERayCastRequestManager.cs | 683 ++++ OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs | 2825 ++++++++++++++ .../Region/PhysicsModules/ubOde/ODESitAvatar.cs | 356 ++ .../ubOde/Properties/AssemblyInfo.cs | 61 + .../PhysicsModules/ubOdeMeshing/HelperTypes.cs | 340 ++ OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs | 601 +++ .../PhysicsModules/ubOdeMeshing/Meshmerizer.cs | 1458 ++++++++ .../PhysicsModules/ubOdeMeshing/PrimMesher.cs | 1708 +++++++++ .../ubOdeMeshing/Properties/AssemblyInfo.cs | 36 + .../PhysicsModules/ubOdeMeshing/SculptMap.cs | 244 ++ .../PhysicsModules/ubOdeMeshing/SculptMesh.cs | 220 ++ 34 files changed, 18425 insertions(+), 18424 deletions(-) delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/HelperTypes.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/Mesh.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/Meshmerizer.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/PrimMesher.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/Properties/AssemblyInfo.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/SculptMap.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitMeshing/SculptMesh.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODEModule.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs delete mode 100644 OpenSim/Region/PhysicsModules/UbitOde/Properties/AssemblyInfo.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs (limited to 'OpenSim/Region/PhysicsModules') diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/HelperTypes.cs deleted file mode 100644 index fb38cdb..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/HelperTypes.cs +++ /dev/null @@ -1,340 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.Diagnostics; -using System.Globalization; -using OpenMetaverse; -using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModule.UbitMeshing; - -public class Vertex : IComparable -{ - 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 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; - } - - public Vertex(Vector3 v) - { - vector = v; - } - - public Vertex Clone() - { - return new Vertex(X, Y, Z); - } - - public static Vertex FromAngle(double angle) - { - return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f); - } - - public float Length() - { - return vector.Length(); - } - - public virtual bool Equals(Vertex v, float tolerance) - { - Vertex diff = this - v; - float d = diff.Length(); - if (d < tolerance) - return true; - - return false; - } - - - public int CompareTo(Vertex other) - { - if (X < other.X) - return -1; - - if (X > other.X) - return 1; - - if (Y < other.Y) - return -1; - - if (Y > other.Y) - return 1; - - if (Z < other.Z) - return -1; - - if (Z > other.Z) - return 1; - - 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? - // Well, the raw format uses the english/US notation of numbers - // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1. - // The german notation uses these characters exactly vice versa! - // The Float.ToString() routine is a localized one, giving different results depending on the country - // settings your machine works with. Unusable for a machine readable file format :-( - NumberFormatInfo nfi = new NumberFormatInfo(); - nfi.NumberDecimalSeparator = "."; - nfi.NumberDecimalDigits = 6; - - String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi); - - return s1; - } -} - -public class Triangle -{ - public Vertex v1; - public Vertex v2; - public Vertex v3; - - public Triangle(Vertex _v1, Vertex _v2, Vertex _v3) - { - v1 = _v1; - v2 = _v2; - v3 = _v3; - } - - public Triangle(float _v1x,float _v1y,float _v1z, - float _v2x,float _v2y,float _v2z, - float _v3x,float _v3y,float _v3z) - { - v1 = new Vertex(_v1x, _v1y, _v1z); - v2 = new Vertex(_v2x, _v2y, _v2z); - v3 = new Vertex(_v3x, _v3y, _v3z); - } - - public override String ToString() - { - NumberFormatInfo nfi = new NumberFormatInfo(); - nfi.CurrencyDecimalDigits = 2; - nfi.CurrencyDecimalSeparator = "."; - - String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">"; - String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">"; - String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">"; - - 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() - { - String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw(); - return output; - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/Mesh.cs deleted file mode 100644 index e3bfe77..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/Mesh.cs +++ /dev/null @@ -1,601 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.IO; -using System.Runtime.InteropServices; -using OpenSim.Region.PhysicsModules.SharedBase; -using PrimMesher; -using OpenMetaverse; -using System.Runtime.Serialization; -using System.Runtime.Serialization.Formatters.Binary; - -namespace OpenSim.Region.PhysicsModule.UbitMeshing -{ - public class MeshBuildingData - { - public Dictionary m_vertices; - public List m_triangles; - public float m_obbXmin; - public float m_obbXmax; - public float m_obbYmin; - public float m_obbYmax; - public float m_obbZmin; - public float m_obbZmax; - public Vector3 m_centroid; - public int m_centroidDiv; - } - - [Serializable()] - public class Mesh : IMesh - { - float[] vertices; - int[] indexes; - Vector3 m_obb; - Vector3 m_obboffset; - [NonSerialized()] - MeshBuildingData m_bdata; - [NonSerialized()] - GCHandle vhandler; - [NonSerialized()] - GCHandle ihandler; - [NonSerialized()] - IntPtr m_verticesPtr = IntPtr.Zero; - [NonSerialized()] - IntPtr m_indicesPtr = IntPtr.Zero; - [NonSerialized()] - int m_vertexCount = 0; - [NonSerialized()] - int m_indexCount = 0; - - public int RefCount { get; set; } - public AMeshKey Key { get; set; } - - 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 Mesh() - { - vertexcomp vcomp = new vertexcomp(); - - m_bdata = new MeshBuildingData(); - m_bdata.m_vertices = new Dictionary(vcomp); - m_bdata.m_triangles = new List(); - m_bdata.m_centroid = Vector3.Zero; - m_bdata.m_centroidDiv = 0; - m_bdata.m_obbXmin = float.MaxValue; - m_bdata.m_obbXmax = float.MinValue; - m_bdata.m_obbYmin = float.MaxValue; - m_bdata.m_obbYmax = float.MinValue; - m_bdata.m_obbZmin = float.MaxValue; - m_bdata.m_obbZmax = float.MinValue; - m_obb = new Vector3(0.5f, 0.5f, 0.5f); - m_obboffset = Vector3.Zero; - } - - - public Mesh Scale(Vector3 scale) - { - if (m_verticesPtr == null || m_indicesPtr == null) - return null; - - Mesh result = new Mesh(); - - float x = scale.X; - float y = scale.Y; - float z = scale.Z; - - result.m_obb.X = m_obb.X * x; - result.m_obb.Y = m_obb.Y * y; - result.m_obb.Z = m_obb.Z * z; - result.m_obboffset.X = m_obboffset.X * x; - result.m_obboffset.Y = m_obboffset.Y * y; - 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++) - { - result.vertices[j] = vertices[j] * x; - j++; - result.vertices[j] = vertices[j] * y; - j++; - result.vertices[j] = vertices[j] * z; - j++; - } - - result.indexes = new int[indexes.Length]; - indexes.CopyTo(result.indexes,0); - - result.pinMemory(); - - return result; - } - - public Mesh Clone() - { - Mesh result = new Mesh(); - - if (m_bdata != null) - { - result.m_bdata = new MeshBuildingData(); - foreach (Triangle t in m_bdata.m_triangles) - { - result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); - } - result.m_bdata.m_centroid = m_bdata.m_centroid; - result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv; - result.m_bdata.m_obbXmin = m_bdata.m_obbXmin; - result.m_bdata.m_obbXmax = m_bdata.m_obbXmax; - result.m_bdata.m_obbYmin = m_bdata.m_obbYmin; - result.m_bdata.m_obbYmax = m_bdata.m_obbYmax; - result.m_bdata.m_obbZmin = m_bdata.m_obbZmin; - result.m_bdata.m_obbZmax = m_bdata.m_obbZmax; - } - result.m_obb = m_obb; - result.m_obboffset = m_obboffset; - return result; - } - - public void addVertexLStats(Vertex v) - { - float x = v.X; - float y = v.Y; - float z = v.Z; - - m_bdata.m_centroid.X += x; - m_bdata.m_centroid.Y += y; - m_bdata.m_centroid.Z += z; - m_bdata.m_centroidDiv++; - - if (x > m_bdata.m_obbXmax) - m_bdata.m_obbXmax = x; - else if (x < m_bdata.m_obbXmin) - m_bdata.m_obbXmin = x; - - if (y > m_bdata.m_obbYmax) - m_bdata.m_obbYmax = y; - else if (y < m_bdata.m_obbYmin) - m_bdata.m_obbYmin = y; - - if (z > m_bdata.m_obbZmax) - m_bdata.m_obbZmax = z; - else if (z < m_bdata.m_obbZmin) - m_bdata.m_obbZmin = z; - - } - - public void Add(Triangle triangle) - { - if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) - throw new NotSupportedException("Attempt to Add to a pinned Mesh"); - - - triangle.v1.X = (float)Math.Round(triangle.v1.X, 6); - triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6); - triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6); - triangle.v2.X = (float)Math.Round(triangle.v2.X, 6); - triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6); - triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6); - triangle.v3.X = (float)Math.Round(triangle.v3.X, 6); - triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6); - triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6); - - if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) - || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) - || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) - ) - { - return; - } - - if (m_bdata.m_vertices.Count == 0) - { - m_bdata.m_centroidDiv = 0; - m_bdata.m_centroid = Vector3.Zero; - } - - if (!m_bdata.m_vertices.ContainsKey(triangle.v1)) - { - m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count; - addVertexLStats(triangle.v1); - } - if (!m_bdata.m_vertices.ContainsKey(triangle.v2)) - { - m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count; - addVertexLStats(triangle.v2); - } - if (!m_bdata.m_vertices.ContainsKey(triangle.v3)) - { - m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count; - addVertexLStats(triangle.v3); - } - m_bdata.m_triangles.Add(triangle); - } - - public Vector3 GetCentroid() - { - return m_obboffset; - - } - - public Vector3 GetOBB() - { - return m_obb; - float x, y, z; - if (m_bdata.m_centroidDiv > 0) - { - x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; - y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; - z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; - } - else // ?? - { - x = 0.5f; - y = 0.5f; - z = 0.5f; - } - return new Vector3(x, y, z); - } - - public List getVertexList() - { - List result = new List(); - foreach (Vertex v in m_bdata.m_vertices.Keys) - { - result.Add(new Vector3(v.X, v.Y, v.Z)); - } - return result; - } - - public float[] getVertexListAsFloat() - { - if (m_bdata.m_vertices == null) - throw new NotSupportedException(); - float[] result = new float[m_bdata.m_vertices.Count * 3]; - foreach (KeyValuePair kvp in m_bdata.m_vertices) - { - Vertex v = kvp.Key; - int i = kvp.Value; - result[3 * i + 0] = v.X; - result[3 * i + 1] = v.Y; - result[3 * i + 2] = v.Z; - } - return result; - } - - public float[] getVertexListAsFloatLocked() - { - return null; - } - - public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount) - { - // A vertex is 3 floats - vertexStride = 3 * sizeof(float); - - // If there isn't an unmanaged array allocated yet, do it now - if (m_verticesPtr == IntPtr.Zero && m_bdata != null) - { - vertices = getVertexListAsFloat(); - // Each vertex is 3 elements (floats) - m_vertexCount = vertices.Length / 3; - vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); - m_verticesPtr = vhandler.AddrOfPinnedObject(); - GC.AddMemoryPressure(Buffer.ByteLength(vertices)); - } - _vertices = m_verticesPtr; - vertexCount = m_vertexCount; - } - - public int[] getIndexListAsInt() - { - if (m_bdata.m_triangles == null) - throw new NotSupportedException(); - int[] result = new int[m_bdata.m_triangles.Count * 3]; - for (int i = 0; i < m_bdata.m_triangles.Count; i++) - { - Triangle t = m_bdata.m_triangles[i]; - result[3 * i + 0] = m_bdata.m_vertices[t.v1]; - result[3 * i + 1] = m_bdata.m_vertices[t.v2]; - result[3 * i + 2] = m_bdata.m_vertices[t.v3]; - } - return result; - } - - /// - /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA - /// - /// - public int[] getIndexListAsIntLocked() - { - return null; - } - - public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount) - { - // If there isn't an unmanaged array allocated yet, do it now - if (m_indicesPtr == IntPtr.Zero && m_bdata != null) - { - indexes = getIndexListAsInt(); - m_indexCount = indexes.Length; - ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); - m_indicesPtr = ihandler.AddrOfPinnedObject(); - GC.AddMemoryPressure(Buffer.ByteLength(indexes)); - } - // A triangle is 3 ints (indices) - triStride = 3 * sizeof(int); - indices = m_indicesPtr; - indexCount = m_indexCount; - } - - public void releasePinned() - { - if (m_verticesPtr != IntPtr.Zero) - { - vhandler.Free(); - vertices = null; - m_verticesPtr = IntPtr.Zero; - } - if (m_indicesPtr != IntPtr.Zero) - { - ihandler.Free(); - indexes = null; - m_indicesPtr = IntPtr.Zero; - } - } - - /// - /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions - /// - public void releaseSourceMeshData() - { - if (m_bdata != null) - { - m_bdata.m_triangles = null; - m_bdata.m_vertices = null; - } - } - - public void releaseBuildingMeshData() - { - if (m_bdata != null) - { - m_bdata.m_triangles = null; - m_bdata.m_vertices = null; - m_bdata = null; - } - } - - public void Append(IMesh newMesh) - { - if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) - throw new NotSupportedException("Attempt to Append to a pinned Mesh"); - - if (!(newMesh is Mesh)) - return; - - foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles) - Add(t); - } - - // Do a linear transformation of mesh. - public void TransformLinear(float[,] matrix, float[] offset) - { - if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) - throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); - - foreach (Vertex v in m_bdata.m_vertices.Keys) - { - if (v == null) - continue; - float x, y, z; - x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0]; - y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1]; - z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2]; - v.X = x + offset[0]; - v.Y = y + offset[1]; - v.Z = z + offset[2]; - } - } - - public void DumpRaw(String path, String name, String title) - { - if (path == null) - return; - if (m_bdata == null) - return; - String fileName = name + "_" + title + ".raw"; - String completePath = System.IO.Path.Combine(path, fileName); - StreamWriter sw = new StreamWriter(completePath); - foreach (Triangle t in m_bdata.m_triangles) - { - String s = t.ToStringRaw(); - sw.WriteLine(s); - } - sw.Close(); - } - - public void TrimExcess() - { - m_bdata.m_triangles.TrimExcess(); - } - - public void pinMemory() - { - m_vertexCount = vertices.Length / 3; - vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); - m_verticesPtr = vhandler.AddrOfPinnedObject(); - GC.AddMemoryPressure(Buffer.ByteLength(vertices)); - - m_indexCount = indexes.Length; - ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); - m_indicesPtr = ihandler.AddrOfPinnedObject(); - GC.AddMemoryPressure(Buffer.ByteLength(indexes)); - } - - public void PrepForOde() - { - // If there isn't an unmanaged array allocated yet, do it now - if (m_verticesPtr == IntPtr.Zero) - vertices = getVertexListAsFloat(); - - // If there isn't an unmanaged array allocated yet, do it now - if (m_indicesPtr == IntPtr.Zero) - indexes = getIndexListAsInt(); - - pinMemory(); - - float x, y, z; - - if (m_bdata.m_centroidDiv > 0) - { - m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv); - x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; - y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; - z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; - } - - else - { - m_obboffset = Vector3.Zero; - x = 0.5f; - y = 0.5f; - z = 0.5f; - } - m_obb = new Vector3(x, y, z); - - releaseBuildingMeshData(); - } - public bool ToStream(Stream st) - { - if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero) - return false; - - BinaryWriter bw = new BinaryWriter(st); - bool ok = true; - - try - { - - bw.Write(m_vertexCount); - bw.Write(m_indexCount); - - for (int i = 0; i < 3 * m_vertexCount; i++) - bw.Write(vertices[i]); - for (int i = 0; i < m_indexCount; i++) - bw.Write(indexes[i]); - bw.Write(m_obb.X); - bw.Write(m_obb.Y); - bw.Write(m_obb.Z); - bw.Write(m_obboffset.X); - bw.Write(m_obboffset.Y); - bw.Write(m_obboffset.Z); - } - catch - { - ok = false; - } - - if (bw != null) - { - bw.Flush(); - bw.Close(); - } - - return ok; - } - - public static Mesh FromStream(Stream st, AMeshKey key) - { - Mesh mesh = new Mesh(); - mesh.releaseBuildingMeshData(); - - BinaryReader br = new BinaryReader(st); - - bool ok = true; - try - { - mesh.m_vertexCount = br.ReadInt32(); - mesh.m_indexCount = br.ReadInt32(); - - int n = 3 * mesh.m_vertexCount; - mesh.vertices = new float[n]; - for (int i = 0; i < n; i++) - mesh.vertices[i] = br.ReadSingle(); - - mesh.indexes = new int[mesh.m_indexCount]; - for (int i = 0; i < mesh.m_indexCount; i++) - mesh.indexes[i] = br.ReadInt32(); - - mesh.m_obb.X = br.ReadSingle(); - mesh.m_obb.Y = br.ReadSingle(); - mesh.m_obb.Z = br.ReadSingle(); - - mesh.m_obboffset.X = br.ReadSingle(); - mesh.m_obboffset.Y = br.ReadSingle(); - mesh.m_obboffset.Z = br.ReadSingle(); - } - catch - { - ok = false; - } - - br.Close(); - - if (ok) - { - mesh.pinMemory(); - - mesh.Key = key; - mesh.RefCount = 1; - - return mesh; - } - - mesh.vertices = null; - mesh.indexes = null; - return null; - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/Meshmerizer.cs deleted file mode 100644 index 58a93a1..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/Meshmerizer.cs +++ /dev/null @@ -1,1458 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ -//#define SPAM - -using System; -using System.Collections.Generic; -using OpenSim.Framework; -using OpenSim.Region.Framework.Scenes; -using OpenSim.Region.Framework.Interfaces; -using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet; -using OpenMetaverse; -using OpenMetaverse.StructuredData; -using System.Drawing; -using System.Drawing.Imaging; -using System.IO.Compression; -using PrimMesher; -using log4net; -using Nini.Config; -using System.Reflection; -using System.IO; -using ComponentAce.Compression.Libs.zlib; -using System.Runtime.Serialization; -using System.Runtime.Serialization.Formatters.Binary; - -using Mono.Addins; - -namespace OpenSim.Region.PhysicsModule.UbitMeshing -{ - [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "UbitMeshmerizer")] - public class UbitMeshmerizer : IMesher, INonSharedRegionModule - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - // Setting baseDir to a path will enable the dumping of raw files - // raw files can be imported by blender so a visual inspection of the results can be done - - private bool m_Enabled = false; - - public object diskLock = new object(); - - public bool doMeshFileCache = true; - - public string cachePath = "MeshCache"; - public TimeSpan CacheExpire; - public bool doCacheExpire = true; - -// const string baseDir = "rawFiles"; - private const string baseDir = null; //"rawFiles"; - - private bool useMeshiesPhysicsMesh = false; - - private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh - - private Dictionary m_uniqueMeshes = new Dictionary(); - private Dictionary m_uniqueReleasedMeshes = new Dictionary(); - - #region INonSharedRegionModule - public string Name - { - get { return "UbitMeshmerizer"; } - } - - public Type ReplaceableInterface - { - get { return null; } - } - - public void Initialise(IConfigSource config) - { - IConfig start_config = config.Configs["Startup"]; - - string mesher = start_config.GetString("meshing", string.Empty); - if (mesher == Name) - { - float fcache = 48.0f; - // float fcache = 0.02f; - - IConfig mesh_config = config.Configs["Mesh"]; - if (mesh_config != null) - { - useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); - if (useMeshiesPhysicsMesh) - { - doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache); - cachePath = mesh_config.GetString("MeshFileCachePath", cachePath); - fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache); - doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire); - } - else - { - doMeshFileCache = false; - doCacheExpire = false; - } - - m_Enabled = true; - } - - CacheExpire = TimeSpan.FromHours(fcache); - - } - } - - public void Close() - { - } - - public void AddRegion(Scene scene) - { - if (!m_Enabled) - return; - - scene.RegisterModuleInterface(this); - } - - public void RemoveRegion(Scene scene) - { - if (!m_Enabled) - return; - - scene.UnregisterModuleInterface(this); - } - - public void RegionLoaded(Scene scene) - { - if (!m_Enabled) - return; - } - - #endregion - - /// - /// creates a simple box mesh of the specified size. This mesh is of very low vertex count and may - /// be useful as a backup proxy when level of detail is not needed or when more complex meshes fail - /// for some reason - /// - /// - /// - /// - /// - /// - /// - /// - private static Mesh CreateSimpleBoxMesh(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) - { - Mesh box = new Mesh(); - List vertices = new List(); - // bottom - - vertices.Add(new Vertex(minX, maxY, minZ)); - vertices.Add(new Vertex(maxX, maxY, minZ)); - vertices.Add(new Vertex(maxX, minY, minZ)); - vertices.Add(new Vertex(minX, minY, minZ)); - - box.Add(new Triangle(vertices[0], vertices[1], vertices[2])); - box.Add(new Triangle(vertices[0], vertices[2], vertices[3])); - - // top - - vertices.Add(new Vertex(maxX, maxY, maxZ)); - vertices.Add(new Vertex(minX, maxY, maxZ)); - vertices.Add(new Vertex(minX, minY, maxZ)); - vertices.Add(new Vertex(maxX, minY, maxZ)); - - box.Add(new Triangle(vertices[4], vertices[5], vertices[6])); - box.Add(new Triangle(vertices[4], vertices[6], vertices[7])); - - // sides - - box.Add(new Triangle(vertices[5], vertices[0], vertices[3])); - box.Add(new Triangle(vertices[5], vertices[3], vertices[6])); - - box.Add(new Triangle(vertices[1], vertices[0], vertices[5])); - box.Add(new Triangle(vertices[1], vertices[5], vertices[4])); - - box.Add(new Triangle(vertices[7], vertices[1], vertices[4])); - box.Add(new Triangle(vertices[7], vertices[2], vertices[1])); - - box.Add(new Triangle(vertices[3], vertices[2], vertices[7])); - box.Add(new Triangle(vertices[3], vertices[7], vertices[6])); - - return box; - } - - /// - /// Creates a simple bounding box mesh for a complex input mesh - /// - /// - /// - private static Mesh CreateBoundingBoxMesh(Mesh meshIn) - { - float minX = float.MaxValue; - float maxX = float.MinValue; - float minY = float.MaxValue; - float maxY = float.MinValue; - float minZ = float.MaxValue; - float maxZ = float.MinValue; - - foreach (Vector3 v in meshIn.getVertexList()) - { - if (v.X < minX) minX = v.X; - if (v.Y < minY) minY = v.Y; - if (v.Z < minZ) minZ = v.Z; - - if (v.X > maxX) maxX = v.X; - if (v.Y > maxY) maxY = v.Y; - if (v.Z > maxZ) maxZ = v.Z; - } - - return CreateSimpleBoxMesh(minX, maxX, minY, maxY, minZ, maxZ); - } - - private void ReportPrimError(string message, string primName, PrimMesh primMesh) - { - m_log.Error(message); - m_log.Error("\nPrim Name: " + primName); - m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString()); - } - - /// - /// Add a submesh to an existing list of coords and faces. - /// - /// - /// Size of entire object - /// - /// - private 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"])) - return; - - OpenMetaverse.Vector3 posMax; - OpenMetaverse.Vector3 posMin; - if (subMeshData.ContainsKey("PositionDomain")) - { - posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); - posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); - } - else - { - posMax = new Vector3(0.5f, 0.5f, 0.5f); - posMin = new Vector3(-0.5f, -0.5f, -0.5f); - } - - ushort faceIndexOffset = (ushort)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); - - 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); - } - - byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); - for (int i = 0; i < triangleBytes.Length; i += 6) - { - 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); - } - } - - /// - /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type. - /// - /// - /// - /// - /// - /// - private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex) - { -// m_log.DebugFormat( -// "[MESH]: Creating physics proxy for {0}, shape {1}", -// primName, (OpenMetaverse.SculptType)primShape.SculptType); - - List coords; - List faces; - - if (primShape.SculptEntry) - { - if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh) - { - if (!useMeshiesPhysicsMesh) - return null; - - if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex)) - return null; - } - else - { - if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces)) - return null; - } - } - else - { - if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, out coords, out faces)) - return null; - } - - primShape.SculptData = Utils.EmptyBytes; - - int numCoords = coords.Count; - int numFaces = faces.Count; - - Mesh mesh = new Mesh(); - // Add the corresponding triangles to the mesh - for (int i = 0; i < numFaces; i++) - { - Face f = faces[i]; - mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z, - coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z, - coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); - } - - coords.Clear(); - faces.Clear(); - - return mesh; - } - - /// - /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim. - /// - /// - /// - /// - /// 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( - string primName, PrimitiveBaseShape primShape, out List coords, out List faces, bool convex) - { -// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); - - bool usemesh = false; - - coords = new List(); - faces = new List(); - OSD meshOsd = null; - - if (primShape.SculptData.Length <= 0) - { -// m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); - return false; - } - - long start = 0; - using (MemoryStream data = new MemoryStream(primShape.SculptData)) - { - try - { - OSD osd = OSDParser.DeserializeLLSDBinary(data); - if (osd is OSDMap) - meshOsd = (OSDMap)osd; - else - { - m_log.Warn("[Mesh}: unable to cast mesh asset to OSDMap"); - return false; - } - } - catch (Exception e) - { - m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString()); - } - - start = data.Position; - } - - if (meshOsd is OSDMap) - { - OSDMap physicsParms = null; - OSDMap map = (OSDMap)meshOsd; - - if (!convex) - { - if (map.ContainsKey("physics_shape")) - physicsParms = (OSDMap)map["physics_shape"]; // old asset format - else if (map.ContainsKey("physics_mesh")) - physicsParms = (OSDMap)map["physics_mesh"]; // new asset format - - if (physicsParms != null) - usemesh = true; - } - - if(!usemesh && (map.ContainsKey("physics_convex"))) - physicsParms = (OSDMap)map["physics_convex"]; - - - if (physicsParms == null) - { - m_log.Warn("[MESH]: unknown mesh type"); - return false; - } - - int physOffset = physicsParms["offset"].AsInteger() + (int)start; - int physSize = physicsParms["size"].AsInteger(); - - if (physOffset < 0 || physSize == 0) - return false; // no mesh data in asset - - OSD decodedMeshOsd = new OSD(); - byte[] meshBytes = new byte[physSize]; - System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize); - - try - { - using (MemoryStream inMs = new MemoryStream(meshBytes)) - { - using (MemoryStream outMs = new MemoryStream()) - { - using (ZOutputStream zOut = new ZOutputStream(outMs)) - { - byte[] readBuffer = new byte[2048]; - int readLen = 0; - while ((readLen = inMs.Read(readBuffer, 0, readBuffer.Length)) > 0) - { - zOut.Write(readBuffer, 0, readLen); - } - zOut.Flush(); - outMs.Seek(0, SeekOrigin.Begin); - - byte[] decompressedBuf = outMs.GetBuffer(); - - decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf); - } - } - } - } - catch (Exception e) - { - m_log.Error("[MESH]: exception decoding physical mesh: " + e.ToString()); - return false; - } - - if (usemesh) - { - OSDArray decodedMeshOsdArray = null; - - // physics_shape is an array of OSDMaps, one for each submesh - if (decodedMeshOsd is OSDArray) - { -// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); - - decodedMeshOsdArray = (OSDArray)decodedMeshOsd; - foreach (OSD subMeshOsd in decodedMeshOsdArray) - { - if (subMeshOsd is OSDMap) - AddSubMesh(subMeshOsd as OSDMap, coords, faces); - } - } - } - else - { - OSDMap cmap = (OSDMap)decodedMeshOsd; - if (cmap == null) - return false; - - byte[] data; - - List vs = new List(); - PHullResult hullr = new PHullResult(); - float3 f3; - Coord c; - Face f; - Vector3 range; - Vector3 min; - - const float invMaxU16 = 1.0f / 65535f; - int t1; - int t2; - int t3; - int i; - int nverts; - int nindexs; - - if (cmap.ContainsKey("Max")) - range = cmap["Max"].AsVector3(); - else - range = new Vector3(0.5f, 0.5f, 0.5f); - - if (cmap.ContainsKey("Min")) - min = cmap["Min"].AsVector3(); - else - min = new Vector3(-0.5f, -0.5f, -0.5f); - - range = range - min; - range *= invMaxU16; - - if (!convex && cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions")) - { - List hsizes = new List(); - int totalpoints = 0; - data = cmap["HullList"].AsBinary(); - for (i = 0; i < data.Length; i++) - { - t1 = data[i]; - if (t1 == 0) - t1 = 256; - totalpoints += t1; - hsizes.Add(t1); - } - - data = cmap["Positions"].AsBinary(); - int ptr = 0; - int vertsoffset = 0; - - if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point - { - foreach (int hullsize in hsizes) - { - for (i = 0; i < hullsize; i++ ) - { - 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) - { - 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 == 4) - { - // 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); - } - vertsoffset += vs.Count; - vs.Clear(); - continue; - } - - if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) - { - vs.Clear(); - continue; - } - - nverts = hullr.Vertices.Count; - nindexs = hullr.Indices.Count; - - if (nindexs % 3 != 0) - { - vs.Clear(); - continue; - } - - for (i = 0; i < nverts; i++) - { - c.X = hullr.Vertices[i].x; - c.Y = hullr.Vertices[i].y; - c.Z = hullr.Vertices[i].z; - coords.Add(c); - } - - for (i = 0; i < nindexs; i += 3) - { - t1 = hullr.Indices[i]; - if (t1 > nverts) - break; - t2 = hullr.Indices[i + 1]; - if (t2 > nverts) - break; - t3 = hullr.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) - return true; - } - - vs.Clear(); - - if (cmap.ContainsKey("BoundingVerts")) - { - 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); - } - - if (vs.Count < 3) - { - vs.Clear(); - return false; - } - - if (vs.Count < 5) - { - foreach (float3 point in vs) - { - c.X = point.x; - c.Y = point.y; - c.Z = point.z; - coords.Add(c); - } - f = new Face(0, 1, 2); - faces.Add(f); - - if (vs.Count == 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); - } - vs.Clear(); - return true; - } - - if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) - return false; - - nverts = hullr.Vertices.Count; - nindexs = hullr.Indices.Count; - - if (nindexs % 3 != 0) - return false; - - for (i = 0; i < nverts; i++) - { - c.X = hullr.Vertices[i].x; - c.Y = hullr.Vertices[i].y; - c.Z = hullr.Vertices[i].z; - coords.Add(c); - } - for (i = 0; i < nindexs; i += 3) - { - t1 = hullr.Indices[i]; - if (t1 > nverts) - break; - t2 = hullr.Indices[i + 1]; - if (t2 > nverts) - break; - t3 = hullr.Indices[i + 2]; - if (t3 > nverts) - break; - f = new Face(t1, t2, t3); - faces.Add(f); - } - - if (coords.Count > 0 && faces.Count > 0) - return true; - } - else - return false; - } - } - - return true; - } - - /// - /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim. - /// - /// - /// - /// - /// - /// 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 GenerateCoordsAndFacesFromPrimSculptData( - string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) - { - coords = new List(); - faces = new List(); - PrimMesher.SculptMesh sculptMesh; - Image idata = null; - - if (primShape.SculptData == null || primShape.SculptData.Length == 0) - return false; - - try - { - OpenMetaverse.Imaging.ManagedImage unusedData; - OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata); - - unusedData = null; - - if (idata == null) - { - // In some cases it seems that the decode can return a null bitmap without throwing - // an exception - m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName); - return false; - } - } - catch (DllNotFoundException) - { - m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed. Often times this is because of an old version of GLIBC. You must have version 2.4 or above!"); - return false; - } - catch (IndexOutOfRangeException) - { - m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed"); - return false; - } - catch (Exception ex) - { - m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message); - return false; - } - - PrimMesher.SculptMesh.SculptType sculptType; - // remove mirror and invert bits - OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f)); - switch (pbsSculptType) - { - case OpenMetaverse.SculptType.Cylinder: - sculptType = PrimMesher.SculptMesh.SculptType.cylinder; - break; - case OpenMetaverse.SculptType.Plane: - sculptType = PrimMesher.SculptMesh.SculptType.plane; - break; - case OpenMetaverse.SculptType.Torus: - sculptType = PrimMesher.SculptMesh.SculptType.torus; - break; - case OpenMetaverse.SculptType.Sphere: - sculptType = PrimMesher.SculptMesh.SculptType.sphere; - break; - default: - sculptType = PrimMesher.SculptMesh.SculptType.plane; - break; - } - - bool mirror = ((primShape.SculptType & 128) != 0); - bool invert = ((primShape.SculptType & 64) != 0); - - sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert); - - idata.Dispose(); - -// sculptMesh.DumpRaw(baseDir, primName, "primMesh"); - - coords = sculptMesh.coords; - faces = sculptMesh.faces; - - return true; - } - - /// - /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim. - /// - /// - /// - /// - /// 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 GenerateCoordsAndFacesFromPrimShapeData( - string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) - { - PrimMesh primMesh; - coords = new List(); - faces = new List(); - - float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f; - float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f; - float pathBegin = (float)primShape.PathBegin * 2.0e-5f; - float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f; - float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f; - float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f; - - float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f; - float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f; - - if (profileBegin < 0.0f) - profileBegin = 0.0f; - - if (profileEnd < 0.02f) - profileEnd = 0.02f; - else if (profileEnd > 1.0f) - profileEnd = 1.0f; - - if (profileBegin >= profileEnd) - profileBegin = profileEnd - 0.02f; - - float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f; - if (profileHollow > 0.95f) - profileHollow = 0.95f; - - int sides = 4; - LevelOfDetail iLOD = (LevelOfDetail)lod; - byte profshape = (byte)(primShape.ProfileCurve & 0x07); - - if (profshape == (byte)ProfileShape.EquilateralTriangle - || profshape == (byte)ProfileShape.IsometricTriangle - || profshape == (byte)ProfileShape.RightTriangle) - sides = 3; - else if (profshape == (byte)ProfileShape.Circle) - { - switch (iLOD) - { - case LevelOfDetail.High: sides = 24; break; - case LevelOfDetail.Medium: sides = 12; break; - case LevelOfDetail.Low: sides = 6; break; - case LevelOfDetail.VeryLow: sides = 3; break; - default: sides = 24; break; - } - } - else if (profshape == (byte)ProfileShape.HalfCircle) - { // half circle, prim is a sphere - switch (iLOD) - { - case LevelOfDetail.High: sides = 24; break; - case LevelOfDetail.Medium: sides = 12; break; - case LevelOfDetail.Low: sides = 6; break; - case LevelOfDetail.VeryLow: sides = 3; break; - default: sides = 24; break; - } - - profileBegin = 0.5f * profileBegin + 0.5f; - profileEnd = 0.5f * profileEnd + 0.5f; - } - - int hollowSides = sides; - if (primShape.HollowShape == HollowShape.Circle) - { - switch (iLOD) - { - case LevelOfDetail.High: hollowSides = 24; break; - case LevelOfDetail.Medium: hollowSides = 12; break; - case LevelOfDetail.Low: hollowSides = 6; break; - case LevelOfDetail.VeryLow: hollowSides = 3; break; - default: hollowSides = 24; break; - } - } - else if (primShape.HollowShape == HollowShape.Square) - hollowSides = 4; - else if (primShape.HollowShape == HollowShape.Triangle) - { - if (profshape == (byte)ProfileShape.HalfCircle) - hollowSides = 6; - else - hollowSides = 3; - } - - primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides); - - if (primMesh.errorMessage != null) - if (primMesh.errorMessage.Length > 0) - m_log.Error("[ERROR] " + primMesh.errorMessage); - - primMesh.topShearX = pathShearX; - primMesh.topShearY = pathShearY; - primMesh.pathCutBegin = pathBegin; - primMesh.pathCutEnd = pathEnd; - - if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible) - { - primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10; - primMesh.twistEnd = (primShape.PathTwist * 18) / 10; - primMesh.taperX = pathScaleX; - primMesh.taperY = pathScaleY; - -#if SPAM - m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString()); -#endif - try - { - primMesh.ExtrudeLinear(); - } - catch (Exception ex) - { - ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); - return false; - } - } - else - { - primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f; - primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f; - primMesh.radius = 0.01f * primShape.PathRadiusOffset; - primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions; - primMesh.skew = 0.01f * primShape.PathSkew; - primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10; - primMesh.twistEnd = (primShape.PathTwist * 36) / 10; - primMesh.taperX = primShape.PathTaperX * 0.01f; - primMesh.taperY = primShape.PathTaperY * 0.01f; - -#if SPAM - m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString()); -#endif - try - { - primMesh.ExtrudeCircular(); - } - catch (Exception ex) - { - ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); - return false; - } - } - -// primMesh.DumpRaw(baseDir, primName, "primMesh"); - - coords = primMesh.coords; - faces = primMesh.faces; - - return true; - } - - public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex) - { - AMeshKey key = new AMeshKey(); - Byte[] someBytes; - - key.hashB = 5181; - key.hashC = 5181; - ulong hash = 5381; - - if (primShape.SculptEntry) - { - key.uuid = primShape.SculptTexture; - key.hashC = mdjb2(key.hashC, primShape.SculptType); - key.hashC = mdjb2(key.hashC, primShape.PCode); - } - else - { - hash = mdjb2(hash, primShape.PathCurve); - hash = mdjb2(hash, (byte)primShape.HollowShape); - hash = mdjb2(hash, (byte)primShape.ProfileShape); - hash = mdjb2(hash, primShape.PathBegin); - hash = mdjb2(hash, primShape.PathEnd); - hash = mdjb2(hash, primShape.PathScaleX); - hash = mdjb2(hash, primShape.PathScaleY); - hash = mdjb2(hash, primShape.PathShearX); - key.hashA = hash; - hash = key.hashB; - hash = mdjb2(hash, primShape.PathShearY); - hash = mdjb2(hash, (byte)primShape.PathTwist); - hash = mdjb2(hash, (byte)primShape.PathTwistBegin); - hash = mdjb2(hash, (byte)primShape.PathRadiusOffset); - hash = mdjb2(hash, (byte)primShape.PathTaperX); - hash = mdjb2(hash, (byte)primShape.PathTaperY); - hash = mdjb2(hash, primShape.PathRevolutions); - hash = mdjb2(hash, (byte)primShape.PathSkew); - hash = mdjb2(hash, primShape.ProfileBegin); - hash = mdjb2(hash, primShape.ProfileEnd); - hash = mdjb2(hash, primShape.ProfileHollow); - hash = mdjb2(hash, primShape.PCode); - key.hashB = hash; - } - - hash = key.hashC; - - hash = mdjb2(hash, lod); - - if (size == m_MeshUnitSize) - { - hash = hash << 8; - hash |= 8; - } - else - { - someBytes = size.GetBytes(); - for (int i = 0; i < someBytes.Length; i++) - hash = mdjb2(hash, someBytes[i]); - hash = hash << 8; - } - - if (convex) - hash |= 4; - - if (primShape.SculptEntry) - { - hash |= 1; - if (primShape.SculptType == (byte)SculptType.Mesh) - hash |= 2; - } - - key.hashC = hash; - - return key; - } - - private ulong mdjb2(ulong hash, byte c) - { - return ((hash << 5) + hash) + (ulong)c; - } - - private ulong mdjb2(ulong hash, ushort c) - { - hash = ((hash << 5) + hash) + (ulong)((byte)c); - return ((hash << 5) + hash) + (ulong)(c >> 8); - } - - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) - { - return CreateMesh(primName, primShape, size, lod, false,false,false); - } - - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) - { - return CreateMesh(primName, primShape, size, lod, false,false,false); - } - - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) - { - return CreateMesh(primName, primShape, size, lod, false, false, false); - } - - public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) - { - Mesh mesh = null; - - if (size.X < 0.01f) size.X = 0.01f; - if (size.Y < 0.01f) size.Y = 0.01f; - if (size.Z < 0.01f) size.Z = 0.01f; - - AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex); - lock (m_uniqueMeshes) - { - m_uniqueMeshes.TryGetValue(key, out mesh); - - if (mesh != null) - { - mesh.RefCount++; - return mesh; - } - - // try to find a identical mesh on meshs recently released - lock (m_uniqueReleasedMeshes) - { - m_uniqueReleasedMeshes.TryGetValue(key, out mesh); - if (mesh != null) - { - m_uniqueReleasedMeshes.Remove(key); - try - { - m_uniqueMeshes.Add(key, mesh); - } - catch { } - mesh.RefCount = 1; - return mesh; - } - } - } - return null; - } - - private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f); - - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) - { -#if SPAM - m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); -#endif - - Mesh mesh = null; - - if (size.X < 0.01f) size.X = 0.01f; - if (size.Y < 0.01f) size.Y = 0.01f; - if (size.Z < 0.01f) size.Z = 0.01f; - - // try to find a identical mesh on meshs in use - - AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex); - - lock (m_uniqueMeshes) - { - m_uniqueMeshes.TryGetValue(key, out mesh); - - if (mesh != null) - { - mesh.RefCount++; - return mesh; - } - - // try to find a identical mesh on meshs recently released - lock (m_uniqueReleasedMeshes) - { - m_uniqueReleasedMeshes.TryGetValue(key, out mesh); - if (mesh != null) - { - m_uniqueReleasedMeshes.Remove(key); - try - { - m_uniqueMeshes.Add(key, mesh); - } - catch { } - mesh.RefCount = 1; - return mesh; - } - } - } - - Mesh UnitMesh = null; - AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex); - - lock (m_uniqueReleasedMeshes) - { - m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh); - if (UnitMesh != null) - { - UnitMesh.RefCount = 1; - } - } - - if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache) - UnitMesh = GetFromFileCache(unitKey); - - if (UnitMesh == null) - { - UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex); - - if (UnitMesh == null) - return null; - - UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z"); - - if (forOde) - { - // force pinned mem allocation - UnitMesh.PrepForOde(); - } - else - UnitMesh.TrimExcess(); - - UnitMesh.Key = unitKey; - UnitMesh.RefCount = 1; - - if (doMeshFileCache && primShape.SculptEntry) - StoreToFileCache(unitKey, UnitMesh); - - lock (m_uniqueReleasedMeshes) - { - try - { - m_uniqueReleasedMeshes.Add(unitKey, UnitMesh); - } - catch { } - } - } - - mesh = UnitMesh.Scale(size); - mesh.Key = key; - mesh.RefCount = 1; - lock (m_uniqueMeshes) - { - try - { - m_uniqueMeshes.Add(key, mesh); - } - catch { } - } - - return mesh; - } - - public void ReleaseMesh(IMesh imesh) - { - if (imesh == null) - return; - - Mesh mesh = (Mesh)imesh; - - lock (m_uniqueMeshes) - { - int curRefCount = mesh.RefCount; - curRefCount--; - - if (curRefCount > 0) - { - mesh.RefCount = curRefCount; - return; - } - - mesh.RefCount = 0; - m_uniqueMeshes.Remove(mesh.Key); - lock (m_uniqueReleasedMeshes) - { - try - { - m_uniqueReleasedMeshes.Add(mesh.Key, mesh); - } - catch { } - } - } - } - - public void ExpireReleaseMeshs() - { - if (m_uniqueReleasedMeshes.Count == 0) - return; - - List meshstodelete = new List(); - int refcntr; - - lock (m_uniqueReleasedMeshes) - { - foreach (Mesh m in m_uniqueReleasedMeshes.Values) - { - refcntr = m.RefCount; - refcntr--; - if (refcntr > -6) - m.RefCount = refcntr; - else - meshstodelete.Add(m); - } - - foreach (Mesh m in meshstodelete) - { - m_uniqueReleasedMeshes.Remove(m.Key); - m.releaseBuildingMeshData(); - m.releasePinned(); - } - } - } - - public void FileNames(AMeshKey key, out string dir,out string fullFileName) - { - string id = key.ToString(); - string init = id.Substring(0, 1); - dir = System.IO.Path.Combine(cachePath, init); - fullFileName = System.IO.Path.Combine(dir, id); - } - - public string FullFileName(AMeshKey key) - { - string id = key.ToString(); - string init = id.Substring(0,1); - id = System.IO.Path.Combine(init, id); - id = System.IO.Path.Combine(cachePath, id); - return id; - } - - private Mesh GetFromFileCache(AMeshKey key) - { - Mesh mesh = null; - string filename = FullFileName(key); - bool ok = true; - - lock (diskLock) - { - if (File.Exists(filename)) - { - FileStream stream = null; - try - { - stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read); - BinaryFormatter bformatter = new BinaryFormatter(); - - mesh = Mesh.FromStream(stream, key); - - } - catch (Exception e) - { - ok = false; - m_log.ErrorFormat( - "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}", - filename, e.Message, e.StackTrace); - } - - if (stream != null) - stream.Close(); - - if (mesh == null || !ok) - File.Delete(filename); - else - File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); - } - } - - return mesh; - } - - private void StoreToFileCache(AMeshKey key, Mesh mesh) - { - Stream stream = null; - bool ok = false; - - // Make sure the target cache directory exists - string dir = String.Empty; - string filename = String.Empty; - - FileNames(key, out dir, out filename); - - lock (diskLock) - { - try - { - if (!Directory.Exists(dir)) - { - Directory.CreateDirectory(dir); - } - - stream = File.Open(filename, FileMode.Create); - ok = mesh.ToStream(stream); - } - catch (IOException e) - { - m_log.ErrorFormat( - "[MESH CACHE]: Failed to write file {0}. Exception {1} {2}.", - filename, e.Message, e.StackTrace); - ok = false; - } - - if (stream != null) - stream.Close(); - - if (File.Exists(filename)) - { - if (ok) - File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); - else - File.Delete(filename); - } - } - } - - public void ExpireFileCache() - { - if (!doCacheExpire) - return; - - string controlfile = System.IO.Path.Combine(cachePath, "cntr"); - - lock (diskLock) - { - try - { - if (File.Exists(controlfile)) - { - int ndeleted = 0; - int totalfiles = 0; - int ndirs = 0; - DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire; - File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow); - - foreach (string dir in Directory.GetDirectories(cachePath)) - { - try - { - foreach (string file in Directory.GetFiles(dir)) - { - try - { - if (File.GetLastAccessTimeUtc(file) < OlderTime) - { - File.Delete(file); - ndeleted++; - } - } - catch { } - totalfiles++; - } - } - catch { } - ndirs++; - } - - if (ndeleted == 0) - m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires", - totalfiles,ndirs); - else - m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}", - totalfiles,ndirs, ndeleted, OlderTime.ToString()); - } - else - { - m_log.Info("[MESH CACHE]: Expire delayed to next startup"); - FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough); - fs.Close(); - } - } - catch { } - } - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/PrimMesher.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/PrimMesher.cs deleted file mode 100644 index 8eb136b..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/PrimMesher.cs +++ /dev/null @@ -1,1708 +0,0 @@ -/* - * Copyright (c) Contributors - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.Text; -using System.IO; - -namespace PrimMesher -{ - public struct Quat - { - /// X value - public float X; - /// Y value - public float Y; - /// Z value - public float Z; - /// W value - public float W; - - public Quat(float x, float y, float z, float w) - { - X = x; - Y = y; - Z = z; - W = w; - } - - public Quat(Coord axis, float angle) - { - axis = axis.Normalize(); - - angle *= 0.5f; - float c = (float)Math.Cos(angle); - float s = (float)Math.Sin(angle); - - X = axis.X * s; - Y = axis.Y * s; - Z = axis.Z * s; - W = c; - - Normalize(); - } - - public float Length() - { - return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); - } - - public Quat Normalize() - { - const float MAG_THRESHOLD = 0.0000001f; - float mag = Length(); - - // Catch very small rounding errors when normalizing - if (mag > MAG_THRESHOLD) - { - float oomag = 1f / mag; - X *= oomag; - Y *= oomag; - Z *= oomag; - W *= oomag; - } - else - { - X = 0f; - Y = 0f; - Z = 0f; - W = 1f; - } - - return this; - } - - public static Quat operator *(Quat q1, Quat q2) - { - float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; - float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X; - float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W; - float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; - return new Quat(x, y, z, w); - } - - public override string ToString() - { - return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">"; - } - } - - public struct Coord - { - public float X; - public float Y; - public float Z; - - public Coord(float x, float y, float z) - { - this.X = x; - this.Y = y; - this.Z = z; - } - - public float Length() - { - return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z); - } - - public Coord Invert() - { - this.X = -this.X; - this.Y = -this.Y; - this.Z = -this.Z; - - return this; - } - - public Coord Normalize() - { - const float MAG_THRESHOLD = 0.0000001f; - float mag = Length(); - - // Catch very small rounding errors when normalizing - if (mag > MAG_THRESHOLD) - { - float oomag = 1.0f / mag; - this.X *= oomag; - this.Y *= oomag; - this.Z *= oomag; - } - else - { - this.X = 0.0f; - this.Y = 0.0f; - this.Z = 0.0f; - } - - return this; - } - - public override string ToString() - { - return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString(); - } - - public static Coord Cross(Coord c1, Coord c2) - { - return new Coord( - c1.Y * c2.Z - c2.Y * c1.Z, - c1.Z * c2.X - c2.Z * c1.X, - c1.X * c2.Y - c2.X * c1.Y - ); - } - - public static Coord operator +(Coord v, Coord a) - { - return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z); - } - - public static Coord operator *(Coord v, Coord m) - { - return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z); - } - - public static Coord operator *(Coord v, Quat q) - { - // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ - - Coord c2 = new Coord(0.0f, 0.0f, 0.0f); - - c2.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; - - c2.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; - - c2.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 c2; - } - } - - public struct Face - { - public int primFace; - - // vertices - public int v1; - public int v2; - public int v3; - - public Face(int v1, int v2, int v3) - { - primFace = 0; - - this.v1 = v1; - this.v2 = v2; - this.v3 = v3; - - } - - public Coord SurfaceNormal(List coordList) - { - Coord c1 = coordList[this.v1]; - Coord c2 = coordList[this.v2]; - Coord c3 = coordList[this.v3]; - - Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); - Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); - - return Coord.Cross(edge1, edge2).Normalize(); - } - } - - internal struct Angle - { - internal float angle; - internal float X; - internal float Y; - - internal Angle(float angle, float x, float y) - { - this.angle = angle; - this.X = x; - this.Y = y; - } - } - - internal class AngleList - { - private float iX, iY; // intersection point - - private static Angle[] angles3 = - { - new Angle(0.0f, 1.0f, 0.0f), - new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), - new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), - new Angle(1.0f, 1.0f, 0.0f) - }; - - private static Angle[] angles4 = - { - new Angle(0.0f, 1.0f, 0.0f), - new Angle(0.25f, 0.0f, 1.0f), - new Angle(0.5f, -1.0f, 0.0f), - new Angle(0.75f, 0.0f, -1.0f), - new Angle(1.0f, 1.0f, 0.0f) - }; - - private static Angle[] angles6 = - { - new Angle(0.0f, 1.0f, 0.0f), - new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), - new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), - new Angle(0.5f, -1.0f, 0.0f), - new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), - new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), - new Angle(1.0f, 1.0f, 0.0f) - }; - - private static Angle[] angles12 = - { - new Angle(0.0f, 1.0f, 0.0f), - new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), - new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), - new Angle(0.25f, 0.0f, 1.0f), - new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), - new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), - new Angle(0.5f, -1.0f, 0.0f), - new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), - new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), - new Angle(0.75f, 0.0f, -1.0f), - new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), - new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), - new Angle(1.0f, 1.0f, 0.0f) - }; - - private static Angle[] angles24 = - { - new Angle(0.0f, 1.0f, 0.0f), - new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f), - new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), - new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f), - new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), - new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f), - new Angle(0.25f, 0.0f, 1.0f), - new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f), - new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), - new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f), - new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), - new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f), - new Angle(0.5f, -1.0f, 0.0f), - new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f), - new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), - new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f), - new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), - new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f), - new Angle(0.75f, 0.0f, -1.0f), - new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f), - new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), - new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f), - new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), - new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f), - new Angle(1.0f, 1.0f, 0.0f) - }; - - private Angle interpolatePoints(float newPoint, Angle p1, Angle p2) - { - float m = (newPoint - p1.angle) / (p2.angle - p1.angle); - return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y)); - } - - private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) - { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ - double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); - double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3); - - if (denom != 0.0) - { - double ua = uaNumerator / denom; - iX = (float)(x1 + ua * (x2 - x1)); - iY = (float)(y1 + ua * (y2 - y1)); - } - } - - internal List angles; - - internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut) - { - angles = new List(); - - const double twoPi = System.Math.PI * 2.0; - const float twoPiInv = (float)(1.0d / twoPi); - - if (sides < 1) - throw new Exception("number of sides not greater than zero"); - if (stopAngle <= startAngle) - throw new Exception("stopAngle not greater than startAngle"); - - if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24)) - { - startAngle *= twoPiInv; - stopAngle *= twoPiInv; - - Angle[] sourceAngles; - switch (sides) - { - case 3: - sourceAngles = angles3; - break; - case 4: - sourceAngles = angles4; - break; - case 6: - sourceAngles = angles6; - break; - case 12: - sourceAngles = angles12; - break; - default: - sourceAngles = angles24; - break; - } - - int startAngleIndex = (int)(startAngle * sides); - int endAngleIndex = sourceAngles.Length - 1; - - if (hasCut) - { - if (stopAngle < 1.0f) - endAngleIndex = (int)(stopAngle * sides) + 1; - if (endAngleIndex == startAngleIndex) - endAngleIndex++; - - for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) - { - angles.Add(sourceAngles[angleIndex]); - } - - if (startAngle > 0.0f) - angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); - - if (stopAngle < 1.0f) - { - int lastAngleIndex = angles.Count - 1; - angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); - } - } - else - { - for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++) - angles.Add(sourceAngles[angleIndex]); - } - } - else - { - double stepSize = twoPi / sides; - - int startStep = (int)(startAngle / stepSize); - double angle = stepSize * startStep; - int step = startStep; - double stopAngleTest = stopAngle; - if (stopAngle < twoPi) - { - stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1); - if (stopAngleTest < stopAngle) - stopAngleTest += stepSize; - if (stopAngleTest > twoPi) - stopAngleTest = twoPi; - } - - while (angle <= stopAngleTest) - { - Angle newAngle; - newAngle.angle = (float)angle; - newAngle.X = (float)System.Math.Cos(angle); - newAngle.Y = (float)System.Math.Sin(angle); - angles.Add(newAngle); - step += 1; - angle = stepSize * step; - } - - if (startAngle > angles[0].angle) - { - Angle newAngle; - intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle)); - newAngle.angle = startAngle; - newAngle.X = iX; - newAngle.Y = iY; - angles[0] = newAngle; - } - - int index = angles.Count - 1; - if (stopAngle < angles[index].angle) - { - Angle newAngle; - intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle)); - newAngle.angle = stopAngle; - newAngle.X = iX; - newAngle.Y = iY; - angles[index] = newAngle; - } - } - } - } - - /// - /// generates a profile for extrusion - /// - public class Profile - { - private const float twoPi = 2.0f * (float)Math.PI; - - public string errorMessage = null; - - public List coords; - public List faces; - - // use these for making individual meshes for each prim face - public List outerCoordIndices = null; - public List hollowCoordIndices = null; - - public int numOuterVerts = 0; - public int numHollowVerts = 0; - - public int outerFaceNumber = -1; - public int hollowFaceNumber = -1; - - public int bottomFaceNumber = 0; - public int numPrimFaces = 0; - - public Profile() - { - this.coords = new List(); - this.faces = new List(); - } - - public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces) - { - const float halfSqr2 = 0.7071067811866f; - - this.coords = new List(); - this.faces = new List(); - - List hollowCoords = new List(); - - bool hasHollow = (hollow > 0.0f); - - AngleList angles = new AngleList(); - AngleList hollowAngles = new AngleList(); - - float xScale = 0.5f; - float yScale = 0.5f; - if (sides == 4) // corners of a square are sqrt(2) from center - { - xScale = halfSqr2; - yScale = halfSqr2; - } - - float startAngle = profileStart * twoPi; - float stopAngle = profileEnd * twoPi; - - try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); } - catch (Exception ex) - { - - errorMessage = "makeAngles failed: Exception: " + ex.ToString() - + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); - - return; - } - - this.numOuterVerts = angles.angles.Count; - - Angle angle; - Coord newVert = new Coord(); - - // flag to create as few triangles as possible for 3 or 4 side profile - bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut); - - if (hasHollow) - { - if (sides == hollowSides) - hollowAngles = angles; - else - { - try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); } - catch (Exception ex) - { - errorMessage = "makeAngles failed: Exception: " + ex.ToString() - + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); - - return; - } - - int numHollowAngles = hollowAngles.angles.Count; - for (int i = 0; i < numHollowAngles; i++) - { - angle = hollowAngles.angles[i]; - newVert.X = hollow * xScale * angle.X; - newVert.Y = hollow * yScale * angle.Y; - newVert.Z = 0.0f; - - hollowCoords.Add(newVert); - } - } - this.numHollowVerts = hollowAngles.angles.Count; - } - else if (!simpleFace) - { - Coord center = new Coord(0.0f, 0.0f, 0.0f); - this.coords.Add(center); - } - - int numAngles = angles.angles.Count; - bool hollowsame = (hasHollow && hollowSides == sides); - - for (int i = 0; i < numAngles; i++) - { - angle = angles.angles[i]; - newVert.X = angle.X * xScale; - newVert.Y = angle.Y * yScale; - newVert.Z = 0.0f; - this.coords.Add(newVert); - if (hollowsame) - { - newVert.X *= hollow; - newVert.Y *= hollow; - hollowCoords.Add(newVert); - } - } - - if (hasHollow) - { - hollowCoords.Reverse(); - this.coords.AddRange(hollowCoords); - - if (createFaces) - { - int numTotalVerts = this.numOuterVerts + this.numHollowVerts; - - if (this.numOuterVerts == this.numHollowVerts) - { - Face newFace = new Face(); - - for (int coordIndex = 0; coordIndex < this.numOuterVerts - 1; coordIndex++) - { - newFace.v1 = coordIndex; - newFace.v2 = coordIndex + 1; - newFace.v3 = numTotalVerts - coordIndex - 1; - this.faces.Add(newFace); - - newFace.v1 = coordIndex + 1; - newFace.v2 = numTotalVerts - coordIndex - 2; - newFace.v3 = numTotalVerts - coordIndex - 1; - this.faces.Add(newFace); - } - if (!hasProfileCut) - { - newFace.v1 = this.numOuterVerts - 1; - newFace.v2 = 0; - newFace.v3 = this.numOuterVerts; - this.faces.Add(newFace); - - newFace.v1 = 0; - newFace.v2 = numTotalVerts - 1; - newFace.v3 = this.numOuterVerts; - this.faces.Add(newFace); - } - } - else if (this.numOuterVerts < this.numHollowVerts) - { - Face newFace = new Face(); - int j = 0; // j is the index for outer vertices - int i; - int maxJ = this.numOuterVerts - 1; - float curHollowAngle = 0; - for (i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices - { - curHollowAngle = hollowAngles.angles[i].angle; - if (j < maxJ) - { - if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) - { - newFace.v1 = numTotalVerts - i - 1; - newFace.v2 = j; - newFace.v3 = j + 1; - this.faces.Add(newFace); - j++; - } - } - else - { - if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) - break; - } - - newFace.v1 = j; - newFace.v2 = numTotalVerts - i - 2; - newFace.v3 = numTotalVerts - i - 1; - - this.faces.Add(newFace); - } - - if (!hasProfileCut) - { - if (i == this.numHollowVerts) - { - newFace.v1 = numTotalVerts - this.numHollowVerts; - newFace.v2 = maxJ; - newFace.v3 = 0; - - this.faces.Add(newFace); - } - else - { - if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f) - { - newFace.v1 = numTotalVerts - i - 1; - newFace.v2 = maxJ; - newFace.v3 = 0; - - this.faces.Add(newFace); - } - - for (; i < this.numHollowVerts - 1; i++) - { - newFace.v1 = 0; - newFace.v2 = numTotalVerts - i - 2; - newFace.v3 = numTotalVerts - i - 1; - - this.faces.Add(newFace); - } - } - - newFace.v1 = 0; - newFace.v2 = numTotalVerts - this.numHollowVerts; - newFace.v3 = numTotalVerts - 1; - this.faces.Add(newFace); - } - } - else // numHollowVerts < numOuterVerts - { - Face newFace = new Face(); - int j = 0; // j is the index for inner vertices - int maxJ = this.numHollowVerts - 1; - for (int i = 0; i < this.numOuterVerts; i++) - { - if (j < maxJ) - if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) - { - newFace.v1 = i; - newFace.v2 = numTotalVerts - j - 2; - newFace.v3 = numTotalVerts - j - 1; - - this.faces.Add(newFace); - j += 1; - } - - newFace.v1 = numTotalVerts - j - 1; - newFace.v2 = i; - newFace.v3 = i + 1; - - this.faces.Add(newFace); - } - - if (!hasProfileCut) - { - int i = this.numOuterVerts - 1; - - if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f) - { - newFace.v1 = 0; - newFace.v2 = numTotalVerts - maxJ - 1; - newFace.v3 = numTotalVerts - 1; - - this.faces.Add(newFace); - } - - newFace.v1 = numTotalVerts - maxJ - 1; - newFace.v2 = i; - newFace.v3 = 0; - - this.faces.Add(newFace); - } - } - } - - } - - else if (createFaces) - { - if (simpleFace) - { - if (sides == 3) - this.faces.Add(new Face(0, 1, 2)); - else if (sides == 4) - { - this.faces.Add(new Face(0, 1, 2)); - this.faces.Add(new Face(0, 2, 3)); - } - } - else - { - for (int i = 1; i < numAngles ; i++) - { - Face newFace = new Face(); - newFace.v1 = 0; - newFace.v2 = i; - newFace.v3 = i + 1; - this.faces.Add(newFace); - } - if (!hasProfileCut) - { - Face newFace = new Face(); - newFace.v1 = 0; - newFace.v2 = numAngles; - newFace.v3 = 1; - this.faces.Add(newFace); - } - } - } - - - hollowCoords = null; - } - - - public Profile Copy() - { - return this.Copy(true); - } - - public Profile Copy(bool needFaces) - { - Profile copy = new Profile(); - - copy.coords.AddRange(this.coords); - - if (needFaces) - copy.faces.AddRange(this.faces); - - copy.numOuterVerts = this.numOuterVerts; - copy.numHollowVerts = this.numHollowVerts; - - return copy; - } - - public void AddPos(Coord v) - { - this.AddPos(v.X, v.Y, v.Z); - } - - public void AddPos(float x, float y, float z) - { - int i; - int numVerts = this.coords.Count; - Coord vert; - - for (i = 0; i < numVerts; i++) - { - vert = this.coords[i]; - vert.X += x; - vert.Y += y; - vert.Z += z; - this.coords[i] = vert; - } - } - - public void AddRot(Quat q) - { - int i; - int numVerts = this.coords.Count; - - for (i = 0; i < numVerts; i++) - this.coords[i] *= q; - } - - public void Scale(float x, float y) - { - int i; - int numVerts = this.coords.Count; - Coord vert; - - for (i = 0; i < numVerts; i++) - { - vert = this.coords[i]; - vert.X *= x; - vert.Y *= y; - this.coords[i] = vert; - } - } - - /// - /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices - /// - public void FlipNormals() - { - int i; - int numFaces = this.faces.Count; - Face tmpFace; - int tmp; - - for (i = 0; i < numFaces; i++) - { - tmpFace = this.faces[i]; - tmp = tmpFace.v3; - tmpFace.v3 = tmpFace.v1; - tmpFace.v1 = tmp; - this.faces[i] = tmpFace; - } - } - - public void AddValue2FaceVertexIndices(int num) - { - int numFaces = this.faces.Count; - Face tmpFace; - for (int i = 0; i < numFaces; i++) - { - tmpFace = this.faces[i]; - tmpFace.v1 += num; - tmpFace.v2 += num; - tmpFace.v3 += num; - - this.faces[i] = tmpFace; - } - } - - public void DumpRaw(String path, String name, String title) - { - if (path == null) - return; - String fileName = name + "_" + title + ".raw"; - String completePath = System.IO.Path.Combine(path, fileName); - StreamWriter sw = new StreamWriter(completePath); - - for (int i = 0; i < this.faces.Count; i++) - { - string s = this.coords[this.faces[i].v1].ToString(); - s += " " + this.coords[this.faces[i].v2].ToString(); - s += " " + this.coords[this.faces[i].v3].ToString(); - - sw.WriteLine(s); - } - - sw.Close(); - } - } - - public struct PathNode - { - public Coord position; - public Quat rotation; - public float xScale; - public float yScale; - public float percentOfPath; - } - - public enum PathType { Linear = 0, Circular = 1, Flexible = 2 } - - public class Path - { - public List pathNodes = new List(); - - public float twistBegin = 0.0f; - public float twistEnd = 0.0f; - public float topShearX = 0.0f; - public float topShearY = 0.0f; - public float pathCutBegin = 0.0f; - public float pathCutEnd = 1.0f; - public float dimpleBegin = 0.0f; - public float dimpleEnd = 1.0f; - public float skew = 0.0f; - public float holeSizeX = 1.0f; // called pathScaleX in pbs - public float holeSizeY = 0.25f; - public float taperX = 0.0f; - public float taperY = 0.0f; - public float radius = 0.0f; - public float revolutions = 1.0f; - public int stepsPerRevolution = 24; - - private const float twoPi = 2.0f * (float)Math.PI; - - public void Create(PathType pathType, int steps) - { - if (this.taperX > 0.999f) - this.taperX = 0.999f; - if (this.taperX < -0.999f) - this.taperX = -0.999f; - if (this.taperY > 0.999f) - this.taperY = 0.999f; - if (this.taperY < -0.999f) - this.taperY = -0.999f; - - if (pathType == PathType.Linear || pathType == PathType.Flexible) - { - int step = 0; - - float length = this.pathCutEnd - this.pathCutBegin; - float twistTotal = twistEnd - twistBegin; - float twistTotalAbs = Math.Abs(twistTotal); - if (twistTotalAbs > 0.01f) - steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number - - float start = -0.5f; - float stepSize = length / (float)steps; - float percentOfPathMultiplier = stepSize * 0.999999f; - float xOffset = this.topShearX * this.pathCutBegin; - float yOffset = this.topShearY * this.pathCutBegin; - float zOffset = start; - float xOffsetStepIncrement = this.topShearX * length / steps; - float yOffsetStepIncrement = this.topShearY * length / steps; - - float percentOfPath = this.pathCutBegin; - zOffset += percentOfPath; - - // sanity checks - - bool done = false; - - while (!done) - { - PathNode newNode = new PathNode(); - - newNode.xScale = 1.0f; - if (this.taperX == 0.0f) - newNode.xScale = 1.0f; - else if (this.taperX > 0.0f) - newNode.xScale = 1.0f - percentOfPath * this.taperX; - else newNode.xScale = 1.0f + (1.0f - percentOfPath) * this.taperX; - - newNode.yScale = 1.0f; - if (this.taperY == 0.0f) - newNode.yScale = 1.0f; - else if (this.taperY > 0.0f) - newNode.yScale = 1.0f - percentOfPath * this.taperY; - else newNode.yScale = 1.0f + (1.0f - percentOfPath) * this.taperY; - - float twist = twistBegin + twistTotal * percentOfPath; - - newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); - newNode.position = new Coord(xOffset, yOffset, zOffset); - newNode.percentOfPath = percentOfPath; - - pathNodes.Add(newNode); - - if (step < steps) - { - step += 1; - percentOfPath += percentOfPathMultiplier; - xOffset += xOffsetStepIncrement; - yOffset += yOffsetStepIncrement; - zOffset += stepSize; - if (percentOfPath > this.pathCutEnd) - done = true; - } - else done = true; - } - } // end of linear path code - - else // pathType == Circular - { - float twistTotal = twistEnd - twistBegin; - - // if the profile has a lot of twist, add more layers otherwise the layers may overlap - // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't - // accurately match the viewer - float twistTotalAbs = Math.Abs(twistTotal); - if (twistTotalAbs > 0.01f) - { - if (twistTotalAbs > Math.PI * 1.5f) - steps *= 2; - if (twistTotalAbs > Math.PI * 3.0f) - steps *= 2; - } - - float yPathScale = this.holeSizeY * 0.5f; - float pathLength = this.pathCutEnd - this.pathCutBegin; - float totalSkew = this.skew * 2.0f * pathLength; - float skewStart = this.pathCutBegin * 2.0f * this.skew - this.skew; - float xOffsetTopShearXFactor = this.topShearX * (0.25f + 0.5f * (0.5f - this.holeSizeY)); - float yShearCompensation = 1.0f + Math.Abs(this.topShearY) * 0.25f; - - // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end - // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used - // to calculate the sine for generating the path radius appears to approximate it's effects there - // too, but there are some subtle differences in the radius which are noticeable as the prim size - // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on - // the meshes generated with this technique appear nearly identical in shape to the same prims when - // displayed by the viewer. - - float startAngle = (twoPi * this.pathCutBegin * this.revolutions) - this.topShearY * 0.9f; - float endAngle = (twoPi * this.pathCutEnd * this.revolutions) - this.topShearY * 0.9f; - float stepSize = twoPi / this.stepsPerRevolution; - - int step = (int)(startAngle / stepSize); - float angle = startAngle; - - bool done = false; - while (!done) // loop through the length of the path and add the layers - { - PathNode newNode = new PathNode(); - - float xProfileScale = (1.0f - Math.Abs(this.skew)) * this.holeSizeX; - float yProfileScale = this.holeSizeY; - - float percentOfPath = angle / (twoPi * this.revolutions); - float percentOfAngles = (angle - startAngle) / (endAngle - startAngle); - - if (this.taperX > 0.01f) - xProfileScale *= 1.0f - percentOfPath * this.taperX; - else if (this.taperX < -0.01f) - xProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperX; - - if (this.taperY > 0.01f) - yProfileScale *= 1.0f - percentOfPath * this.taperY; - else if (this.taperY < -0.01f) - yProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperY; - - newNode.xScale = xProfileScale; - newNode.yScale = yProfileScale; - - float radiusScale = 1.0f; - if (this.radius > 0.001f) - radiusScale = 1.0f - this.radius * percentOfPath; - else if (this.radius < 0.001f) - radiusScale = 1.0f + this.radius * (1.0f - percentOfPath); - - float twist = twistBegin + twistTotal * percentOfPath; - - float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles); - xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor; - - float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale; - - float zOffset = (float)Math.Sin(angle + this.topShearY) * (0.5f - yPathScale) * radiusScale; - - newNode.position = new Coord(xOffset, yOffset, zOffset); - - // now orient the rotation of the profile layer relative to it's position on the path - // adding taperY to the angle used to generate the quat appears to approximate the viewer - - newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + this.topShearY); - - // next apply twist rotation to the profile layer - if (twistTotal != 0.0f || twistBegin != 0.0f) - newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); - - newNode.percentOfPath = percentOfPath; - - pathNodes.Add(newNode); - - // calculate terms for next iteration - // calculate the angle for the next iteration of the loop - - if (angle >= endAngle - 0.01) - done = true; - else - { - step += 1; - angle = stepSize * step; - if (angle > endAngle) - angle = endAngle; - } - } - } - } - } - - public class PrimMesh - { - public string errorMessage = ""; - private const float twoPi = 2.0f * (float)Math.PI; - - public List coords; -// public List normals; - public List faces; - - private int sides = 4; - private int hollowSides = 4; - private float profileStart = 0.0f; - private float profileEnd = 1.0f; - private float hollow = 0.0f; - public int twistBegin = 0; - public int twistEnd = 0; - public float topShearX = 0.0f; - public float topShearY = 0.0f; - public float pathCutBegin = 0.0f; - public float pathCutEnd = 1.0f; - public float dimpleBegin = 0.0f; - public float dimpleEnd = 1.0f; - public float skew = 0.0f; - public float holeSizeX = 1.0f; // called pathScaleX in pbs - public float holeSizeY = 0.25f; - public float taperX = 0.0f; - public float taperY = 0.0f; - public float radius = 0.0f; - public float revolutions = 1.0f; - public int stepsPerRevolution = 24; - - private bool hasProfileCut = false; - private bool hasHollow = false; - - public int numPrimFaces = 0; - - /// - /// Human readable string representation of the parameters used to create a mesh. - /// - /// - public string ParamsToDisplayString() - { - string s = ""; - s += "sides..................: " + this.sides.ToString(); - s += "\nhollowSides..........: " + this.hollowSides.ToString(); - s += "\nprofileStart.........: " + this.profileStart.ToString(); - s += "\nprofileEnd...........: " + this.profileEnd.ToString(); - s += "\nhollow...............: " + this.hollow.ToString(); - s += "\ntwistBegin...........: " + this.twistBegin.ToString(); - s += "\ntwistEnd.............: " + this.twistEnd.ToString(); - s += "\ntopShearX............: " + this.topShearX.ToString(); - s += "\ntopShearY............: " + this.topShearY.ToString(); - s += "\npathCutBegin.........: " + this.pathCutBegin.ToString(); - s += "\npathCutEnd...........: " + this.pathCutEnd.ToString(); - s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString(); - s += "\ndimpleEnd............: " + this.dimpleEnd.ToString(); - s += "\nskew.................: " + this.skew.ToString(); - s += "\nholeSizeX............: " + this.holeSizeX.ToString(); - s += "\nholeSizeY............: " + this.holeSizeY.ToString(); - s += "\ntaperX...............: " + this.taperX.ToString(); - s += "\ntaperY...............: " + this.taperY.ToString(); - s += "\nradius...............: " + this.radius.ToString(); - s += "\nrevolutions..........: " + this.revolutions.ToString(); - s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString(); - s += "\nhasProfileCut........: " + this.hasProfileCut.ToString(); - s += "\nhasHollow............: " + this.hasHollow.ToString(); - - return s; - } - - public bool HasProfileCut - { - get { return hasProfileCut; } - set { hasProfileCut = value; } - } - - public bool HasHollow - { - get { return hasHollow; } - } - - - /// - /// Constructs a PrimMesh object and creates the profile for extrusion. - /// - /// - /// - /// - /// - /// - /// - public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides) - { - this.coords = new List(); - this.faces = new List(); - - this.sides = sides; - this.profileStart = profileStart; - this.profileEnd = profileEnd; - this.hollow = hollow; - this.hollowSides = hollowSides; - - if (sides < 3) - this.sides = 3; - if (hollowSides < 3) - this.hollowSides = 3; - if (profileStart < 0.0f) - this.profileStart = 0.0f; - if (profileEnd > 1.0f) - this.profileEnd = 1.0f; - if (profileEnd < 0.02f) - this.profileEnd = 0.02f; - if (profileStart >= profileEnd) - this.profileStart = profileEnd - 0.02f; - if (hollow > 0.99f) - this.hollow = 0.99f; - if (hollow < 0.0f) - this.hollow = 0.0f; - } - - /// - /// Extrudes a profile along a path. - /// - public void Extrude(PathType pathType) - { - bool needEndFaces = false; - - this.coords = new List(); - this.faces = new List(); - - int steps = 1; - - float length = this.pathCutEnd - this.pathCutBegin; - - this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; - - this.hasHollow = (this.hollow > 0.001f); - - float twistBegin = this.twistBegin / 360.0f * twoPi; - float twistEnd = this.twistEnd / 360.0f * twoPi; - float twistTotal = twistEnd - twistBegin; - float twistTotalAbs = Math.Abs(twistTotal); - if (twistTotalAbs > 0.01f) - steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number - - float hollow = this.hollow; - - if (pathType == PathType.Circular) - { - needEndFaces = false; - if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f) - needEndFaces = true; - else if (this.taperX != 0.0f || this.taperY != 0.0f) - needEndFaces = true; - else if (this.skew != 0.0f) - needEndFaces = true; - else if (twistTotal != 0.0f) - needEndFaces = true; - else if (this.radius != 0.0f) - needEndFaces = true; - } - else needEndFaces = true; - - // sanity checks - float initialProfileRot = 0.0f; - if (pathType == PathType.Circular) - { - if (this.sides == 3) - { - initialProfileRot = (float)Math.PI; - if (this.hollowSides == 4) - { - if (hollow > 0.7f) - hollow = 0.7f; - hollow *= 0.707f; - } - else hollow *= 0.5f; - } - else if (this.sides == 4) - { - initialProfileRot = 0.25f * (float)Math.PI; - if (this.hollowSides != 4) - hollow *= 0.707f; - } - else if (this.sides > 4) - { - initialProfileRot = (float)Math.PI; - if (this.hollowSides == 4) - { - if (hollow > 0.7f) - hollow = 0.7f; - hollow /= 0.7f; - } - } - } - else - { - if (this.sides == 3) - { - if (this.hollowSides == 4) - { - if (hollow > 0.7f) - hollow = 0.7f; - hollow *= 0.707f; - } - else hollow *= 0.5f; - } - else if (this.sides == 4) - { - initialProfileRot = 1.25f * (float)Math.PI; - if (this.hollowSides != 4) - hollow *= 0.707f; - } - else if (this.sides == 24 && this.hollowSides == 4) - hollow *= 1.414f; - } - - Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, this.hasProfileCut,true); - this.errorMessage = profile.errorMessage; - - this.numPrimFaces = profile.numPrimFaces; - - if (initialProfileRot != 0.0f) - { - profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot)); - } - - float thisV = 0.0f; - float lastV = 0.0f; - - Path path = new Path(); - path.twistBegin = twistBegin; - path.twistEnd = twistEnd; - path.topShearX = topShearX; - path.topShearY = topShearY; - path.pathCutBegin = pathCutBegin; - path.pathCutEnd = pathCutEnd; - path.dimpleBegin = dimpleBegin; - path.dimpleEnd = dimpleEnd; - path.skew = skew; - path.holeSizeX = holeSizeX; - path.holeSizeY = holeSizeY; - path.taperX = taperX; - path.taperY = taperY; - path.radius = radius; - path.revolutions = revolutions; - path.stepsPerRevolution = stepsPerRevolution; - - path.Create(pathType, steps); - - int lastNode = path.pathNodes.Count -1; - - for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) - { - PathNode node = path.pathNodes[nodeIndex]; - Profile newLayer = profile.Copy(); - - newLayer.Scale(node.xScale, node.yScale); - newLayer.AddRot(node.rotation); - newLayer.AddPos(node.position); - - if (needEndFaces && nodeIndex == 0) - { - newLayer.FlipNormals(); - } // if (nodeIndex == 0) - - // append this layer - - int coordsLen = this.coords.Count; - newLayer.AddValue2FaceVertexIndices(coordsLen); - - this.coords.AddRange(newLayer.coords); - - if (needEndFaces) - { - if (nodeIndex == 0) - this.faces.AddRange(newLayer.faces); - else if (nodeIndex == lastNode) - { - if (node.xScale > 1e-6 && node.yScale > 1e-6) - this.faces.AddRange(newLayer.faces); - } - } - - // fill faces between layers - - int numVerts = newLayer.coords.Count; - Face newFace1 = new Face(); - Face newFace2 = new Face(); - - thisV = 1.0f - node.percentOfPath; - - if (nodeIndex > 0) - { - int startVert = coordsLen; - int endVert = this.coords.Count; - if (!this.hasProfileCut) - { - int i = startVert; - for (int l = 0; l < profile.numOuterVerts - 1; l++) - { - newFace1.v1 = i; - newFace1.v2 = i - numVerts; - newFace1.v3 = i + 1; - this.faces.Add(newFace1); - - newFace2.v1 = i + 1; - newFace2.v2 = i - numVerts; - newFace2.v3 = i + 1 - numVerts; - this.faces.Add(newFace2); - i++; - } - - newFace1.v1 = i; - newFace1.v2 = i - numVerts; - newFace1.v3 = startVert; - this.faces.Add(newFace1); - - newFace2.v1 = startVert; - newFace2.v2 = i - numVerts; - newFace2.v3 = startVert - numVerts; - this.faces.Add(newFace2); - - if (this.hasHollow) - { - startVert = ++i; - for (int l = 0; l < profile.numHollowVerts - 1; l++) - { - newFace1.v1 = i; - newFace1.v2 = i - numVerts; - newFace1.v3 = i + 1; - this.faces.Add(newFace1); - - newFace2.v1 = i + 1; - newFace2.v2 = i - numVerts; - newFace2.v3 = i + 1 - numVerts; - this.faces.Add(newFace2); - i++; - } - - newFace1.v1 = i; - newFace1.v2 = i - numVerts; - newFace1.v3 = startVert; - this.faces.Add(newFace1); - - newFace2.v1 = startVert; - newFace2.v2 = i - numVerts; - newFace2.v3 = startVert - numVerts; - this.faces.Add(newFace2); - } - - - } - else - { - for (int i = startVert; i < endVert; i++) - { - int iNext = i + 1; - if (i == endVert - 1) - iNext = startVert; - - newFace1.v1 = i; - newFace1.v2 = i - numVerts; - newFace1.v3 = iNext; - this.faces.Add(newFace1); - - newFace2.v1 = iNext; - newFace2.v2 = i - numVerts; - newFace2.v3 = iNext - numVerts; - this.faces.Add(newFace2); - - } - } - } - - lastV = thisV; - - } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) - - } - - - /// - /// DEPRICATED - use Extrude(PathType.Linear) instead - /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism. - /// - /// - public void ExtrudeLinear() - { - this.Extrude(PathType.Linear); - } - - - /// - /// DEPRICATED - use Extrude(PathType.Circular) instead - /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring. - /// - /// - public void ExtrudeCircular() - { - this.Extrude(PathType.Circular); - } - - - private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3) - { - Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); - Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); - - Coord normal = Coord.Cross(edge1, edge2); - - normal.Normalize(); - - return normal; - } - - private Coord SurfaceNormal(Face face) - { - return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]); - } - - /// - /// Calculate the surface normal for a face in the list of faces - /// - /// - /// - public Coord SurfaceNormal(int faceIndex) - { - int numFaces = this.faces.Count; - if (faceIndex < 0 || faceIndex >= numFaces) - throw new Exception("faceIndex out of range"); - - return SurfaceNormal(this.faces[faceIndex]); - } - - /// - /// Duplicates a PrimMesh object. All object properties are copied by value, including lists. - /// - /// - public PrimMesh Copy() - { - PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides); - copy.twistBegin = this.twistBegin; - copy.twistEnd = this.twistEnd; - copy.topShearX = this.topShearX; - copy.topShearY = this.topShearY; - copy.pathCutBegin = this.pathCutBegin; - copy.pathCutEnd = this.pathCutEnd; - copy.dimpleBegin = this.dimpleBegin; - copy.dimpleEnd = this.dimpleEnd; - copy.skew = this.skew; - copy.holeSizeX = this.holeSizeX; - copy.holeSizeY = this.holeSizeY; - copy.taperX = this.taperX; - copy.taperY = this.taperY; - copy.radius = this.radius; - copy.revolutions = this.revolutions; - copy.stepsPerRevolution = this.stepsPerRevolution; - - copy.numPrimFaces = this.numPrimFaces; - copy.errorMessage = this.errorMessage; - - copy.coords = new List(this.coords); - copy.faces = new List(this.faces); - - return copy; - } - - /// - /// Adds a value to each XYZ vertex coordinate in the mesh - /// - /// - /// - /// - public void AddPos(float x, float y, float z) - { - int i; - int numVerts = this.coords.Count; - Coord vert; - - for (i = 0; i < numVerts; i++) - { - vert = this.coords[i]; - vert.X += x; - vert.Y += y; - vert.Z += z; - this.coords[i] = vert; - } - } - - /// - /// Rotates the mesh - /// - /// - public void AddRot(Quat q) - { - int i; - int numVerts = this.coords.Count; - - for (i = 0; i < numVerts; i++) - this.coords[i] *= q; - } - -#if VERTEX_INDEXER - public VertexIndexer GetVertexIndexer() - { - return null; - } -#endif - - /// - /// Scales the mesh - /// - /// - /// - /// - public void Scale(float x, float y, float z) - { - int i; - int numVerts = this.coords.Count; - //Coord vert; - - Coord m = new Coord(x, y, z); - for (i = 0; i < numVerts; i++) - this.coords[i] *= m; - } - - /// - /// Dumps the mesh to a Blender compatible "Raw" format file - /// - /// - /// - /// - public void DumpRaw(String path, String name, String title) - { - if (path == null) - return; - String fileName = name + "_" + title + ".raw"; - String completePath = System.IO.Path.Combine(path, fileName); - StreamWriter sw = new StreamWriter(completePath); - - for (int i = 0; i < this.faces.Count; i++) - { - string s = this.coords[this.faces[i].v1].ToString(); - s += " " + this.coords[this.faces[i].v2].ToString(); - s += " " + this.coords[this.faces[i].v3].ToString(); - - sw.WriteLine(s); - } - - sw.Close(); - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/Properties/AssemblyInfo.cs deleted file mode 100644 index 6881e26..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/Properties/AssemblyInfo.cs +++ /dev/null @@ -1,36 +0,0 @@ -using System.Reflection; -using System.Runtime.CompilerServices; -using System.Runtime.InteropServices; -using Mono.Addins; - -// General Information about an assembly is controlled through the following -// set of attributes. Change these attribute values to modify the information -// associated with an assembly. -[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.UbitMeshing")] -[assembly: AssemblyDescription("")] -[assembly: AssemblyConfiguration("")] -[assembly: AssemblyCompany("http://opensimulator.org")] -[assembly: AssemblyProduct("OpenSim")] -[assembly: AssemblyCopyright("OpenSimulator developers")] -[assembly: AssemblyTrademark("")] -[assembly: AssemblyCulture("")] - -// Setting ComVisible to false makes the types in this assembly not visible -// to COM components. If you need to access a type in this assembly from -// COM, set the ComVisible attribute to true on that type. -[assembly: ComVisible(false)] - -// The following GUID is for the ID of the typelib if this project is exposed to COM -[assembly: Guid("4b7e35c2-a9dd-4b10-b778-eb417f4f6884")] - -// Version information for an assembly consists of the following four values: -// -// Major Version -// Minor Version -// Build Number -// Revision -// -[assembly: AssemblyVersion("0.8.2.*")] - -[assembly: Addin("OpenSim.Region.PhysicsModule.UbitMeshing", OpenSim.VersionInfo.VersionNumber)] -[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMap.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMap.cs deleted file mode 100644 index 1c75db6..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMap.cs +++ /dev/null @@ -1,244 +0,0 @@ -/* - * Copyright (c) Contributors - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.Text; - -using System.Drawing; -using System.Drawing.Imaging; - -namespace PrimMesher -{ - public class SculptMap - { - public int width; - public int height; - public byte[] redBytes; - public byte[] greenBytes; - public byte[] blueBytes; - - public SculptMap() - { - } - - public SculptMap(Bitmap bm, int lod) - { - int bmW = bm.Width; - int bmH = bm.Height; - - if (bmW == 0 || bmH == 0) - throw new Exception("SculptMap: bitmap has no data"); - - int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image - - bool needsScaling = false; - bool smallMap = false; - - width = bmW; - height = bmH; - - while (width * height > numLodPixels * 4) - { - width >>= 1; - height >>= 1; - needsScaling = true; - } - - try - { - if (needsScaling) - bm = ScaleImage(bm, width, height); - } - - catch (Exception e) - { - throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); - } - - if (width * height > numLodPixels) - { - smallMap = false; - width >>= 1; - height >>= 1; - } - else - smallMap = true; - - int numBytes = (width + 1) * (height + 1); - redBytes = new byte[numBytes]; - greenBytes = new byte[numBytes]; - blueBytes = new byte[numBytes]; - - int byteNdx = 0; - Color c; - - try - { - for (int y = 0; y <= height; y++) - { - for (int x = 0; x < width; x++) - { - if (smallMap) - c = bm.GetPixel(x, y < height ? y : y - 1); - else - c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1); - - redBytes[byteNdx] = c.R; - greenBytes[byteNdx] = c.G; - blueBytes[byteNdx] = c.B; - - ++byteNdx; - } - - if (smallMap) - c = bm.GetPixel(width - 1, y < height ? y : y - 1); - else - c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1); - - redBytes[byteNdx] = c.R; - greenBytes[byteNdx] = c.G; - blueBytes[byteNdx] = c.B; - - ++byteNdx; - } - } - catch (Exception e) - { - throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString()); - } - - width++; - height++; - } - - public List> ToRows(bool mirror) - { - int numRows = height; - int numCols = width; - - List> rows = new List>(numRows); - - float pixScale = 1.0f / 255; - - int rowNdx, colNdx; - int smNdx = 0; - - for (rowNdx = 0; rowNdx < numRows; rowNdx++) - { - List row = new List(numCols); - for (colNdx = 0; colNdx < numCols; colNdx++) - { - - if (mirror) - row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); - else - row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); - - ++smNdx; - } - rows.Add(row); - } - return rows; - } - - private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) - { - - Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); - - Color c; - - - // will let last step to be eventually diferent, as seems to be in sl - - float xscale = (float)srcImage.Width / (float)destWidth; - float yscale = (float)srcImage.Height / (float)destHeight; - - int lastsx = srcImage.Width - 1; - int lastsy = srcImage.Height - 1; - int lastdx = destWidth - 1; - int lastdy = destHeight - 1; - - float sy = 0.5f; - float sx; - - for (int y = 0; y < lastdy; y++) - { - sx = 0.5f; - for (int x = 0; x < lastdx; x++) - { - try - { - c = srcImage.GetPixel((int)(sx), (int)(sy)); - scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); - } - catch (IndexOutOfRangeException) - { - } - sx += xscale; - } - try - { - c = srcImage.GetPixel(lastsx, (int)(sy)); - scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B)); - } - catch (IndexOutOfRangeException) - { - } - - sy += yscale; - } - - sx = 0.5f; - for (int x = 0; x < lastdx; x++) - { - try - { - c = srcImage.GetPixel((int)(sx), lastsy); - scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B)); - } - catch (IndexOutOfRangeException) - { - } - - sx += xscale; - } - try - { - c = srcImage.GetPixel(lastsx, lastsy); - scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B)); - } - catch (IndexOutOfRangeException) - { - } - - srcImage.Dispose(); - return scaledImage; - } - } -} \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMesh.cs b/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMesh.cs deleted file mode 100644 index bc1375b..0000000 --- a/OpenSim/Region/PhysicsModules/UbitMeshing/SculptMesh.cs +++ /dev/null @@ -1,220 +0,0 @@ -/* - * Copyright (c) Contributors - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.Text; -using System.IO; - -using System.Drawing; -using System.Drawing.Imaging; - -namespace PrimMesher -{ - - public class SculptMesh - { - public List coords; - public List faces; - - public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 }; - - - public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert) - { - if (mirror) - invert = !invert; - - SculptMap smap = new SculptMap(sculptBitmap, lod); - - List> rows = smap.ToRows(mirror); - - _SculptMesh(rows, sculptType, invert); - } - - private void _SculptMesh(List> rows, SculptType sculptType, bool invert) - { - coords = new List(); - faces = new List(); - - sculptType = (SculptType)(((int)sculptType) & 0x07); - - int width = rows[0].Count; - - int p1, p2, p3, p4; - - int imageX, imageY; - - if (sculptType != SculptType.plane) - { - if (rows.Count % 2 == 0) - { - for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++) - rows[rowNdx].Add(rows[rowNdx][0]); - } - else - { - int lastIndex = rows[0].Count - 1; - - for (int i = 0; i < rows.Count; i++) - rows[i][0] = rows[i][lastIndex]; - } - } - - Coord topPole = rows[0][width / 2]; - Coord bottomPole = rows[rows.Count - 1][width / 2]; - - if (sculptType == SculptType.sphere) - { - if (rows.Count % 2 == 0) - { - int count = rows[0].Count; - List topPoleRow = new List(count); - List bottomPoleRow = new List(count); - - for (int i = 0; i < count; i++) - { - topPoleRow.Add(topPole); - bottomPoleRow.Add(bottomPole); - } - rows.Insert(0, topPoleRow); - rows.Add(bottomPoleRow); - } - else - { - int count = rows[0].Count; - - List topPoleRow = rows[0]; - List bottomPoleRow = rows[rows.Count - 1]; - - for (int i = 0; i < count; i++) - { - topPoleRow[i] = topPole; - bottomPoleRow[i] = bottomPole; - } - } - } - - if (sculptType == SculptType.torus) - rows.Add(rows[0]); - - int coordsDown = rows.Count; - int coordsAcross = rows[0].Count; - - float widthUnit = 1.0f / (coordsAcross - 1); - float heightUnit = 1.0f / (coordsDown - 1); - - for (imageY = 0; imageY < coordsDown; imageY++) - { - int rowOffset = imageY * coordsAcross; - - for (imageX = 0; imageX < coordsAcross; imageX++) - { - /* - * p1-----p2 - * | \ f2 | - * | \ | - * | f1 \| - * p3-----p4 - */ - - p4 = rowOffset + imageX; - p3 = p4 - 1; - - p2 = p4 - coordsAcross; - p1 = p3 - coordsAcross; - - this.coords.Add(rows[imageY][imageX]); - - if (imageY > 0 && imageX > 0) - { - Face f1, f2; - - if (invert) - { - f1 = new Face(p1, p4, p3); - f2 = new Face(p1, p2, p4); - } - else - { - f1 = new Face(p1, p3, p4); - f2 = new Face(p1, p4, p2); - } - - this.faces.Add(f1); - this.faces.Add(f2); - } - } - } - } - - /// - /// Duplicates a SculptMesh object. All object properties are copied by value, including lists. - /// - /// - public SculptMesh Copy() - { - return new SculptMesh(this); - } - - public SculptMesh(SculptMesh sm) - { - coords = new List(sm.coords); - faces = new List(sm.faces); - } - - public void Scale(float x, float y, float z) - { - int i; - int numVerts = this.coords.Count; - - Coord m = new Coord(x, y, z); - for (i = 0; i < numVerts; i++) - this.coords[i] *= m; - } - - public void DumpRaw(String path, String name, String title) - { - if (path == null) - return; - String fileName = name + "_" + title + ".raw"; - String completePath = System.IO.Path.Combine(path, fileName); - StreamWriter sw = new StreamWriter(completePath); - - for (int i = 0; i < this.faces.Count; i++) - { - string s = this.coords[this.faces[i].v1].ToString(); - s += " " + this.coords[this.faces[i].v2].ToString(); - s += " " + this.coords[this.faces[i].v3].ToString(); - - sw.WriteLine(s); - } - - sw.Close(); - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs deleted file mode 100644 index 5622e48..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs +++ /dev/null @@ -1,1847 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ - - -// Revision by Ubit 2011/12 - -using System; -using System.Collections.Generic; -using System.Reflection; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; -using log4net; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - /// - /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. - /// - - public enum dParam : int - { - LowStop = 0, - HiStop = 1, - Vel = 2, - FMax = 3, - FudgeFactor = 4, - Bounce = 5, - CFM = 6, - StopERP = 7, - StopCFM = 8, - LoStop2 = 256, - HiStop2 = 257, - Vel2 = 258, - FMax2 = 259, - StopERP2 = 7 + 256, - StopCFM2 = 8 + 256, - LoStop3 = 512, - HiStop3 = 513, - Vel3 = 514, - FMax3 = 515, - StopERP3 = 7 + 512, - StopCFM3 = 8 + 512 - } - - public class OdeCharacter : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private Vector3 _position; - private Vector3 _zeroPosition; - private Vector3 _velocity; - private Vector3 _target_velocity; - private Vector3 _acceleration; - private Vector3 m_rotationalVelocity; - private Vector3 m_size; - private Vector3 m_collideNormal; - private Quaternion m_orientation; - private Quaternion m_orientation2D; - private float m_mass = 80f; - public float m_density = 60f; - private bool m_pidControllerActive = true; - - const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) - const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) - public float PID_D; - public float PID_P; - - private float timeStep; - private float invtimeStep; - - private float m_feetOffset = 0; - private float feetOff = 0; - private float boneOff = 0; - private float AvaAvaSizeXsq = 0.3f; - private float AvaAvaSizeYsq = 0.2f; - - public float walkDivisor = 1.3f; - public float runDivisor = 0.8f; - private bool flying = false; - private bool m_iscolliding = false; - private bool m_iscollidingGround = false; - private bool m_iscollidingObj = false; - private bool m_alwaysRun = false; - - private bool _zeroFlag = false; - - - private uint m_localID = 0; - public bool m_returnCollisions = false; - // taints and their non-tainted counterparts - public bool m_isPhysical = false; // the current physical status - public float MinimumGroundFlightOffset = 3f; - - private float m_buoyancy = 0f; - - private bool m_freemove = false; - // private CollisionLocker ode; - -// private string m_name = String.Empty; - // other filter control - int m_colliderfilter = 0; - int m_colliderGroundfilter = 0; - int m_colliderObjectfilter = 0; - - // Default we're a Character - private CollisionCategories m_collisionCategories = (CollisionCategories.Character); - - // Default, Collide with Other Geometries, spaces, bodies and characters. - private CollisionCategories m_collisionFlags = (CollisionCategories.Character - | CollisionCategories.Geom - | CollisionCategories.VolumeDtc - ); - // we do land collisions not ode | CollisionCategories.Land); - public IntPtr Body = IntPtr.Zero; - private ODEScene _parent_scene; - private IntPtr capsule = IntPtr.Zero; - public IntPtr collider = IntPtr.Zero; - - public IntPtr Amotor = IntPtr.Zero; - - public d.Mass ShellMass; - - public int m_eventsubscription = 0; - private int m_cureventsubscription = 0; - private CollisionEventUpdate CollisionEventsThisFrame = null; - private bool SentEmptyCollisionsEvent; - - // unique UUID of this character object - public UUID m_uuid; - public bool bad = false; - - float mu; - - public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) - { - m_uuid = UUID.Random(); - m_localID = localID; - - timeStep = parent_scene.ODE_STEPSIZE; - invtimeStep = 1 / timeStep; - - if (pos.IsFinite()) - { - if (pos.Z > 99999f) - { - pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - if (pos.Z < -100f) // shouldn't this be 0 ? - { - pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - _position = pos; - } - else - { - _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); - m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); - } - - _parent_scene = parent_scene; - - - m_size.X = pSize.X; - m_size.Y = pSize.Y; - m_size.Z = pSize.Z; - - if(m_size.X <0.01f) - m_size.X = 0.01f; - if(m_size.Y <0.01f) - m_size.Y = 0.01f; - if(m_size.Z <0.01f) - m_size.Z = 0.01f; - - m_feetOffset = pfeetOffset; - m_orientation = Quaternion.Identity; - m_orientation2D = Quaternion.Identity; - m_density = density; - - // force lower density for testing - m_density = 3.0f; - - mu = parent_scene.AvatarFriction; - - walkDivisor = walk_divisor; - runDivisor = rundivisor; - - m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default - - PID_D = basePID_D * m_mass * invtimeStep; - PID_P = basePID_P * m_mass * invtimeStep; - - m_isPhysical = false; // current status: no ODE information exists - - Name = avName; - - AddChange(changes.Add, null); - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Agent; } - set { return; } - } - - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = 0; - cdata.softcolide = false; - } - - public override bool Building { get; set; } - - /// - /// If this is set, the avatar will move faster - /// - public override bool SetAlwaysRun - { - get { return m_alwaysRun; } - set { m_alwaysRun = value; } - } - - public override uint LocalID - { - get { return m_localID; } - set { m_localID = value; } - } - - public override PhysicsActor ParentActor - { - get { return (PhysicsActor)this; } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set { return; } - } - - public override float Buoyancy - { - get { return m_buoyancy; } - set { m_buoyancy = value; } - } - - public override bool FloatOnWater - { - set { return; } - } - - public override bool IsPhysical - { - get { return m_isPhysical; } - set { return; } - } - - public override bool ThrottleUpdates - { - get { return false; } - set { return; } - } - - public override bool Flying - { - get { return flying; } - set - { - flying = value; -// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); - } - } - - /// - /// Returns if the avatar is colliding in general. - /// This includes the ground and objects and avatar. - /// - public override bool IsColliding - { - get { return (m_iscolliding || m_iscollidingGround); } - set - { - if (value) - { - m_colliderfilter += 3; - if (m_colliderfilter > 3) - m_colliderfilter = 3; - } - else - { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; - } - - if (m_colliderfilter == 0) - m_iscolliding = false; - else - { - m_pidControllerActive = true; - m_iscolliding = true; - m_freemove = false; - } - } - } - - /// - /// Returns if an avatar is colliding with the ground - /// - public override bool CollidingGround - { - get { return m_iscollidingGround; } - set - { -/* we now control this - if (value) - { - m_colliderGroundfilter += 2; - if (m_colliderGroundfilter > 2) - m_colliderGroundfilter = 2; - } - else - { - m_colliderGroundfilter--; - if (m_colliderGroundfilter < 0) - m_colliderGroundfilter = 0; - } - - if (m_colliderGroundfilter == 0) - m_iscollidingGround = false; - else - m_iscollidingGround = true; - */ - } - - } - - /// - /// Returns if the avatar is colliding with an object - /// - public override bool CollidingObj - { - get { return m_iscollidingObj; } - set - { - // Ubit filter this also - if (value) - { - m_colliderObjectfilter += 2; - if (m_colliderObjectfilter > 2) - m_colliderObjectfilter = 2; - } - else - { - m_colliderObjectfilter--; - if (m_colliderObjectfilter < 0) - m_colliderObjectfilter = 0; - } - - if (m_colliderObjectfilter == 0) - m_iscollidingObj = false; - else - m_iscollidingObj = true; - -// m_iscollidingObj = value; - - if (m_iscollidingObj) - m_pidControllerActive = false; - else - m_pidControllerActive = true; - } - } - - /// - /// turn the PID controller on or off. - /// The PID Controller will turn on all by itself in many situations - /// - /// - public void SetPidStatus(bool status) - { - m_pidControllerActive = status; - } - - public override bool Stopped - { - get { return _zeroFlag; } - } - - /// - /// This 'puts' an avatar somewhere in the physics space. - /// Not really a good choice unless you 'know' it's a good - /// spot otherwise you're likely to orbit the avatar. - /// - public override Vector3 Position - { - get { return _position; } - set - { - if (value.IsFinite()) - { - if (value.Z > 9999999f) - { - value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - if (value.Z < -100f) - { - value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; - } - AddChange(changes.Position, value); - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); - } - } - } - - public override Vector3 RotationalVelocity - { - get { return m_rotationalVelocity; } - set { m_rotationalVelocity = value; } - } - - /// - /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight - /// and use it to offset landings properly - /// - public override Vector3 Size - { - get - { - return m_size; - } - set - { - if (value.IsFinite()) - { - if(value.X <0.01f) - value.X = 0.01f; - if(value.Y <0.01f) - value.Y = 0.01f; - if(value.Z <0.01f) - value.Z = 0.01f; - - AddChange(changes.Size, value); - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); - } - } - } - - public override void setAvatarSize(Vector3 size, float feetOffset) - { - if (size.IsFinite()) - { - if (size.X < 0.01f) - size.X = 0.01f; - if (size.Y < 0.01f) - size.Y = 0.01f; - if (size.Z < 0.01f) - size.Z = 0.01f; - - strAvatarSize st = new strAvatarSize(); - st.size = size; - st.offset = feetOffset; - AddChange(changes.AvatarSize, st); - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character"); - } - - } - /// - /// This creates the Avatar's physical Surrogate at the position supplied - /// - /// - /// - /// - - // - /// - /// Uses the capped cyllinder volume formula to calculate the avatar's mass. - /// This may be used in calculations in the scene/scenepresence - /// - public override float Mass - { - get - { - return m_mass; - } - } - public override void link(PhysicsActor obj) - { - - } - - public override void delink() - { - - } - - public override void LockAngularMotion(Vector3 axis) - { - - } - - - public override Vector3 Force - { - get { return _target_velocity; } - set { return; } - } - - public override int VehicleType - { - get { return 0; } - set { return; } - } - - public override void VehicleFloatParam(int param, float value) - { - - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - - } - - public override void VehicleRotationParam(int param, Quaternion rotation) - { - - } - - public override void VehicleFlags(int param, bool remove) - { - - } - - public override void SetVolumeDetect(int param) - { - - } - - public override Vector3 CenterOfMass - { - get - { - Vector3 pos = _position; - return pos; - } - } - - public override Vector3 GeometricCenter - { - get - { - Vector3 pos = _position; - return pos; - } - } - - public override PrimitiveBaseShape Shape - { - set { return; } - } - - public override Vector3 Velocity - { - get - { - return _velocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); - } - } - } - - public override Vector3 Torque - { - get { return Vector3.Zero; } - set { return; } - } - - public override float CollisionScore - { - get { return 0f; } - set { } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get { return m_orientation; } - set - { -// fakeori = value; -// givefakeori++; - value.Normalize(); - AddChange(changes.Orientation, value); - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public void SetAcceleration(Vector3 accel) - { - m_pidControllerActive = true; - _acceleration = accel; - } - - /// - /// Adds the force supplied to the Target Velocity - /// The PID controller takes this target velocity and tries to make it a reality - /// - /// - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - if (pushforce) - { - AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f)); - } - else - { - AddChange(changes.Velocity, force); - } - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); - } - //m_lastUpdateSent = false; - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - - } - - public override void SetMomentum(Vector3 momentum) - { - if (momentum.IsFinite()) - AddChange(changes.Momentum, momentum); - } - - - private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) - { - // sizes one day should came from visual parameters - float sx = m_size.X; - float sy = m_size.Y; - float sz = m_size.Z; - - float bot = -sz * 0.5f + m_feetOffset; - boneOff = bot + 0.3f; - - float feetsz = sz * 0.45f; - if (feetsz > 0.6f) - feetsz = 0.6f; - - feetOff = bot + feetsz; - - AvaAvaSizeXsq = 0.4f * sx; - AvaAvaSizeXsq *= AvaAvaSizeXsq; - AvaAvaSizeYsq = 0.5f * sy; - AvaAvaSizeYsq *= AvaAvaSizeYsq; - - _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); - - collider = d.HashSpaceCreate(_parent_scene.CharsSpace); - d.HashSpaceSetLevels(collider, -4, 3); - d.SpaceSetSublevel(collider, 3); - d.SpaceSetCleanup(collider, false); - d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); - d.GeomSetCollideBits(collider, (uint)m_collisionFlags); - - float r = m_size.X; - if (m_size.Y > r) - r = m_size.Y; - float l = m_size.Z - r; - r *= 0.5f; - - capsule = d.CreateCapsule(collider, r, l); - - m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass - - d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); - - PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE; - PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE; - - Body = d.BodyCreate(_parent_scene.world); - - _zeroFlag = false; - m_pidControllerActive = true; - m_freemove = false; - - _velocity = Vector3.Zero; - - d.BodySetAutoDisableFlag(Body, false); - d.BodySetPosition(Body, npositionX, npositionY, npositionZ); - - _position.X = npositionX; - _position.Y = npositionY; - _position.Z = npositionZ; - - d.BodySetMass(Body, ref ShellMass); - d.GeomSetBody(capsule, Body); - - // The purpose of the AMotor here is to keep the avatar's physical - // surrogate from rotating while moving - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - d.JointSetAMotorNumAxes(Amotor, 3); - d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); - d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); - d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); - - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorAngle(Amotor, 1, 0); - d.JointSetAMotorAngle(Amotor, 2, 0); - - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); - d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); - - // These lowstops and high stops are effectively (no wiggle room) - d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); - d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); - - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); - - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); - } - - /// - /// Destroys the avatar body and geom - - private void AvatarGeomAndBodyDestroy() - { - // Kill the Amotor - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - if (Body != IntPtr.Zero) - { - //kill the body - d.BodyDestroy(Body); - Body = IntPtr.Zero; - } - - //kill the Geoms - if (capsule != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(capsule); - _parent_scene.waitForSpaceUnlock(collider); - d.GeomDestroy(capsule); - capsule = IntPtr.Zero; - } - - if (collider != IntPtr.Zero) - { - d.SpaceDestroy(collider); - collider = IntPtr.Zero; - } - - } - - //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z - public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) - { - float sin = 2.0f * rot.Z * rot.W; - float cos = rot.W * rot.W - rot.Z * rot.Z; - float tx = x; - - x = tx * cos - y * sin; - y = tx * sin + y * cos; - } - public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) - { - float tx = x; - x = tx * cos - y * sin; - y = tx * sin + y * cos; - } - public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) - { - float tx = x; - x = tx * cos + y * sin; - y = -tx * sin + y * cos; - } - - public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) - { - float sin = - 2.0f * rot.Z * rot.W; - float cos = rot.W * rot.W - rot.Z * rot.Z; - float tx = x; - - x = tx * cos - y * sin; - y = tx * sin + y * cos; - } - - public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, - ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) - { - feetcollision = false; - useAltcontact = false; - - if (me == capsule) - { - Vector3 offset; - - float h = contact.pos.Z - _position.Z; - offset.Z = h - feetOff; - - offset.X = contact.pos.X - _position.X; - offset.Y = contact.pos.Y - _position.Y; - - d.GeomClassID gtype = d.GeomGetClass(other); - if (gtype == d.GeomClassID.CapsuleClass) - { - Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); - float r = roff.X *roff.X / AvaAvaSizeXsq; - r += (roff.Y * roff.Y) / AvaAvaSizeYsq; - if (r > 1.0f) - return false; - - float dp = 1.0f -(float)Math.Sqrt((double)r); - if (dp > 0.05f) - dp = 0.05f; - - contact.depth = dp; - - if (offset.Z < 0) - { - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } - } - return true; - } -/* - d.AABB aabb; - d.GeomGetAABB(other,out aabb); - float othertop = aabb.MaxZ - _position.Z; -*/ -// if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) - if (offset.Z > 0 || contact.normal.Z > 0.35f) - { - if (offset.Z <= 0) - { - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } - } - return true; - } - - altContact = contact; - useAltcontact = true; - - offset.Z -= 0.2f; - - offset.Normalize(); - - if (contact.depth > 0.1f) - contact.depth = 0.1f; - - if (reverse) - { - altContact.normal.X = offset.X; - altContact.normal.Y = offset.Y; - altContact.normal.Z = offset.Z; - } - else - { - altContact.normal.X = -offset.X; - altContact.normal.Y = -offset.Y; - altContact.normal.Z = -offset.Z; - } - - feetcollision = true; - if (h < boneOff) - { - m_collideNormal.X = contact.normal.X; - m_collideNormal.Y = contact.normal.Y; - m_collideNormal.Z = contact.normal.Z; - IsColliding = true; - } - return true; - } - return false; - } - - /// - /// Called from Simulate - /// This is the avatar's movement control + PID Controller - /// - /// - public void Move(List defects) - { - if (Body == IntPtr.Zero) - return; - - d.Vector3 dtmp = d.BodyGetPosition(Body); - Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - - // the Amotor still lets avatar rotation to drift during colisions - // so force it back to identity - - d.Quaternion qtmp; - qtmp.W = m_orientation2D.W; - qtmp.X = m_orientation2D.X; - qtmp.Y = m_orientation2D.Y; - qtmp.Z = m_orientation2D.Z; - d.BodySetQuaternion(Body, ref qtmp); - - if (m_pidControllerActive == false) - { - _zeroPosition = localpos; - } - - if (!localpos.IsFinite()) - { - m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); - defects.Add(this); - // _parent_scene.RemoveCharacter(this); - - // destroy avatar capsule and related ODE data - AvatarGeomAndBodyDestroy(); - return; - } - - // check outbounds forcing to be in world - bool fixbody = false; - if (localpos.X < 0.0f) - { - fixbody = true; - localpos.X = 0.1f; - } - else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f) - { - fixbody = true; - localpos.X = _parent_scene.WorldExtents.X - 0.1f; - } - if (localpos.Y < 0.0f) - { - fixbody = true; - localpos.Y = 0.1f; - } - else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1) - { - fixbody = true; - localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; - } - if (fixbody) - { - m_freemove = false; - d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); - } - - float breakfactor; - - Vector3 vec = Vector3.Zero; - dtmp = d.BodyGetLinearVel(Body); - Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - float velLengthSquared = vel.LengthSquared(); - - - Vector3 ctz = _target_velocity; - - float movementdivisor = 1f; - //Ubit change divisions into multiplications below - if (!m_alwaysRun) - movementdivisor = 1 / walkDivisor; - else - movementdivisor = 1 / runDivisor; - - ctz.X *= movementdivisor; - ctz.Y *= movementdivisor; - - //****************************************** - // colide with land - - d.AABB aabb; -// d.GeomGetAABB(feetbox, out aabb); - d.GeomGetAABB(capsule, out aabb); - float chrminZ = aabb.MinZ; // move up a bit - Vector3 posch = localpos; - - float ftmp; - - if (flying) - { - ftmp = timeStep; - posch.X += vel.X * ftmp; - posch.Y += vel.Y * ftmp; - } - - float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); - if (chrminZ < terrainheight) - { - if (ctz.Z < 0) - ctz.Z = 0; - - Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); - float depth = terrainheight - chrminZ; - - vec.Z = depth * PID_P * 50; - - if (!flying) - vec.Z += -vel.Z * PID_D; - - if (depth < 0.2f) - { - m_colliderGroundfilter++; - if (m_colliderGroundfilter > 2) - { - m_iscolliding = true; - m_colliderfilter = 2; - - if (m_colliderGroundfilter > 10) - { - m_colliderGroundfilter = 10; - m_freemove = false; - } - - m_collideNormal.X = n.X; - m_collideNormal.Y = n.Y; - m_collideNormal.Z = n.Z; - - m_iscollidingGround = true; - - - ContactPoint contact = new ContactPoint(); - contact.PenetrationDepth = depth; - contact.Position.X = localpos.X; - contact.Position.Y = localpos.Y; - contact.Position.Z = terrainheight; - contact.SurfaceNormal.X = -n.X; - contact.SurfaceNormal.Y = -n.Y; - contact.SurfaceNormal.Z = -n.Z; - contact.RelativeSpeed = -vel.Z; - contact.CharacterFeet = true; - AddCollisionEvent(0, contact); - -// vec.Z *= 0.5f; - } - } - - else - { - m_colliderGroundfilter -= 5; - if (m_colliderGroundfilter <= 0) - { - m_colliderGroundfilter = 0; - m_iscollidingGround = false; - } - } - } - else - { - m_colliderGroundfilter -= 5; - if (m_colliderGroundfilter <= 0) - { - m_colliderGroundfilter = 0; - m_iscollidingGround = false; - } - } - - - //****************************************** - if (!m_iscolliding) - m_collideNormal.Z = 0; - - bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); - - - - if (!tviszero) - { - m_freemove = false; - - // movement relative to surface if moving on it - // dont disturbe vertical movement, ie jumps - if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) - { - float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; - ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); - ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); - ctz.Z -= p; - if (ctz.Z < 0) - ctz.Z *= 2; - - } - - } - - - if (!m_freemove) - { - - // if velocity is zero, use position control; otherwise, velocity control - if (tviszero && m_iscolliding && !flying) - { - // keep track of where we stopped. No more slippin' & slidin' - if (!_zeroFlag) - { - _zeroFlag = true; - _zeroPosition = localpos; - } - if (m_pidControllerActive) - { - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); - vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); - if(vel.Z > 0) - vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; - else - vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; -/* - if (flying) - { - vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; - } -*/ - } - //PidStatus = true; - } - else - { - m_pidControllerActive = true; - _zeroFlag = false; - - if (m_iscolliding) - { - if (!flying) - { - // we are on a surface - if (ctz.Z > 0f) - { - // moving up or JUMPING - vec.Z += (ctz.Z - vel.Z) * PID_D * 2f; - vec.X += (ctz.X - vel.X) * (PID_D); - vec.Y += (ctz.Y - vel.Y) * (PID_D); - } - else - { - // we are moving down on a surface - if (ctz.Z == 0) - { - if (vel.Z > 0) - vec.Z -= vel.Z * PID_D * 2f; - vec.X += (ctz.X - vel.X) * (PID_D); - vec.Y += (ctz.Y - vel.Y) * (PID_D); - } - // intencionally going down - else - { - if (ctz.Z < vel.Z) - vec.Z += (ctz.Z - vel.Z) * PID_D; - else - { - } - - if (Math.Abs(ctz.X) > Math.Abs(vel.X)) - vec.X += (ctz.X - vel.X) * (PID_D); - if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) - vec.Y += (ctz.Y - vel.Y) * (PID_D); - } - } - - // We're standing on something - } - else - { - // We're flying and colliding with something - vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); - vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); - vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); - } - } - else // ie not colliding - { - if (flying) //(!m_iscolliding && flying) - { - // we're in mid air suspended - vec.X += (ctz.X - vel.X) * (PID_D); - vec.Y += (ctz.Y - vel.Y) * (PID_D); - vec.Z += (ctz.Z - vel.Z) * (PID_D); - } - - else - { - // we're not colliding and we're not flying so that means we're falling! - // m_iscolliding includes collisions with the ground. - - // d.Vector3 pos = d.BodyGetPosition(Body); - vec.X += (ctz.X - vel.X) * PID_D * 0.833f; - vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; - // hack for breaking on fall - if (ctz.Z == -9999f) - vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass; - } - } - } - - if (velLengthSquared > 2500.0f) // 50m/s apply breaks - { - breakfactor = 0.16f * m_mass; - vec.X -= breakfactor * vel.X; - vec.Y -= breakfactor * vel.Y; - vec.Z -= breakfactor * vel.Z; - } - } - else - { - breakfactor = m_mass; - vec.X -= breakfactor * vel.X; - vec.Y -= breakfactor * vel.Y; - if (flying) - vec.Z -= 0.5f * breakfactor * vel.Z; - else - vec.Z -= .16f* m_mass * vel.Z; - } - - if (flying) - { - vec.Z -= _parent_scene.gravityz * m_mass; - - //Added for auto fly height. Kitto Flora - float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; - - if (localpos.Z < target_altitude) - { - vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; - } - // end add Kitto Flora - } - - if (vec.IsFinite()) - { - if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) - d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); - m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); - defects.Add(this); - // _parent_scene.RemoveCharacter(this); - // destroy avatar capsule and related ODE data - AvatarGeomAndBodyDestroy(); - return; - } - - // update our local ideia of position velocity and aceleration - // _position = localpos; - _position = localpos; - - if (_zeroFlag) - { - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - } - else - { - Vector3 a =_velocity; // previus velocity - SetSmooth(ref _velocity, ref vel, 2); - a = (_velocity - a) * invtimeStep; - SetSmooth(ref _acceleration, ref a, 2); - - dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = 0f; - m_rotationalVelocity.Y = 0f; - m_rotationalVelocity.Z = dtmp.Z; - Math.Round(m_rotationalVelocity.Z,3); - } - } - - public void round(ref Vector3 v, int digits) - { - v.X = (float)Math.Round(v.X, digits); - v.Y = (float)Math.Round(v.Y, digits); - v.Z = (float)Math.Round(v.Z, digits); - } - - public void SetSmooth(ref Vector3 dst, ref Vector3 value) - { - dst.X = 0.1f * dst.X + 0.9f * value.X; - dst.Y = 0.1f * dst.Y + 0.9f * value.Y; - dst.Z = 0.1f * dst.Z + 0.9f * value.Z; - } - - public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) - { - dst.X = 0.4f * dst.X + 0.6f * value.X; - dst.X = (float)Math.Round(dst.X, rounddigits); - - dst.Y = 0.4f * dst.Y + 0.6f * value.Y; - dst.Y = (float)Math.Round(dst.Y, rounddigits); - - dst.Z = 0.4f * dst.Z + 0.6f * value.Z; - dst.Z = (float)Math.Round(dst.Z, rounddigits); - } - - - /// - /// Updates the reported position and velocity. - /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording - /// also outbounds checking - /// copy and outbounds now done in move(..) at ode rate - /// - /// - public void UpdatePositionAndVelocity() - { - return; - -// if (Body == IntPtr.Zero) -// return; - - } - - /// - /// Cleanup the things we use in the scene. - /// - public void Destroy() - { - AddChange(changes.Remove, null); - } - - public override void CrossingFailure() - { - } - - public override Vector3 PIDTarget { set { return; } } - public override bool PIDActive {get {return m_pidControllerActive;} set { return; } } - public override float PIDTau { set { return; } } - - public override float PIDHoverHeight { set { return; } } - public override bool PIDHoverActive { set { return; } } - public override PIDHoverType PIDHoverType { set { return; } } - public override float PIDHoverTau { set { return; } } - - public override Quaternion APIDTarget { set { return; } } - - public override bool APIDActive { set { return; } } - - public override float APIDStrength { set { return; } } - - public override float APIDDamping { set { return; } } - - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - m_cureventsubscription = 0; - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - SentEmptyCollisionsEvent = false; - } - - public override void UnSubscribeEvents() - { - if (CollisionEventsThisFrame != null) - { - lock (CollisionEventsThisFrame) - { - CollisionEventsThisFrame.Clear(); - CollisionEventsThisFrame = null; - } - } - m_eventsubscription = 0; - } - - public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - lock (CollisionEventsThisFrame) - { - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - _parent_scene.AddCollisionEventReporting(this); - } - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - lock (CollisionEventsThisFrame) - { - if (m_cureventsubscription < m_eventsubscription) - return; - - m_cureventsubscription = 0; - - int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; - - if (!SentEmptyCollisionsEvent || ncolisions > 0) - { - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (ncolisions == 0) - { - SentEmptyCollisionsEvent = true; - _parent_scene.RemoveCollisionEventReporting(this); - } - else - { - SentEmptyCollisionsEvent = false; - CollisionEventsThisFrame.Clear(); - } - } - } - } - - internal void AddCollisionFrameTime(int t) - { - // protect it from overflow crashing - if (m_cureventsubscription < 50000) - m_cureventsubscription += t; - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - private void changePhysicsStatus(bool NewStatus) - { - if (NewStatus != m_isPhysical) - { - if (NewStatus) - { - AvatarGeomAndBodyDestroy(); - - AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); - - _parent_scene.actor_name_map[collider] = (PhysicsActor)this; - _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; - _parent_scene.AddCharacter(this); - } - else - { - _parent_scene.RemoveCollisionEventReporting(this); - _parent_scene.RemoveCharacter(this); - // destroy avatar capsule and related ODE data - AvatarGeomAndBodyDestroy(); - } - m_freemove = false; - m_isPhysical = NewStatus; - } - } - - private void changeAdd() - { - changePhysicsStatus(true); - } - - private void changeRemove() - { - changePhysicsStatus(false); - } - - private void changeShape(PrimitiveBaseShape arg) - { - } - - private void changeAvatarSize(strAvatarSize st) - { - m_feetOffset = st.offset; - changeSize(st.size); - } - - private void changeSize(Vector3 pSize) - { - if (pSize.IsFinite()) - { - // for now only look to Z changes since viewers also don't change X and Y - if (pSize.Z != m_size.Z) - { - AvatarGeomAndBodyDestroy(); - - - float oldsz = m_size.Z; - m_size = pSize; - - - AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (m_size.Z - oldsz) * 0.5f); - - Velocity = Vector3.Zero; - - - _parent_scene.actor_name_map[collider] = (PhysicsActor)this; - _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; - } - m_freemove = false; - m_pidControllerActive = true; - } - else - { - m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); - } - } - - private void changePosition( Vector3 newPos) - { - if (Body != IntPtr.Zero) - d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - m_freemove = false; - m_pidControllerActive = true; - } - - private void changeOrientation(Quaternion newOri) - { - if (m_orientation != newOri) - { - m_orientation = newOri; // keep a copy for core use - // but only use rotations around Z - - m_orientation2D.W = newOri.W; - m_orientation2D.Z = newOri.Z; - - float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; - if (t > 0) - { - t = 1.0f / (float)Math.Sqrt(t); - m_orientation2D.W *= t; - m_orientation2D.Z *= t; - } - else - { - m_orientation2D.W = 1.0f; - m_orientation2D.Z = 0f; - } - m_orientation2D.Y = 0f; - m_orientation2D.X = 0f; - - d.Quaternion myrot = new d.Quaternion(); - myrot.X = m_orientation2D.X; - myrot.Y = m_orientation2D.Y; - myrot.Z = m_orientation2D.Z; - myrot.W = m_orientation2D.W; - d.BodySetQuaternion(Body, ref myrot); - } - } - - private void changeVelocity(Vector3 newVel) - { - m_pidControllerActive = true; - m_freemove = false; - _target_velocity = newVel; - } - - private void changeSetTorque(Vector3 newTorque) - { - } - - private void changeAddForce(Vector3 newForce) - { - } - - private void changeAddAngularForce(Vector3 arg) - { - } - - private void changeAngularLock(Vector3 arg) - { - } - - private void changeFloatOnWater(bool arg) - { - } - - private void changeVolumedetetion(bool arg) - { - } - - private void changeSelectedStatus(bool arg) - { - } - - private void changeDisable(bool arg) - { - } - - private void changeBuilding(bool arg) - { - } - - private void setFreeMove() - { - m_pidControllerActive = true; - _zeroFlag = false; - _target_velocity = Vector3.Zero; - m_freemove = true; - m_colliderfilter = -1; - m_colliderObjectfilter = -1; - m_colliderGroundfilter = -1; - - m_iscolliding = false; - m_iscollidingGround = false; - m_iscollidingObj = false; - - CollisionEventsThisFrame.Clear(); - } - - private void changeForce(Vector3 newForce) - { - setFreeMove(); - - if (Body != IntPtr.Zero) - { - if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) - d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); - } - } - - // for now momentum is actually velocity - private void changeMomentum(Vector3 newmomentum) - { - _velocity = newmomentum; - setFreeMove(); - - if (Body != IntPtr.Zero) - d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); - } - - private void donullchange() - { - } - - public bool DoAChange(changes what, object arg) - { - if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) - { - return false; - } - - // nasty switch - switch (what) - { - case changes.Add: - changeAdd(); - break; - case changes.Remove: - changeRemove(); - break; - - case changes.Position: - changePosition((Vector3)arg); - break; - - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; - - case changes.PosOffset: - donullchange(); - break; - - case changes.OriOffset: - donullchange(); - break; - - case changes.Velocity: - changeVelocity((Vector3)arg); - break; - - // case changes.Acceleration: - // changeacceleration((Vector3)arg); - // break; - // case changes.AngVelocity: - // changeangvelocity((Vector3)arg); - // break; - - case changes.Force: - changeForce((Vector3)arg); - break; - - case changes.Torque: - changeSetTorque((Vector3)arg); - break; - - case changes.AddForce: - changeAddForce((Vector3)arg); - break; - - case changes.AddAngForce: - changeAddAngularForce((Vector3)arg); - break; - - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; - - case changes.Size: - changeSize((Vector3)arg); - break; - - case changes.AvatarSize: - changeAvatarSize((strAvatarSize)arg); - break; - - case changes.Momentum: - changeMomentum((Vector3)arg); - break; -/* not in use for now - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; - - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; - - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; - - case changes.Physical: - changePhysicsStatus((bool)arg); - break; - - case changes.Selected: - changeSelectedStatus((bool)arg); - break; - - case changes.disabled: - changeDisable((bool)arg); - break; - - case changes.building: - changeBuilding((bool)arg); - break; -*/ - case changes.Null: - donullchange(); - break; - - default: - donullchange(); - break; - } - return false; - } - - public void AddChange(changes what, object arg) - { - _parent_scene.AddChange((PhysicsActor)this, what, arg); - } - - private struct strAvatarSize - { - public Vector3 size; - public float offset; - } - - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs deleted file mode 100644 index abe6f37..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs +++ /dev/null @@ -1,1096 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ - -/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -// Extensive change Ubit 2012 - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using log4net; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - public class ODEDynamics - { - public Vehicle Type - { - get { return m_type; } - } - - private OdePrim rootPrim; - private ODEScene _pParentScene; - - // Vehicle properties - // WARNING this are working copies for internel use - // their values may not be the corresponding parameter - - private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? - - private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - - private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl - - // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); - private float m_linearMotorDecayTimescale = 120; - private float m_linearMotorTimescale = 1000; - private Vector3 m_linearMotorOffset = Vector3.Zero; - - //Angular properties - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate - private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate - private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate - - //Deflection properties - private float m_angularDeflectionEfficiency = 0; - private float m_angularDeflectionTimescale = 1000; - private float m_linearDeflectionEfficiency = 0; - private float m_linearDeflectionTimescale = 1000; - - //Banking properties - private float m_bankingEfficiency = 0; - private float m_bankingMix = 0; - private float m_bankingTimescale = 1000; - - //Hover and Buoyancy properties - private float m_VhoverHeight = 0f; - private float m_VhoverEfficiency = 0f; - private float m_VhoverTimescale = 1000f; - private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 1.0f; // damped - private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. - - - // auxiliar - private float m_lmEfect = 0f; // current linear motor eficiency - private float m_lmDecay = 0f; // current linear decay - - private float m_amEfect = 0; // current angular motor eficiency - private float m_amDecay = 0f; // current linear decay - - private float m_ffactor = 1.0f; - - private float m_timestep = 0.02f; - private float m_invtimestep = 50; - - - float m_ampwr; - float m_amdampX; - float m_amdampY; - float m_amdampZ; - - float m_gravmod; - - public float FrictionFactor - { - get - { - return m_ffactor; - } - } - - public float GravMod - { - set - { - m_gravmod = value; - } - } - - - public ODEDynamics(OdePrim rootp) - { - rootPrim = rootp; - _pParentScene = rootPrim._parent_scene; - m_timestep = _pParentScene.ODE_STEPSIZE; - m_invtimestep = 1.0f / m_timestep; - m_gravmod = rootPrim.GravModifier; - } - - public void DoSetVehicle(VehicleData vd) - { - m_type = vd.m_type; - m_flags = vd.m_flags; - - - // Linear properties - m_linearMotorDirection = vd.m_linearMotorDirection; - - m_linearFrictionTimescale = vd.m_linearFrictionTimescale; - if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; - if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; - if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; - - m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; - if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; - m_linearMotorDecayTimescale += 0.2f; - m_linearMotorDecayTimescale *= m_invtimestep; - - m_linearMotorTimescale = vd.m_linearMotorTimescale; - if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; - - m_linearMotorOffset = vd.m_linearMotorOffset; - - //Angular properties - m_angularMotorDirection = vd.m_angularMotorDirection; - m_angularMotorTimescale = vd.m_angularMotorTimescale; - if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; - - m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; - if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; - m_angularMotorDecayTimescale *= m_invtimestep; - - m_angularFrictionTimescale = vd.m_angularFrictionTimescale; - if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; - if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; - if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; - - //Deflection properties - m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; - m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; - if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; - - m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; - m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; - if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; - - //Banking properties - m_bankingEfficiency = vd.m_bankingEfficiency; - m_bankingMix = vd.m_bankingMix; - m_bankingTimescale = vd.m_bankingTimescale; - if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; - - //Hover and Buoyancy properties - m_VhoverHeight = vd.m_VhoverHeight; - m_VhoverEfficiency = vd.m_VhoverEfficiency; - m_VhoverTimescale = vd.m_VhoverTimescale; - if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; - - m_VehicleBuoyancy = vd.m_VehicleBuoyancy; - - //Attractor properties - m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; - m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; - if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; - - // Axis - m_referenceFrame = vd.m_referenceFrame; - - m_lmEfect = 0; - m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); - m_amEfect = 0; - m_ffactor = 1.0f; - } - - internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) - { - float len; - - switch (pParam) - { - case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_angularDeflectionEfficiency = pValue; - break; - case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_angularDeflectionTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - else if (pValue > 120) pValue = 120; - m_angularMotorDecayTimescale = pValue * m_invtimestep; - m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; - break; - case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_angularMotorTimescale = pValue; - break; - case Vehicle.BANKING_EFFICIENCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - m_bankingEfficiency = pValue; - break; - case Vehicle.BANKING_MIX: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_bankingMix = pValue; - break; - case Vehicle.BANKING_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_bankingTimescale = pValue; - break; - case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - m_VehicleBuoyancy = pValue; - break; - case Vehicle.HOVER_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_VhoverEfficiency = pValue; - break; - case Vehicle.HOVER_HEIGHT: - m_VhoverHeight = pValue; - break; - case Vehicle.HOVER_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_VhoverTimescale = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_linearDeflectionEfficiency = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_linearDeflectionTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - else if (pValue > 120) pValue = 120; - m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; - m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); - break; - case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_linearMotorTimescale = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - m_verticalAttractionEfficiency = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_verticalAttractionTimescale = pValue; - break; - - // These are vector properties but the engine lets you use a single float value to - // set all of the components to the same value - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - len = m_angularMotorDirection.Length(); - if (len > 12.566f) - m_angularMotorDirection *= (12.566f / len); - - m_amEfect = 1.0f ; // turn it on - m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; - - if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) - && !rootPrim.m_isSelected && !rootPrim.m_disabled) - d.BodyEnable(rootPrim.Body); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < m_timestep) pValue = m_timestep; - m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - len = m_linearMotorDirection.Length(); - if (len > 100.0f) - m_linearMotorDirection *= (100.0f / len); - - m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; - m_lmEfect = 1.0f; // turn it on - - m_ffactor = 0.0f; - if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) - && !rootPrim.m_isSelected && !rootPrim.m_disabled) - d.BodyEnable(rootPrim.Body); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - len = m_linearMotorOffset.Length(); - if (len > 100.0f) - m_linearMotorOffset *= (100.0f / len); - break; - } - }//end ProcessFloatVehicleParam - - internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) - { - float len; - - switch (pParam) - { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X < m_timestep) pValue.X = m_timestep; - if (pValue.Y < m_timestep) pValue.Y = m_timestep; - if (pValue.Z < m_timestep) pValue.Z = m_timestep; - - m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - // Limit requested angular speed to 2 rps= 4 pi rads/sec - len = m_angularMotorDirection.Length(); - if (len > 12.566f) - m_angularMotorDirection *= (12.566f / len); - - m_amEfect = 1.0f; // turn it on - m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; - - if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) - && !rootPrim.m_isSelected && !rootPrim.m_disabled) - d.BodyEnable(rootPrim.Body); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < m_timestep) pValue.X = m_timestep; - if (pValue.Y < m_timestep) pValue.Y = m_timestep; - if (pValue.Z < m_timestep) pValue.Z = m_timestep; - m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - len = m_linearMotorDirection.Length(); - if (len > 100.0f) - m_linearMotorDirection *= (100.0f / len); - - m_lmEfect = 1.0f; // turn it on - m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; - - m_ffactor = 0.0f; - if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) - && !rootPrim.m_isSelected && !rootPrim.m_disabled) - d.BodyEnable(rootPrim.Body); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - len = m_linearMotorOffset.Length(); - if (len > 100.0f) - m_linearMotorOffset *= (100.0f / len); - break; - case Vehicle.BLOCK_EXIT: - m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - } - }//end ProcessVectorVehicleParam - - internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) - { - switch (pParam) - { - case Vehicle.REFERENCE_FRAME: - // m_referenceFrame = Quaternion.Inverse(pValue); - m_referenceFrame = pValue; - break; - case Vehicle.ROLL_FRAME: - m_RollreferenceFrame = pValue; - break; - } - }//end ProcessRotationVehicleParam - - internal void ProcessVehicleFlags(int pParam, bool remove) - { - if (remove) - { - m_flags &= ~((VehicleFlag)pParam); - } - else - { - m_flags |= (VehicleFlag)pParam; - } - }//end ProcessVehicleFlags - - internal void ProcessTypeChange(Vehicle pType) - { - m_lmEfect = 0; - - m_amEfect = 0; - m_ffactor = 1f; - - m_linearMotorDirection = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; - - m_BlockingEndPoint = Vector3.Zero; - m_RollreferenceFrame = Quaternion.Identity; - m_linearMotorOffset = Vector3.Zero; - - m_referenceFrame = Quaternion.Identity; - - // Set Defaults For Type - m_type = pType; - switch (pType) - { - case Vehicle.TYPE_NONE: - m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120 * m_invtimestep; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 1000 * m_invtimestep; - m_VhoverHeight = 0; - m_VhoverEfficiency = 1; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - m_linearDeflectionEfficiency = 0; - m_linearDeflectionTimescale = 1000; - m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 1000; - m_bankingEfficiency = 0; - m_bankingMix = 1; - m_bankingTimescale = 1000; - m_verticalAttractionEfficiency = 0; - m_verticalAttractionTimescale = 1000; - - m_flags = (VehicleFlag)0; - break; - - case Vehicle.TYPE_SLED: - m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120 * m_invtimestep; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 120 * m_invtimestep; - m_VhoverHeight = 0; - m_VhoverEfficiency = 1; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 0; - m_linearDeflectionEfficiency = 1; - m_linearDeflectionTimescale = 1; - m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1; - m_verticalAttractionTimescale = 1000; - m_bankingEfficiency = 0; - m_bankingMix = 1; - m_bankingTimescale = 10; - m_flags &= - ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.LIMIT_MOTOR_UP); - break; - - case Vehicle.TYPE_CAR: - m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60 * m_invtimestep; - m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f * m_invtimestep; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - m_linearDeflectionEfficiency = 1; - m_linearDeflectionTimescale = 2; - m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1f; - m_verticalAttractionTimescale = 10f; - m_bankingEfficiency = -0.2f; - m_bankingMix = 1; - m_bankingTimescale = 1; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.LIMIT_MOTOR_UP | - VehicleFlag.HOVER_UP_ONLY); - break; - case Vehicle.TYPE_BOAT: - m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60 * m_invtimestep; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4 * m_invtimestep; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 2; - m_VehicleBuoyancy = 1; - m_linearDeflectionEfficiency = 0.5f; - m_linearDeflectionTimescale = 3; - m_angularDeflectionEfficiency = 0.5f; - m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0.5f; - m_verticalAttractionTimescale = 5f; - m_bankingEfficiency = -0.3f; - m_bankingMix = 0.8f; - m_bankingTimescale = 1; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | - VehicleFlag.HOVER_UP_ONLY); // | -// VehicleFlag.LIMIT_ROLL_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP | - VehicleFlag.HOVER_UP_ONLY | // new sl - VehicleFlag.HOVER_WATER_ONLY); - break; - - case Vehicle.TYPE_AIRPLANE: - m_linearFrictionTimescale = new Vector3(200, 10, 5); - m_angularFrictionTimescale = new Vector3(20, 20, 20); - m_linearMotorTimescale = 2; - m_linearMotorDecayTimescale = 60 * m_invtimestep; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 8 * m_invtimestep; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - m_linearDeflectionEfficiency = 0.5f; - m_linearDeflectionTimescale = 0.5f; - m_angularDeflectionEfficiency = 1; - m_angularDeflectionTimescale = 2; - m_verticalAttractionEfficiency = 0.9f; - m_verticalAttractionTimescale = 2f; - m_bankingEfficiency = 1; - m_bankingMix = 0.7f; - m_bankingTimescale = 2; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); - break; - - case Vehicle.TYPE_BALLOON: - m_linearFrictionTimescale = new Vector3(5, 5, 5); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60 * m_invtimestep; - m_angularMotorTimescale = 6; - m_angularMotorDecayTimescale = 10 * m_invtimestep; - m_VhoverHeight = 5; - m_VhoverEfficiency = 0.8f; - m_VhoverTimescale = 10; - m_VehicleBuoyancy = 1; - m_linearDeflectionEfficiency = 0; - m_linearDeflectionTimescale = 5 * m_invtimestep; - m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 1f; - m_verticalAttractionTimescale = 1000f; - m_bankingEfficiency = 0; - m_bankingMix = 0.7f; - m_bankingTimescale = 5; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP | //); - VehicleFlag.LIMIT_ROLL_ONLY | // new sl - VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl - -// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | -// VehicleFlag.HOVER_GLOBAL_HEIGHT); - break; - - } - - m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); - m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; - - }//end SetDefaultsForType - - internal void Stop() - { - m_lmEfect = 0; - m_lmDecay = 0f; - m_amEfect = 0; - m_amDecay = 0; - m_ffactor = 1f; - } - - public static Vector3 Xrot(Quaternion rot) - { - Vector3 vec; - rot.Normalize(); // just in case - vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; - vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); - vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); - return vec; - } - - public static Vector3 Zrot(Quaternion rot) - { - Vector3 vec; - rot.Normalize(); // just in case - vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); - vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); - vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; - - return vec; - } - - private const float pi = (float)Math.PI; - private const float halfpi = 0.5f * (float)Math.PI; - private const float twopi = 2.0f * pi; - - public static Vector3 ubitRot2Euler(Quaternion rot) - { - // returns roll in X - // pitch in Y - // yaw in Z - Vector3 vec; - - // assuming rot is normalised - // rot.Normalize(); - - float zX = rot.X * rot.Z + rot.Y * rot.W; - - if (zX < -0.49999f) - { - vec.X = 0; - vec.Y = -halfpi; - vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); - } - else if (zX > 0.49999f) - { - vec.X = 0; - vec.Y = halfpi; - vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); - } - else - { - vec.Y = (float)Math.Asin(2 * zX); - - float sqw = rot.W * rot.W; - - float minuszY = rot.X * rot.W - rot.Y * rot.Z; - float zZ = rot.Z * rot.Z + sqw - 0.5f; - - vec.X = (float)Math.Atan2(minuszY, zZ); - - float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) - float yY = rot.X * rot.X + sqw - 0.5f; - vec.Z = (float)Math.Atan2(yX, yY); - } - return vec; - } - - public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) - { - // assuming rot is normalised - // rot.Normalize(); - - float zX = rot.X * rot.Z + rot.Y * rot.W; - - if (zX < -0.49999f) - { - roll = 0; - pitch = -halfpi; - } - else if (zX > 0.49999f) - { - roll = 0; - pitch = halfpi; - } - else - { - pitch = (float)Math.Asin(2 * zX); - - float minuszY = rot.X * rot.W - rot.Y * rot.Z; - float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; - - roll = (float)Math.Atan2(minuszY, zZ); - } - return ; - } - - internal void Step() - { - IntPtr Body = rootPrim.Body; - - d.Mass dmass; - d.BodyGetMass(Body, out dmass); - - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion rotq = objrotq; // rotq = rotation of object - rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame - Quaternion irotq = Quaternion.Inverse(rotq); - - d.Vector3 dvtmp; - Vector3 tmpV; - Vector3 curVel; // velocity in world - Vector3 curAngVel; // angular velocity in world - Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame - Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame - d.Vector3 dtorque = new d.Vector3(); - - dvtmp = d.BodyGetLinearVel(Body); - curVel.X = dvtmp.X; - curVel.Y = dvtmp.Y; - curVel.Z = dvtmp.Z; - Vector3 curLocalVel = curVel * irotq; // current velocity in local - - dvtmp = d.BodyGetAngularVel(Body); - curAngVel.X = dvtmp.X; - curAngVel.Y = dvtmp.Y; - curAngVel.Z = dvtmp.Z; - Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local - - float ldampZ = 0; - - // linear motor - if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) - { - tmpV = m_linearMotorDirection - curLocalVel; // velocity error - tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep - tmpV *= rotq; // to world - - if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) - tmpV.Z = 0; - - if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) - { - // have offset, do it now - tmpV *= dmass.mass; - d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); - } - else - { - force.X += tmpV.X; - force.Y += tmpV.Y; - force.Z += tmpV.Z; - } - - m_lmEfect *= m_lmDecay; -// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); - m_ffactor = 0.0f; - } - else - { - m_lmEfect = 0; - m_ffactor = 1f; - } - - // hover - if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) - { - // d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); - pos.Z -= 0.21f; // minor offset that seems to be always there in sl - - float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); - float perr; - - // default to global but don't go underground - perr = m_VhoverHeight - pos.Z; - - if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) - { - if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) - { - perr += _pParentScene.GetWaterLevel(); - } - else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) - { - perr += t; - } - else - { - float w = _pParentScene.GetWaterLevel(); - if (t > w) - perr += t; - else - perr += w; - } - } - else if (t > m_VhoverHeight) - perr = t - pos.Z; ; - - if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) - { - ldampZ = m_VhoverEfficiency * m_invtimestep; - - perr *= (1.0f + ldampZ) / m_VhoverTimescale; - - // force.Z += perr - curVel.Z * tmp; - force.Z += perr; - ldampZ *= -curVel.Z; - - force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); - } - else // no buoyancy - force.Z += _pParentScene.gravityz; - } - else - { - // default gravity and Buoyancy - force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); - } - - // linear deflection - if (m_linearDeflectionEfficiency > 0) - { - float len = curVel.Length(); - if (len > 0.01) // if moving - { - Vector3 atAxis; - atAxis = Xrot(rotq); // where are we pointing to - atAxis *= len; // make it same size as world velocity vector - - tmpV = -atAxis; // oposite direction - atAxis -= curVel; // error to one direction - len = atAxis.LengthSquared(); - - tmpV -= curVel; // error to oposite - float lens = tmpV.LengthSquared(); - - if (len > 0.01 || lens > 0.01) // do nothing if close enougth - { - if (len < lens) - tmpV = atAxis; - - tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep - force.X += tmpV.X; - force.Y += tmpV.Y; - if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) - force.Z += tmpV.Z; - } - } - } - - // linear friction/damping - if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) - { - tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; - tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; - tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; - tmpV *= rotq; // to world - - if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) - tmpV.Z = ldampZ; - force.X += tmpV.X; - force.Y += tmpV.Y; - force.Z += tmpV.Z; - } - - // vertical atractor - if (m_verticalAttractionTimescale < 300) - { - float roll; - float pitch; - - - - float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; - - float ftmp2; - ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; - m_amdampX = ftmp2; - - m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; - - GetRollPitch(irotq, out roll, out pitch); - - if (roll > halfpi) - roll = pi - roll; - else if (roll < -halfpi) - roll = -pi - roll; - - float effroll = pitch / halfpi; - effroll *= effroll; - effroll = 1 - effroll; - effroll *= roll; - - - torque.X += effroll * ftmp; - - if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) - { - float effpitch = roll / halfpi; - effpitch *= effpitch; - effpitch = 1 - effpitch; - effpitch *= pitch; - - torque.Y += effpitch * ftmp; - } - - if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) - { - - float broll = effroll; - /* - if (broll > halfpi) - broll = pi - broll; - else if (broll < -halfpi) - broll = -pi - broll; - */ - broll *= m_bankingEfficiency; - if (m_bankingMix != 0) - { - float vfact = Math.Abs(curLocalVel.X) / 10.0f; - if (vfact > 1.0f) vfact = 1.0f; - - if (curLocalVel.X >= 0) - broll *= (1 + (vfact - 1) * m_bankingMix); - else - broll *= -(1 + (vfact - 1) * m_bankingMix); - } - // make z rot be in world Z not local as seems to be in sl - - broll = broll / m_bankingTimescale; - - - tmpV = Zrot(irotq); - tmpV *= broll; - - torque.X += tmpV.X; - torque.Y += tmpV.Y; - torque.Z += tmpV.Z; - - m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; - m_amdampY = m_amdampZ; - - } - else - { - m_amdampZ = 1 / m_angularFrictionTimescale.Z; - m_amdampY = m_amdampX; - } - } - else - { - m_ampwr = 1.0f; - m_amdampX = 1 / m_angularFrictionTimescale.X; - m_amdampY = 1 / m_angularFrictionTimescale.Y; - m_amdampZ = 1 / m_angularFrictionTimescale.Z; - } - - // angular motor - if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) - { - tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error - tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep - torque.X += tmpV.X * m_ampwr; - torque.Y += tmpV.Y * m_ampwr; - torque.Z += tmpV.Z; - - m_amEfect *= m_amDecay; - } - else - m_amEfect = 0; - - // angular deflection - if (m_angularDeflectionEfficiency > 0) - { - Vector3 dirv; - - if (curLocalVel.X > 0.01f) - dirv = curLocalVel; - else if (curLocalVel.X < -0.01f) - // use oposite - dirv = -curLocalVel; - else - { - // make it fall into small positive x case - dirv.X = 0.01f; - dirv.Y = curLocalVel.Y; - dirv.Z = curLocalVel.Z; - } - - float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; - - if (Math.Abs(dirv.Z) > 0.01) - { - torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; - } - - if (Math.Abs(dirv.Y) > 0.01) - { - torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; - } - } - - // angular friction - if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) - { - torque.X -= curLocalAngVel.X * m_amdampX; - torque.Y -= curLocalAngVel.Y * m_amdampY; - torque.Z -= curLocalAngVel.Z * m_amdampZ; - } - - - if (force.X != 0 || force.Y != 0 || force.Z != 0) - { - force *= dmass.mass; - d.BodyAddForce(Body, force.X, force.Y, force.Z); - } - - if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) - { - torque *= m_referenceFrame; // to object frame - dtorque.X = torque.X ; - dtorque.Y = torque.Y; - dtorque.Z = torque.Z; - - d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); - d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame - } - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs deleted file mode 100644 index 330b191..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs +++ /dev/null @@ -1,933 +0,0 @@ -/* - * AJLDuarte 2012 - */ - -using System; -using System.Threading; -using System.Collections.Generic; -using System.IO; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Text; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; -using OdeAPI; -using log4net; -using Nini.Config; -using OpenMetaverse; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - public enum MeshState : byte - { - noNeed = 0, - - loadingAsset = 1, - - AssetOK = 0x0f, // 00001111 - - NeedMask = 0x30, // 00110000 - needMesh = 0x10, // 00010000 - needAsset = 0x20, // 00100000 - - FailMask = 0xC0, // 11000000 - AssetFailed = 0x40, // 01000000 - MeshFailed = 0x80, // 10000000 - - MeshNoColide = FailMask | needAsset - } - - public enum meshWorkerCmnds : byte - { - nop = 0, - addnew, - changefull, - changesize, - changeshapetype, - getmesh, - } - - public class ODEPhysRepData - { - public PhysicsActor actor; - public PrimitiveBaseShape pbs; - public IMesh mesh; - - public Vector3 size; - public Vector3 OBB; - public Vector3 OBBOffset; - - public float volume; - - public byte shapetype; - public bool hasOBB; - public bool hasMeshVolume; - public MeshState meshState; - public UUID? assetID; - public meshWorkerCmnds comand; - } - - public class ODEMeshWorker - { - - private ILog m_log; - private ODEScene m_scene; - private IMesher m_mesher; - - public bool meshSculptedPrim = true; - public bool forceSimplePrimMeshing = false; - public float meshSculptLOD = 32; - public float MeshSculptphysicalLOD = 32; - - - private OpenSim.Framework.BlockingQueue createqueue = new OpenSim.Framework.BlockingQueue(); - private bool m_running; - - private Thread m_thread; - - public ODEMeshWorker(ODEScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) - { - m_scene = pScene; - m_log = pLog; - m_mesher = pMesher; - - if (pConfig != null) - { - forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); - meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); - meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); - MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); - } - m_running = true; - m_thread = new Thread(DoWork); - m_thread.Name = "OdeMeshWorker"; - m_thread.Start(); - } - - private void DoWork() - { - m_mesher.ExpireFileCache(); - - while(m_running) - { - ODEPhysRepData nextRep = createqueue.Dequeue(); - if(!m_running) - return; - if (nextRep == null) - continue; - if (m_scene.haveActor(nextRep.actor)) - { - switch (nextRep.comand) - { - case meshWorkerCmnds.changefull: - case meshWorkerCmnds.changeshapetype: - case meshWorkerCmnds.changesize: - GetMesh(nextRep); - if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) - m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); - break; - case meshWorkerCmnds.getmesh: - DoRepDataGetMesh(nextRep); - break; - } - } - } - } - - public void Stop() - { - try - { - m_thread.Abort(); - createqueue.Clear(); - } - catch - { - } - } - - public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, - Vector3 size, byte shapetype) - { - ODEPhysRepData repData = new ODEPhysRepData(); - repData.actor = actor; - repData.pbs = pbs; - repData.size = size; - repData.shapetype = shapetype; - - CheckMesh(repData); - CalcVolumeData(repData); - m_scene.AddChange(actor, changes.PhysRepData, repData); - return; - } - - public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, - Vector3 size, byte shapetype) - { - ODEPhysRepData repData = new ODEPhysRepData(); - repData.actor = actor; - repData.pbs = pbs; - repData.size = size; - repData.shapetype = shapetype; - - CheckMesh(repData); - CalcVolumeData(repData); - m_scene.AddChange(actor, changes.AddPhysRep, repData); - return repData; - } - - public void RequestMesh(ODEPhysRepData repData) - { - repData.mesh = null; - - if (repData.meshState == MeshState.needAsset) - { - PrimitiveBaseShape pbs = repData.pbs; - - // check if we got outdated - - if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero) - { - repData.meshState = MeshState.noNeed; - return; - } - - repData.assetID = pbs.SculptTexture; - repData.meshState = MeshState.loadingAsset; - - repData.comand = meshWorkerCmnds.getmesh; - createqueue.Enqueue(repData); - } - } - - // creates and prepares a mesh to use and calls parameters estimation - public bool CreateActorPhysRep(ODEPhysRepData repData) - { - IMesh mesh = repData.mesh; - - if (mesh != null) - { - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", - repData.actor.Name, repData.pbs.SculptTexture.ToString()); - repData.meshState = MeshState.MeshFailed; - repData.hasOBB = false; - repData.mesh = null; - m_scene.mesher.ReleaseMesh(mesh); - } - else - { - repData.OBBOffset = mesh.GetCentroid(); - repData.OBB = mesh.GetOBB(); - repData.hasOBB = true; - mesh.releaseSourceMeshData(); - } - } - CalcVolumeData(repData); - return true; - } - - public void AssetLoaded(ODEPhysRepData repData) - { - if (m_scene.haveActor(repData.actor)) - { - if (needsMeshing(repData.pbs)) // no need for pbs now? - { - repData.comand = meshWorkerCmnds.changefull; - createqueue.Enqueue(repData); - } - } - else - repData.pbs.SculptData = Utils.EmptyBytes; - } - - public void DoRepDataGetMesh(ODEPhysRepData repData) - { - if (!repData.pbs.SculptEntry) - return; - - if (repData.meshState != MeshState.loadingAsset) - return; - - if (repData.assetID == null || repData.assetID == UUID.Zero) - return; - - if (repData.assetID != repData.pbs.SculptTexture) - return; - - // check if it is in cache - GetMesh(repData); - if (repData.meshState != MeshState.needAsset) - { - CreateActorPhysRep(repData); - m_scene.AddChange(repData.actor, changes.PhysRepData, repData); - return; - } - - RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; - if (assetProvider == null) - return; - ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log); - } - - - /// - /// Routine to figure out if we need to mesh this prim with our mesher - /// - /// - /// - public bool needsMeshing(PrimitiveBaseShape pbs) - { - // check sculpts or meshs - if (pbs.SculptEntry) - { - if (meshSculptedPrim) - return true; - - if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs - return true; - - return false; - } - - if (forceSimplePrimMeshing) - return true; - - // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim - - if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) - || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 - && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) - { - - if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 - && pbs.ProfileHollow == 0 - && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 - && pbs.PathBegin == 0 && pbs.PathEnd == 0 - && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 - && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 - && pbs.PathShearX == 0 && pbs.PathShearY == 0) - { - return false; - } - } - - // following code doesn't give meshs to boxes and spheres ever - // and it's odd.. so for now just return true if asked to force meshs - // hopefully mesher will fail if doesn't suport so things still get basic boxes - - int iPropertiesNotSupportedDefault = 0; - - if (pbs.ProfileHollow != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) - iPropertiesNotSupportedDefault++; - - if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) - iPropertiesNotSupportedDefault++; - - if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) - iPropertiesNotSupportedDefault++; - - if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1) - iPropertiesNotSupportedDefault++; - - // test for torus - if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - - // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) - { - if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) - { - iPropertiesNotSupportedDefault++; - } - } - else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) - { - if (pbs.PathCurve == (byte)Extrusion.Straight) - { - iPropertiesNotSupportedDefault++; - } - else if (pbs.PathCurve == (byte)Extrusion.Curve1) - { - iPropertiesNotSupportedDefault++; - } - } - - if (iPropertiesNotSupportedDefault == 0) - { - return false; - } - return true; - } - - // see if we need a mesh and if so if we have a cached one - // called with a new repData - public void CheckMesh(ODEPhysRepData repData) - { - PhysicsActor actor = repData.actor; - PrimitiveBaseShape pbs = repData.pbs; - - if (!needsMeshing(pbs)) - { - repData.meshState = MeshState.noNeed; - return; - } - - IMesh mesh = null; - - Vector3 size = repData.size; - byte shapetype = repData.shapetype; - - bool convex; - - int clod = (int)LevelOfDetail.High; - if (shapetype == 0) - convex = false; - else - { - convex = true; - if (pbs.SculptType != (byte)SculptType.Mesh) - clod = (int)LevelOfDetail.Low; - } - - mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); - - if (mesh == null) - { - if (pbs.SculptEntry) - { - if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) - { - repData.assetID = pbs.SculptTexture; - repData.meshState = MeshState.needAsset; - } - else - repData.meshState = MeshState.MeshFailed; - - return; - } - else - { - repData.meshState = MeshState.needMesh; - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); - if (mesh == null) - { - repData.meshState = MeshState.MeshFailed; - return; - } - } - } - - repData.meshState = MeshState.AssetOK; - repData.mesh = mesh; - - if (pbs.SculptEntry) - { - repData.assetID = pbs.SculptTexture; - } - - pbs.SculptData = Utils.EmptyBytes; - return ; - } - - public void GetMesh(ODEPhysRepData repData) - { - PhysicsActor actor = repData.actor; - - PrimitiveBaseShape pbs = repData.pbs; - - repData.mesh = null; - repData.hasOBB = false; - - if (!needsMeshing(pbs)) - { - repData.meshState = MeshState.noNeed; - return; - } - - if (repData.meshState == MeshState.MeshFailed) - return; - - if (pbs.SculptEntry) - { - if (repData.meshState == MeshState.AssetFailed) - { - if (pbs.SculptTexture == repData.assetID) - return; - } - } - - repData.meshState = MeshState.noNeed; - - IMesh mesh = null; - Vector3 size = repData.size; - byte shapetype = repData.shapetype; - - bool convex; - int clod = (int)LevelOfDetail.High; - if (shapetype == 0) - convex = false; - else - { - convex = true; - if (pbs.SculptType != (byte)SculptType.Mesh) - clod = (int)LevelOfDetail.Low; - } - - mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); - - if (mesh == null) - { - if (pbs.SculptEntry) - { - if (pbs.SculptTexture == UUID.Zero) - return; - - repData.assetID = pbs.SculptTexture; - - if (pbs.SculptData == null || pbs.SculptData.Length == 0) - { - repData.meshState = MeshState.needAsset; - return; - } - } - } - - repData.mesh = mesh; - repData.pbs.SculptData = Utils.EmptyBytes; - - if (mesh == null) - { - if (pbs.SculptEntry) - repData.meshState = MeshState.AssetFailed; - else - repData.meshState = MeshState.MeshFailed; - - return; - } - - repData.meshState = MeshState.AssetOK; - - return; - } - - private void CalculateBasicPrimVolume(ODEPhysRepData repData) - { - PrimitiveBaseShape _pbs = repData.pbs; - Vector3 _size = repData.size; - - float volume = _size.X * _size.Y * _size.Z; // default - float tmp; - - float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - - switch (_pbs.ProfileShape) - { - case ProfileShape.Square: - // default box - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - - hollowVolume *= (0.5f * .5f); - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - //a tube - - volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume * tmp * tmp; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Square: - case HollowShape.Same: - break; - - case HollowShape.Circle: - hollowVolume *= 0.78539816339f; - break; - - case HollowShape.Triangle: - hollowVolume *= 0.5f * 0.5f; - break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - break; - - case ProfileShape.Circle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.78539816339f; // elipse base - - if (hollowAmount > 0.0) - { - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Circle: - break; - - case HollowShape.Square: - hollowVolume *= 0.5f * 2.5984480504799f; - break; - - case HollowShape.Triangle: - hollowVolume *= .5f * 1.27323954473516f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - case ProfileShape.HalfCircle: - if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.5236f; - - if (hollowAmount > 0.0) - { - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Circle: - case HollowShape.Triangle: // diference in sl is minor and odd - case HollowShape.Same: - break; - - case HollowShape.Square: - hollowVolume *= 0.909f; - break; - - // case HollowShape.Triangle: - // hollowVolume *= .827f; - // break; - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - - } - break; - - case ProfileShape.EquilateralTriangle: - - if (_pbs.PathCurve == (byte)Extrusion.Straight) - { - volume *= 0.32475953f; - - if (hollowAmount > 0.0) - { - - // calculate the hollow volume by it's shape compared to the prim shape - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - // Hollow shape is a perfect cyllinder in respect to the cube's scale - // Cyllinder hollow volume calculation - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { - volume *= 0.32475953f; - volume *= 0.01f * (float)(200 - _pbs.PathScaleX); - tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); - volume *= (1.0f - tmp * tmp); - - if (hollowAmount > 0.0) - { - - hollowVolume *= hollowAmount; - - switch (_pbs.HollowShape) - { - case HollowShape.Same: - case HollowShape.Triangle: - hollowVolume *= .25f; - break; - - case HollowShape.Square: - hollowVolume *= 0.499849f * 3.07920140172638f; - break; - - case HollowShape.Circle: - - hollowVolume *= 0.1963495f * 3.07920140172638f; - break; - - default: - hollowVolume = 0; - break; - } - volume *= (1.0f - hollowVolume); - } - } - break; - - default: - break; - } - - float taperX1; - float taperY1; - float taperX; - float taperY; - float pathBegin; - float pathEnd; - float profileBegin; - float profileEnd; - - if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { - taperX1 = _pbs.PathScaleX * 0.01f; - if (taperX1 > 1.0f) - taperX1 = 2.0f - taperX1; - taperX = 1.0f - taperX1; - - taperY1 = _pbs.PathScaleY * 0.01f; - if (taperY1 > 1.0f) - taperY1 = 2.0f - taperY1; - taperY = 1.0f - taperY1; - } - else - { - taperX = _pbs.PathTaperX * 0.01f; - if (taperX < 0.0f) - taperX = -taperX; - taperX1 = 1.0f - taperX; - - taperY = _pbs.PathTaperY * 0.01f; - if (taperY < 0.0f) - taperY = -taperY; - taperY1 = 1.0f - taperY; - } - - volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); - - pathBegin = (float)_pbs.PathBegin * 2.0e-5f; - pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; - volume *= (pathEnd - pathBegin); - - // this is crude aproximation - profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; - profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; - volume *= (profileEnd - profileBegin); - - repData.volume = volume; - } - - private void CalcVolumeData(ODEPhysRepData repData) - { - if (repData.hasOBB) - { - Vector3 OBB = repData.OBB; - } - else - { - Vector3 OBB = repData.size; - OBB.X *= 0.5f; - OBB.Y *= 0.5f; - OBB.Z *= 0.5f; - - repData.OBB = OBB; - repData.OBBOffset = Vector3.Zero; - } - - CalculateBasicPrimVolume(repData); - } - } - - public class ODEAssetRequest - { - ODEMeshWorker m_worker; - private ILog m_log; - ODEPhysRepData repData; - - public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, - ODEPhysRepData pRepData, ILog plog) - { - m_worker = pWorker; - m_log = plog; - repData = pRepData; - - repData.meshState = MeshState.AssetFailed; - if (provider == null) - return; - - if (repData.assetID == null) - return; - - UUID assetID = (UUID) repData.assetID; - if (assetID == UUID.Zero) - return; - - repData.meshState = MeshState.loadingAsset; - provider(assetID, ODEassetReceived); - } - - void ODEassetReceived(AssetBase asset) - { - repData.meshState = MeshState.AssetFailed; - if (asset != null) - { - if (asset.Data != null && asset.Data.Length > 0) - { - repData.meshState = MeshState.noNeed; - - if (!repData.pbs.SculptEntry) - return; - if (repData.pbs.SculptTexture != repData.assetID) - return; - -// repData.pbs.SculptData = new byte[asset.Data.Length]; -// asset.Data.CopyTo(repData.pbs.SculptData,0); - repData.pbs.SculptData = asset.Data; - repData.meshState = MeshState.AssetOK; - m_worker.AssetLoaded(repData); - } - else - m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", - repData.actor.Name, asset.ID.ToString()); - } - else - m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", - repData.actor.Name); - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEModule.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEModule.cs deleted file mode 100644 index 8be7c7c..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODEModule.cs +++ /dev/null @@ -1,97 +0,0 @@ -using System; -using System.Reflection; -using log4net; -using Nini.Config; -using Mono.Addins; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.Framework.Scenes; -using OpenSim.Region.Framework.Interfaces; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "UBITODEPhysicsScene")] - class UbitOdeModule : INonSharedRegionModule - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private bool m_Enabled = false; - private IConfigSource m_config; - private ODEScene m_scene; - private bool OdeUbitLib; - - #region INonSharedRegionModule - - public string Name - { - get { return "UbitODE"; } - } - - public Type ReplaceableInterface - { - get { return null; } - } - - public void Initialise(IConfigSource source) - { - IConfig config = source.Configs["Startup"]; - if (config != null) - { - string physics = config.GetString("physics", string.Empty); - if (physics == Name) - { - m_config = source; - m_Enabled = true; - } - } - } - - public void Close() - { - } - - public void AddRegion(Scene scene) - { - if (!m_Enabled) - return; - - if (Util.IsWindows()) - Util.LoadArchSpecificWindowsDll("ode.dll"); - - // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to - // http://opensimulator.org/mantis/view.php?id=2750). - d.InitODE(); - - string ode_config = d.GetConfiguration(); - if (ode_config != null && ode_config != "") - { - m_log.InfoFormat("[UbitODE] ode library configuration: {0}", ode_config); - - if (ode_config.Contains("ODE_Ubit")) - { - OdeUbitLib = true; - } - } - - m_scene = new ODEScene(scene, m_config, Name, OdeUbitLib); - } - - public void RemoveRegion(Scene scene) - { - if (!m_Enabled || m_scene == null) - return; - - m_scene.Dispose(); - m_scene = null; - } - - public void RegionLoaded(Scene scene) - { - if (!m_Enabled || m_scene == null) - return; - - m_scene.RegionLoaded(); - } - #endregion - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs deleted file mode 100644 index 8e8e069..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs +++ /dev/null @@ -1,3901 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ - -/* Revision 2011/12/13 by Ubit Umarov - * - * - */ - -/* - * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces - * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: - * ODEPrim.cs contains methods dealing with Prim editing, Prim - * characteristics and Kinetic motion. - * ODEDynamics.cs contains methods dealing with Prim Physical motion - * (dynamics) and the associated settings. Old Linear and angular - * motors for dynamic motion have been replace with MoveLinear() - * and MoveAngular(); 'Physical' is used only to switch ODE dynamic - * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to - * switch between 'VEHICLE' parameter use and general dynamics - * settings use. - */ - -//#define SPAM - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Threading; -using log4net; -using OpenMetaverse; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - public class OdePrim : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - private bool m_isphysical; - private bool m_fakeisphysical; - private bool m_isphantom; - private bool m_fakeisphantom; - internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively - - protected bool m_building; - protected bool m_forcePosOrRotation; - private bool m_iscolliding; - - internal bool m_isSelected; - private bool m_delaySelect; - private bool m_lastdoneSelected; - internal bool m_outbounds; - - private Quaternion m_lastorientation; - private Quaternion _orientation; - - private Vector3 _position; - private Vector3 _velocity; - private Vector3 m_torque; - private Vector3 m_lastVelocity; - private Vector3 m_lastposition; - private Vector3 m_rotationalVelocity; - private Vector3 _size; - private Vector3 _acceleration; - private Vector3 m_angularlock = Vector3.One; - private IntPtr Amotor; - - private Vector3 m_force; - private Vector3 m_forceacc; - private Vector3 m_angularForceacc; - - private float m_invTimeStep; - private float m_timeStep; - - private Vector3 m_PIDTarget; - private float m_PIDTau; - private bool m_usePID; - - private float m_PIDHoverHeight; - private float m_PIDHoverTau; - private bool m_useHoverPID; - private PIDHoverType m_PIDHoverType; - private float m_targetHoverHeight; - private float m_groundHeight; - private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - - private int body_autodisable_frames; - public int bodydisablecontrol; - private float m_gravmod = 1.0f; - - // Default we're a Geometry - private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); - // Default colide nonphysical don't try to colide with anything - private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; - - private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Land | - CollisionCategories.VolumeDtc); - -// private bool m_collidesLand = true; - private bool m_collidesWater; -// public bool m_returnCollisions; - - private bool m_NoColide; // for now only for internal use for bad meshs - - - // Default, Collide with Other Geometries, spaces and Bodies - private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; - - public bool m_disabled; - - private uint m_localID; - - private IMesh m_mesh; - private object m_meshlock = new object(); - private PrimitiveBaseShape _pbs; - - private UUID? m_assetID; - private MeshState m_meshState; - - public ODEScene _parent_scene; - - /// - /// The physics space which contains prim geometry - /// - public IntPtr m_targetSpace; - - public IntPtr prim_geom; - public IntPtr _triMeshData; - - private PhysicsActor _parent; - - private List childrenPrim = new List(); - - public float m_collisionscore; - private int m_colliderfilter = 0; - - public IntPtr collide_geom; // for objects: geom if single prim space it linkset - - private float m_density; - private byte m_shapetype; - public bool _zeroFlag; - private bool m_lastUpdateSent; - - public IntPtr Body; - - private Vector3 _target_velocity; - - public Vector3 m_OBBOffset; - public Vector3 m_OBB; - public float primOOBradiusSQ; - - private bool m_hasOBB = true; - - private float m_physCost; - private float m_streamCost; - - public d.Mass primdMass; // prim inertia information on it's own referencial - float primMass; // prim own mass - float primVolume; // prim own volume; - float _mass; // object mass acording to case - - public int givefakepos; - private Vector3 fakepos; - public int givefakeori; - private Quaternion fakeori; - - private int m_eventsubscription; - private int m_cureventsubscription; - private CollisionEventUpdate CollisionEventsThisFrame = null; - private bool SentEmptyCollisionsEvent; - - public volatile bool childPrim; - - public ODEDynamics m_vehicle; - - internal int m_material = (int)Material.Wood; - private float mu; - private float bounce; - - /// - /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. - /// - public override bool IsPhysical // this is not reliable for internal use - { - get { return m_fakeisphysical; } - set - { - m_fakeisphysical = value; // we show imediatly to outside that we changed physical - // and also to stop imediatly some updates - // but real change will only happen in taintprocessing - - if (!value) // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - AddChange(changes.Physical, value); - } - } - - public override bool IsVolumeDtc - { - get { return m_fakeisVolumeDetect; } - set - { - m_fakeisVolumeDetect = value; - AddChange(changes.VolumeDtc, value); - } - } - - public override bool Phantom // this is not reliable for internal use - { - get { return m_fakeisphantom; } - set - { - m_fakeisphantom = value; - AddChange(changes.Phantom, value); - } - } - - public override bool Building // this is not reliable for internal use - { - get { return m_building; } - set - { -// if (value) -// m_building = true; - AddChange(changes.building, value); - } - } - - public override void getContactData(ref ContactData cdata) - { - cdata.mu = mu; - cdata.bounce = bounce; - - // cdata.softcolide = m_softcolide; - cdata.softcolide = false; - - if (m_isphysical) - { - ODEDynamics veh; - if (_parent != null) - veh = ((OdePrim)_parent).m_vehicle; - else - veh = m_vehicle; - - if (veh != null && veh.Type != Vehicle.TYPE_NONE) - cdata.mu *= veh.FrictionFactor; -// cdata.mu *= 0; - } - } - - public override float PhysicsCost - { - get - { - return m_physCost; - } - } - - public override float StreamCost - { - get - { - return m_streamCost; - } - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Prim; } - set { return; } - } - - public override bool SetAlwaysRun - { - get { return false; } - set { return; } - } - - public override uint LocalID - { - get { return m_localID; } - set { m_localID = value; } - } - - public override PhysicsActor ParentActor - { - get - { - if (childPrim) - return _parent; - else - return (PhysicsActor)this; - } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set - { - if (value) - m_isSelected = value; // if true set imediatly to stop moves etc - AddChange(changes.Selected, value); - } - } - - public override bool Flying - { - // no flying prims for you - get { return false; } - set { } - } - - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - if (value) - { - m_colliderfilter += 2; - if (m_colliderfilter > 2) - m_colliderfilter = 2; - } - else - { - m_colliderfilter--; - if (m_colliderfilter < 0) - m_colliderfilter = 0; - } - - if (m_colliderfilter == 0) - m_iscolliding = false; - else - m_iscolliding = true; - } - } - - public override bool CollidingGround - { - get { return false; } - set { return; } - } - - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - - public override bool ThrottleUpdates {get;set;} - - public override bool Stopped - { - get { return _zeroFlag; } - } - - public override Vector3 Position - { - get - { - if (givefakepos > 0) - return fakepos; - else - return _position; - } - - set - { - fakepos = value; - givefakepos++; - AddChange(changes.Position, value); - } - } - - public override Vector3 Size - { - get { return _size; } - set - { - if (value.IsFinite()) - { - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); - } - } - } - - public override float Mass - { - get { return primMass; } - } - - public override Vector3 Force - { - get { return m_force; } - set - { - if (value.IsFinite()) - { - AddChange(changes.Force, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); - } - } - } - - public override void SetVolumeDetect(int param) - { - m_fakeisVolumeDetect = (param != 0); - AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); - } - - public override Vector3 GeometricCenter - { - // this is not real geometric center but a average of positions relative to root prim acording to - // http://wiki.secondlife.com/wiki/llGetGeometricCenter - // ignoring tortured prims details since sl also seems to ignore - // so no real use in doing it on physics - get - { - return Vector3.Zero; - } - } - - public override Vector3 CenterOfMass - { - get - { - lock (_parent_scene.OdeLock) - { - d.Vector3 dtmp; - if (!childPrim && Body != IntPtr.Zero) - { - dtmp = d.BodyGetPosition(Body); - return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); - } - else if (prim_geom != IntPtr.Zero) - { - d.Quaternion dq; - d.GeomCopyQuaternion(prim_geom, out dq); - Quaternion q; - q.X = dq.X; - q.Y = dq.Y; - q.Z = dq.Z; - q.W = dq.W; - - Vector3 Ptot = m_OBBOffset * q; - dtmp = d.GeomGetPosition(prim_geom); - Ptot.X += dtmp.X; - Ptot.Y += dtmp.Y; - Ptot.Z += dtmp.Z; - - // if(childPrim) we only know about physical linksets - return Ptot; -/* - float tmass = _mass; - Ptot *= tmass; - - float m; - - foreach (OdePrim prm in childrenPrim) - { - m = prm._mass; - Ptot += prm.CenterOfMass * m; - tmass += m; - } - - if (tmass == 0) - tmass = 0; - else - tmass = 1.0f / tmass; - - Ptot *= tmass; - return Ptot; -*/ - } - else - return _position; - } - } - } - - public override Vector3 OOBsize - { - get - { - return m_OBB; - } - } - - public override Vector3 OOBoffset - { - get - { - return m_OBBOffset; - } - } - - public override float OOBRadiusSQ - { - get - { - return primOOBradiusSQ; - } - } - - public override PrimitiveBaseShape Shape - { - set - { -// AddChange(changes.Shape, value); - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); - } - } - - public override byte PhysicsShapeType - { - get - { - return m_shapetype; - } - set - { - m_shapetype = value; - _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); - } - } - - public override Vector3 Velocity - { - get - { - if (_zeroFlag) - return Vector3.Zero; - return _velocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.Velocity, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); - } - - } - } - - public override Vector3 Torque - { - get - { - if (!IsPhysical || Body == IntPtr.Zero) - return Vector3.Zero; - - return m_torque; - } - - set - { - if (value.IsFinite()) - { - AddChange(changes.Torque, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); - } - } - } - - public override float CollisionScore - { - get { return m_collisionscore; } - set { m_collisionscore = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get - { - if (givefakeori > 0) - return fakeori; - else - - return _orientation; - } - set - { - if (QuaternionIsFinite(value)) - { - fakeori = value; - givefakeori++; - - value.Normalize(); - - AddChange(changes.Orientation, value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); - - } - } - - public override Vector3 Acceleration - { - get { return _acceleration; } - set { } - } - - public override Vector3 RotationalVelocity - { - get - { - Vector3 pv = Vector3.Zero; - if (_zeroFlag) - return pv; - - if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) - return pv; - - return m_rotationalVelocity; - } - set - { - if (value.IsFinite()) - { - AddChange(changes.AngVelocity, value); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); - } - } - } - - public override float Buoyancy - { - get { return m_buoyancy; } - set - { - AddChange(changes.Buoyancy,value); - } - } - - public override bool FloatOnWater - { - set - { - AddChange(changes.CollidesWater, value); - } - } - - public override Vector3 PIDTarget - { - set - { - if (value.IsFinite()) - { - AddChange(changes.PIDTarget,value); - } - else - m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } - } - - public override bool PIDActive - { - get - { - return m_usePID; - } - set - { - AddChange(changes.PIDActive,value); - } - } - - public override float PIDTau - { - set - { - float tmp = 0; - if (value > 0) - { - float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); - if (value < mint) - tmp = mint; - else - tmp = value; - } - AddChange(changes.PIDTau,tmp); - } - } - - public override float PIDHoverHeight - { - set - { - AddChange(changes.PIDHoverHeight,value); - } - } - public override bool PIDHoverActive - { - set - { - AddChange(changes.PIDHoverActive, value); - } - } - - public override PIDHoverType PIDHoverType - { - set - { - AddChange(changes.PIDHoverType,value); - } - } - - public override float PIDHoverTau - { - set - { - float tmp =0; - if (value > 0) - { - float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); - if (value < mint) - tmp = mint; - else - tmp = value; - } - AddChange(changes.PIDHoverTau, tmp); - } - } - - public override Quaternion APIDTarget { set { return; } } - - public override bool APIDActive { set { return; } } - - public override float APIDStrength { set { return; } } - - public override float APIDDamping { set { return; } } - - public override int VehicleType - { - // we may need to put a fake on this - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set - { - AddChange(changes.VehicleType, value); - } - } - - public override void VehicleFloatParam(int param, float value) - { - strVehicleFloatParam fp = new strVehicleFloatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleFloatParam, fp); - } - - public override void VehicleVectorParam(int param, Vector3 value) - { - strVehicleVectorParam fp = new strVehicleVectorParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleVectorParam, fp); - } - - public override void VehicleRotationParam(int param, Quaternion value) - { - strVehicleQuatParam fp = new strVehicleQuatParam(); - fp.param = param; - fp.value = value; - AddChange(changes.VehicleRotationParam, fp); - } - - public override void VehicleFlags(int param, bool value) - { - strVehicleBoolParam bp = new strVehicleBoolParam(); - bp.param = param; - bp.value = value; - AddChange(changes.VehicleFlags, bp); - } - - public override void SetVehicle(object vdata) - { - AddChange(changes.SetVehicle, vdata); - } - public void SetAcceleration(Vector3 accel) - { - _acceleration = accel; - } - - public override void AddForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { - if(pushforce) - AddChange(changes.AddForce, force); - else // a impulse - AddChange(changes.AddForce, force * m_invTimeStep); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); - } - //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - if (force.IsFinite()) - { -// if(pushforce) for now applyrotationimpulse seems more happy applied as a force - AddChange(changes.AddAngForce, force); -// else // a impulse -// AddChange(changes.AddAngForce, force * m_invTimeStep); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); - } - } - - public override void CrossingFailure() - { - if (m_outbounds) - { - _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); - - m_lastposition = _position; - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - - m_lastVelocity = _velocity; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - if(Body != IntPtr.Zero) - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - if (prim_geom != IntPtr.Zero) - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - - m_outbounds = false; - changeDisable(false); - base.RequestPhysicsterseUpdate(); - } - } - - public override void SetMomentum(Vector3 momentum) - { - } - - public override void SetMaterial(int pMaterial) - { - m_material = pMaterial; - mu = _parent_scene.m_materialContactsData[pMaterial].mu; - bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; - } - - public override float Density - { - get - { - return m_density * 100f; - } - set - { - m_density = value / 100f; - // for not prim mass is not updated since this implies full rebuild of body inertia TODO - } - } - public override float GravModifier - { - get - { - return m_gravmod; - } - set - { - m_gravmod = value; - if (m_vehicle != null) - m_vehicle.GravMod = m_gravmod; - } - } - public override float Friction - { - get - { - return mu; - } - set - { - mu = value; - } - } - - public override float Restitution - { - get - { - return bounce; - } - set - { - bounce = value; - } - } - - public void setPrimForRemoval() - { - AddChange(changes.Remove, null); - } - - public override void link(PhysicsActor obj) - { - AddChange(changes.Link, obj); - } - - public override void delink() - { - AddChange(changes.DeLink, null); - } - - public override void LockAngularMotion(Vector3 axis) - { - // reverse the zero/non zero values for ODE. - if (axis.IsFinite()) - { - axis.X = (axis.X > 0) ? 1f : 0f; - axis.Y = (axis.Y > 0) ? 1f : 0f; - axis.Z = (axis.Z > 0) ? 1f : 0f; -// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); - AddChange(changes.AngLock, axis); - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - m_cureventsubscription = 0; - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); - SentEmptyCollisionsEvent = false; - } - - public override void UnSubscribeEvents() - { - if (CollisionEventsThisFrame != null) - { - CollisionEventsThisFrame.Clear(); - CollisionEventsThisFrame = null; - } - m_eventsubscription = 0; - _parent_scene.RemoveCollisionEventReporting(this); - } - - public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) - { - if (CollisionEventsThisFrame == null) - CollisionEventsThisFrame = new CollisionEventUpdate(); -// if(CollisionEventsThisFrame.Count < 32) - CollisionEventsThisFrame.AddCollider(CollidedWith, contact); - } - - public void SendCollisions() - { - if (CollisionEventsThisFrame == null) - return; - - if (m_cureventsubscription < m_eventsubscription) - return; - - m_cureventsubscription = 0; - - int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; - - if (!SentEmptyCollisionsEvent || ncolisions > 0) - { - base.SendCollisionUpdate(CollisionEventsThisFrame); - - if (ncolisions == 0) - { - SentEmptyCollisionsEvent = true; - _parent_scene.RemoveCollisionEventReporting(this); - } - else - { - SentEmptyCollisionsEvent = false; - CollisionEventsThisFrame.Clear(); - } - } - } - - internal void AddCollisionFrameTime(int t) - { - if (m_cureventsubscription < 50000) - m_cureventsubscription += t; - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - public OdePrim(String primName, ODEScene parent_scene, Vector3 pos, Vector3 size, - Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) - { - Name = primName; - LocalID = plocalID; - - m_vehicle = null; - - if (!pos.IsFinite()) - { - pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), - parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); - } - _position = pos; - givefakepos = 0; - - m_timeStep = parent_scene.ODE_STEPSIZE; - m_invTimeStep = 1f / m_timeStep; - - m_density = parent_scene.geomDefaultDensity; - body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - Body = IntPtr.Zero; - - if (!size.IsFinite()) - { - size = new Vector3(0.5f, 0.5f, 0.5f); - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); - } - - if (size.X <= 0) size.X = 0.01f; - if (size.Y <= 0) size.Y = 0.01f; - if (size.Z <= 0) size.Z = 0.01f; - - _size = size; - - if (!QuaternionIsFinite(rotation)) - { - rotation = Quaternion.Identity; - m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); - } - - _orientation = rotation; - givefakeori = 0; - - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = IntPtr.Zero; - - if (pos.Z < 0) - { - m_isphysical = false; - } - else - { - m_isphysical = pisPhysical; - } - m_fakeisphysical = m_isphysical; - - m_isVolumeDetect = false; - m_fakeisVolumeDetect = false; - - m_force = Vector3.Zero; - - m_iscolliding = false; - m_colliderfilter = 0; - m_NoColide = false; - - _triMeshData = IntPtr.Zero; - - m_shapetype = _shapeType; - - m_lastdoneSelected = false; - m_isSelected = false; - m_delaySelect = false; - - m_isphantom = pisPhantom; - m_fakeisphantom = pisPhantom; - - mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; - bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; - - m_building = true; // control must set this to false when done - - // get basic mass parameters - ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); - - primVolume = repData.volume; - m_OBB = repData.OBB; - m_OBBOffset = repData.OBBOffset; - - UpdatePrimBodyData(); - } - - private void resetCollisionAccounting() - { - m_collisionscore = 0; - } - - private void UpdateCollisionCatFlags() - { - if(m_isphysical && m_disabled) - { - m_collisionCategories = 0; - m_collisionFlags = 0; - } - - else if (m_isSelected) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = 0; - } - - else if (m_isVolumeDetect) - { - m_collisionCategories = CollisionCategories.VolumeDtc; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; - else - m_collisionFlags = 0; - } - else if (m_isphantom) - { - m_collisionCategories = CollisionCategories.Phantom; - if (m_isphysical) - m_collisionFlags = CollisionCategories.Land; - else - m_collisionFlags = 0; - } - else - { - m_collisionCategories = CollisionCategories.Geom; - if (m_isphysical) - m_collisionFlags = m_default_collisionFlagsPhysical; - else - m_collisionFlags = m_default_collisionFlagsNotPhysical; - } - } - - private void ApplyCollisionCatFlags() - { - if (prim_geom != IntPtr.Zero) - { - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - { - if (m_isphysical && m_disabled) - { - prm.m_collisionCategories = 0; - prm.m_collisionFlags = 0; - } - else - { - // preserve some - if (prm.m_isSelected) - { - prm.m_collisionCategories = CollisionCategories.Selected; - prm.m_collisionFlags = 0; - } - else if (prm.m_isVolumeDetect) - { - prm.m_collisionCategories = CollisionCategories.VolumeDtc; - if (m_isphysical) - prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; - else - prm.m_collisionFlags = 0; - } - else if (prm.m_isphantom) - { - prm.m_collisionCategories = CollisionCategories.Phantom; - if (m_isphysical) - prm.m_collisionFlags = CollisionCategories.Land; - else - prm.m_collisionFlags = 0; - } - else - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - } - } - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - if (m_isphysical) - d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); - else - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); - } - } - } - } - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, 0); - d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); - } - } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); - } - } - } - } - - private void createAMotor(Vector3 axis) - { - if (Body == IntPtr.Zero) - return; - - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - - int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); - - if (axisnum <= 0) - return; - - // stop it - d.BodySetTorque(Body, 0, 0, 0); - d.BodySetAngularVel(Body, 0, 0, 0); - - Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); - d.JointAttach(Amotor, Body, IntPtr.Zero); - - d.JointSetAMotorMode(Amotor, 0); - - d.JointSetAMotorNumAxes(Amotor, axisnum); - - // get current orientation to lock - - d.Quaternion dcur = d.BodyGetQuaternion(Body); - Quaternion curr; // crap convertion between identical things - curr.X = dcur.X; - curr.Y = dcur.Y; - curr.Z = dcur.Z; - curr.W = dcur.W; - Vector3 ax; - - int i = 0; - int j = 0; - if (axis.X == 0) - { - ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X - // ODE should do this with axis relative to body 1 but seems to fail - d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, 0, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); - i++; - j = 256; // move to next axis set - } - - if (axis.Y == 0) - { - ax = (new Vector3(0, 1, 0)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - i++; - j += 256; - } - - if (axis.Z == 0) - { - ax = (new Vector3(0, 0, 1)) * curr; - d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); - d.JointSetAMotorAngle(Amotor, i, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); - d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); - } - } - - - private void SetGeom(IntPtr geom) - { - prim_geom = geom; - //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); - if (prim_geom != IntPtr.Zero) - { - - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - if (m_isphysical) - { - d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); - } - else - { - d.GeomSetCollideBits(prim_geom, 0); - d.GeomDisable(prim_geom); - } - } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - } - - UpdatePrimBodyData(); - _parent_scene.actor_name_map[prim_geom] = this; - -/* -// debug - d.AABB aabb; - d.GeomGetAABB(prim_geom, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if( x > 60.0f || y > 60.0f || z > 60.0f) - m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", - Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); - else if (x < 0.001f || y < 0.001f || z < 0.001f) - m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", - Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); - -// -*/ - - } - else - m_log.Warn("Setting bad Geom"); - } - - private bool GetMeshGeom() - { - IntPtr vertices, indices; - int vertexCount, indexCount; - int vertexStride, triStride; - - IMesh mesh = m_mesh; - - if (mesh == null) - return false; - - mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); - mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); - - if (vertexCount == 0 || indexCount == 0) - { - m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", - Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); - - m_hasOBB = false; - m_OBBOffset = Vector3.Zero; - m_OBB = _size * 0.5f; - - m_physCost = 0.1f; - m_streamCost = 1.0f; - - _parent_scene.mesher.ReleaseMesh(mesh); - m_meshState = MeshState.MeshFailed; - m_mesh = null; - return false; - } - - IntPtr geo = IntPtr.Zero; - - try - { - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); - d.GeomTriMeshDataPreprocess(_triMeshData); - - geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); - } - - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); - if (_triMeshData != IntPtr.Zero) - { - try - { - d.GeomTriMeshDataDestroy(_triMeshData); - } - catch - { - } - } - _triMeshData = IntPtr.Zero; - - m_hasOBB = false; - m_OBBOffset = Vector3.Zero; - m_OBB = _size * 0.5f; - m_physCost = 0.1f; - m_streamCost = 1.0f; - - _parent_scene.mesher.ReleaseMesh(mesh); - m_meshState = MeshState.MeshFailed; - m_mesh = null; - return false; - } - - m_physCost = 0.0013f * (float)indexCount; - // todo - m_streamCost = 1.0f; - - SetGeom(geo); - - return true; - } - - private void CreateGeom() - { - bool hasMesh = false; - - m_NoColide = false; - - if ((m_meshState & MeshState.MeshNoColide) != 0) - m_NoColide = true; - - else if(m_mesh != null) - { - if (GetMeshGeom()) - hasMesh = true; - else - m_NoColide = true; - } - - - if (!hasMesh) - { - IntPtr geo = IntPtr.Zero; - - if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 - && _size.X == _size.Y && _size.Y == _size.Z) - { // it's a sphere - try - { - geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); - return; - } - } - else - {// do it as a box - try - { - geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - } - catch (Exception e) - { - m_log.Warn("[PHYSICS]: Create box failed: {0}", e); - return; - } - } - m_physCost = 0.1f; - m_streamCost = 1.0f; - SetGeom(geo); - } - } - - private void RemoveGeom() - { - if (prim_geom != IntPtr.Zero) - { - _parent_scene.actor_name_map.Remove(prim_geom); - - try - { - d.GeomDestroy(prim_geom); - if (_triMeshData != IntPtr.Zero) - { - d.GeomTriMeshDataDestroy(_triMeshData); - _triMeshData = IntPtr.Zero; - } - } - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); - } - - prim_geom = IntPtr.Zero; - collide_geom = IntPtr.Zero; - m_targetSpace = IntPtr.Zero; - } - else - { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); - } - - lock (m_meshlock) - { - if (m_mesh != null) - { - _parent_scene.mesher.ReleaseMesh(m_mesh); - m_mesh = null; - } - } - - Body = IntPtr.Zero; - m_hasOBB = false; - } - - //sets non physical prim m_targetSpace to right space in spaces grid for static prims - // should only be called for non physical prims unless they are becoming non physical - private void SetInStaticSpace(OdePrim prim) - { - IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); - prim.m_targetSpace = targetSpace; - collide_geom = IntPtr.Zero; - } - - public void enableBodySoft() - { - m_disabled = false; - if (!childPrim && !m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - - d.BodyEnable(Body); - } - } - resetCollisionAccounting(); - } - - private void disableBodySoft() - { - m_disabled = true; - if (!childPrim) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_isSelected) - m_collisionFlags = CollisionCategories.Selected; - else - m_collisionCategories = 0; - m_collisionFlags = 0; - ApplyCollisionCatFlags(); - d.BodyDisable(Body); - } - } - } - - private void MakeBody() - { - if (!m_isphysical) // only physical get bodies - return; - - if (childPrim) // child prims don't get bodies; - return; - - if (m_building) - return; - - if (prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); - return; - } - - if (Body != IntPtr.Zero) - { - DestroyBody(); - m_log.Warn("[PHYSICS]: MakeBody called having a body"); - } - - if (d.GeomGetBody(prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); - } - - d.Matrix3 mymat = new d.Matrix3(); - d.Quaternion myrot = new d.Quaternion(); - d.Mass objdmass = new d.Mass { }; - - Body = d.BodyCreate(_parent_scene.world); - - objdmass = primdMass; - - // rotate inertia - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - - // set the body rotation - d.BodySetRotation(Body, ref mymat); - - // recompute full object inertia if needed - if (childrenPrim.Count > 0) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - d.Mass tmpdmass = new d.Mass { }; - Vector3 rcm; - - rcm.X = _position.X; - rcm.Y = _position.Y; - rcm.Z = _position.Z; - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - { - m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); - continue; - } - - tmpdmass = prm.primdMass; - - // apply prim current rotation to inertia - quat.X = prm._orientation.X; - quat.Y = prm._orientation.Y; - quat.Z = prm._orientation.Z; - quat.W = prm._orientation.W; - d.RfromQ(out mat, ref quat); - d.MassRotate(ref tmpdmass, ref mat); - - Vector3 ppos = prm._position; - ppos.X -= rcm.X; - ppos.Y -= rcm.Y; - ppos.Z -= rcm.Z; - // refer inertia to root prim center of mass position - d.MassTranslate(ref tmpdmass, - ppos.X, - ppos.Y, - ppos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia - // fix prim colision cats - - if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) - { - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); - } - - d.GeomClearOffset(prm.prim_geom); - d.GeomSetBody(prm.prim_geom, Body); - prm.Body = Body; - d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation - } - } - } - - d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset - // associate root geom with body - d.GeomSetBody(prim_geom, Body); - - d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); - d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); - - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - myrot.X = -myrot.X; - myrot.Y = -myrot.Y; - myrot.Z = -myrot.Z; - - d.RfromQ(out mymat, ref myrot); - d.MassRotate(ref objdmass, ref mymat); - - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode(Body, false); - - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - d.BodySetAutoDisableAngularThreshold(Body, 0.01f); - d.BodySetAutoDisableLinearThreshold(Body, 0.01f); - d.BodySetDamping(Body, .005f, .001f); - - if (m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(m_targetSpace); - if (d.SpaceQuery(m_targetSpace, prim_geom)) - d.SpaceRemove(m_targetSpace, prim_geom); - } - - if (childrenPrim.Count == 0) - { - collide_geom = prim_geom; - m_targetSpace = _parent_scene.ActiveSpace; - } - else - { - m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); - d.HashSpaceSetLevels(m_targetSpace, -2, 8); - d.SpaceSetSublevel(m_targetSpace, 3); - d.SpaceSetCleanup(m_targetSpace, false); - - d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(m_targetSpace, 0); - collide_geom = m_targetSpace; - } - - d.SpaceAdd(m_targetSpace, prim_geom); - - if (m_delaySelect) - { - m_isSelected = true; - m_delaySelect = false; - } - - m_collisionscore = 0; - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - - _parent_scene.addActivePrim(this); - - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.prim_geom == IntPtr.Zero) - continue; - - Vector3 ppos = prm._position; - d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position - - if (prm.m_targetSpace != m_targetSpace) - { - if (prm.m_targetSpace != IntPtr.Zero) - { - _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); - if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) - d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); - } - prm.m_targetSpace = m_targetSpace; - d.SpaceAdd(m_targetSpace, prm.prim_geom); - } - - prm.m_collisionscore = 0; - - if(!m_disabled) - prm.m_disabled = false; - - _parent_scene.addActivePrim(prm); - } - } - - // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) - { - createAMotor(m_angularlock); - } - - - if (m_isSelected || m_disabled) - { - d.BodyDisable(Body); - } - else - { - d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - } - _parent_scene.addActiveGroups(this); - } - - private void DestroyBody() - { - if (Body != IntPtr.Zero) - { - _parent_scene.remActivePrim(this); - - collide_geom = IntPtr.Zero; - - if (m_disabled) - m_collisionCategories = 0; - else if (m_isSelected) - m_collisionCategories = CollisionCategories.Selected; - else if (m_isVolumeDetect) - m_collisionCategories = CollisionCategories.VolumeDtc; - else if (m_isphantom) - m_collisionCategories = CollisionCategories.Phantom; - else - m_collisionCategories = CollisionCategories.Geom; - - m_collisionFlags = 0; - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - } - UpdateDataFromGeom(); - d.GeomSetBody(prim_geom, IntPtr.Zero); - SetInStaticSpace(this); - } - - if (!childPrim) - { - lock (childrenPrim) - { - foreach (OdePrim prm in childrenPrim) - { - _parent_scene.remActivePrim(prm); - - if (prm.m_isSelected) - prm.m_collisionCategories = CollisionCategories.Selected; - else if (prm.m_isVolumeDetect) - prm.m_collisionCategories = CollisionCategories.VolumeDtc; - else if (prm.m_isphantom) - prm.m_collisionCategories = CollisionCategories.Phantom; - else - prm.m_collisionCategories = CollisionCategories.Geom; - - prm.m_collisionFlags = 0; - - if (prm.prim_geom != IntPtr.Zero) - { - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); - } - prm.UpdateDataFromGeom(); - SetInStaticSpace(prm); - } - prm.Body = IntPtr.Zero; - prm._mass = prm.primMass; - prm.m_collisionscore = 0; - } - } - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - _parent_scene.remActiveGroup(this); - d.BodyDestroy(Body); - } - Body = IntPtr.Zero; - } - _mass = primMass; - m_collisionscore = 0; - } - - private void FixInertia(Vector3 NewPos,Quaternion newrot) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - - d.BodyGetMass(Body, out tmpdmass); - objdmass = tmpdmass; - - d.Vector3 dobjpos; - d.Vector3 thispos; - - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - - // get prim own inertia in its local frame - tmpdmass = primdMass; - - // transform to object frame - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); - - // back prim own inertia - tmpdmass = primdMass; - - // update to new position and orientation - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); - - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); - - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); - } - d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); - - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - } - - - - private void FixInertia(Vector3 NewPos) - { - d.Matrix3 primmat = new d.Matrix3(); - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Mass primmass = new d.Mass { }; - - d.Vector3 dobjpos; - d.Vector3 thispos; - - d.BodyGetMass(Body, out objdmass); - - // get prim own inertia in its local frame - primmass = primdMass; - // transform to object frame - primmat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref primmass, ref primmat); - - tmpdmass = primmass; - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); - - // update to new position - _position = NewPos; - d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); - - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref primmass, - thispos.X, - thispos.Y, - thispos.Z); - - d.MassAdd(ref objdmass, ref primmass); - - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); - } - - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); - - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - } - - private void FixInertia(Quaternion newrot) - { - d.Matrix3 mat = new d.Matrix3(); - d.Quaternion quat = new d.Quaternion(); - - d.Mass tmpdmass = new d.Mass { }; - d.Mass objdmass = new d.Mass { }; - d.Vector3 dobjpos; - d.Vector3 thispos; - - d.BodyGetMass(Body, out objdmass); - - // get prim own inertia in its local frame - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - // transform to object frame - thispos = d.GeomGetOffsetPosition(prim_geom); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - // subtract current prim inertia from object - DMassSubPartFromObj(ref tmpdmass, ref objdmass); - - // update to new orientation - _orientation = newrot; - quat.X = newrot.X; - quat.Y = newrot.Y; - quat.Z = newrot.Z; - quat.W = newrot.W; - d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); - - tmpdmass = primdMass; - mat = d.GeomGetOffsetRotation(prim_geom); - d.MassRotate(ref tmpdmass, ref mat); - d.MassTranslate(ref tmpdmass, - thispos.X, - thispos.Y, - thispos.Z); - - d.MassAdd(ref objdmass, ref tmpdmass); - - // fix all positions - IntPtr g = d.BodyGetFirstGeom(Body); - while (g != IntPtr.Zero) - { - thispos = d.GeomGetOffsetPosition(g); - thispos.X -= objdmass.c.X; - thispos.Y -= objdmass.c.Y; - thispos.Z -= objdmass.c.Z; - d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); - g = d.dBodyGetNextGeom(g); - } - - d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); - // get current object position and rotation - dobjpos = d.BodyGetPosition(Body); - - d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); - d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body - d.BodySetMass(Body, ref objdmass); - _mass = objdmass.mass; - } - - - #region Mass Calculation - - private void UpdatePrimBodyData() - { - primMass = m_density * primVolume; - - if (primMass <= 0) - primMass = 0.0001f;//ckrinke: Mass must be greater then zero. - if (primMass > _parent_scene.maximumMassObject) - primMass = _parent_scene.maximumMassObject; - - _mass = primMass; // just in case - - d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); - - d.MassTranslate(ref primdMass, - m_OBBOffset.X, - m_OBBOffset.Y, - m_OBBOffset.Z); - - primOOBradiusSQ = m_OBB.LengthSquared(); - - if (_triMeshData != IntPtr.Zero) - { - float pc = m_physCost; - float psf = primOOBradiusSQ; - psf *= 1.33f * .2f; - pc *= psf; - if (pc < 0.1f) - pc = 0.1f; - - m_physCost = pc; - } - else - m_physCost = 0.1f; - - m_streamCost = 1.0f; - } - - #endregion - - - /// - /// Add a child prim to this parent prim. - /// - /// Child prim - // I'm the parent - // prim is the child - public void ParentPrim(OdePrim prim) - { - //Console.WriteLine("ParentPrim " + m_primName); - if (this.m_localID != prim.m_localID) - { - DestroyBody(); // for now we need to rebuil entire object on link change - - lock (childrenPrim) - { - // adopt the prim - if (!childrenPrim.Contains(prim)) - childrenPrim.Add(prim); - - // see if this prim has kids and adopt them also - // should not happen for now - foreach (OdePrim prm in prim.childrenPrim) - { - if (!childrenPrim.Contains(prm)) - { - if (prm.Body != IntPtr.Zero) - { - if (prm.prim_geom != IntPtr.Zero) - d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if (prm.Body != prim.Body) - prm.DestroyBody(); // don't loose bodies around - prm.Body = IntPtr.Zero; - } - - childrenPrim.Add(prm); - prm._parent = this; - } - } - } - //Remove old children from the prim - prim.childrenPrim.Clear(); - - if (prim.Body != IntPtr.Zero) - { - if (prim.prim_geom != IntPtr.Zero) - d.GeomSetBody(prim.prim_geom, IntPtr.Zero); - prim.DestroyBody(); // don't loose bodies around - prim.Body = IntPtr.Zero; - } - - prim.childPrim = true; - prim._parent = this; - - MakeBody(); // full nasty reconstruction - } - } - - private void UpdateChildsfromgeom() - { - if (childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.UpdateDataFromGeom(); - } - } - - private void UpdateDataFromGeom() - { - if (prim_geom != IntPtr.Zero) - { - d.Quaternion qtmp; - d.GeomCopyQuaternion(prim_geom, out qtmp); - _orientation.X = qtmp.X; - _orientation.Y = qtmp.Y; - _orientation.Z = qtmp.Z; - _orientation.W = qtmp.W; -/* -// Debug - float qlen = _orientation.Length(); - if (qlen > 1.01f || qlen < 0.99) - m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); -// -*/ - _orientation.Normalize(); - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - } - } - - private void ChildDelink(OdePrim odePrim, bool remakebodies) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) // delinking the root prim - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - if (remakebodies) - newroot.MakeBody(); - } - } - } - - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - // odePrim.UpdateDataFromGeom(); - if (remakebodies) - odePrim.MakeBody(); - } - } - if (remakebodies) - MakeBody(); - } - - protected void ChildRemove(OdePrim odePrim, bool reMakeBody) - { - // Okay, we have a delinked child.. destroy all body and remake - if (odePrim != this && !childrenPrim.Contains(odePrim)) - return; - - DestroyBody(); - - if (odePrim == this) - { - OdePrim newroot = null; - lock (childrenPrim) - { - if (childrenPrim.Count > 0) - { - newroot = childrenPrim[0]; - childrenPrim.RemoveAt(0); - foreach (OdePrim prm in childrenPrim) - { - newroot.childrenPrim.Add(prm); - } - childrenPrim.Clear(); - } - if (newroot != null) - { - newroot.childPrim = false; - newroot._parent = null; - newroot.MakeBody(); - } - } - if (reMakeBody) - MakeBody(); - return; - } - else - { - lock (childrenPrim) - { - childrenPrim.Remove(odePrim); - odePrim.childPrim = false; - odePrim._parent = null; - if (reMakeBody) - odePrim.MakeBody(); - } - } - MakeBody(); - } - - - #region changes - - private void changeadd() - { - } - - private void changeAngularLock(Vector3 newLock) - { - // do we have a Physical object? - if (Body != IntPtr.Zero) - { - //Check that we have a Parent - //If we have a parent then we're not authorative here - if (_parent == null) - { - if (!newLock.ApproxEquals(Vector3.One, 0f)) - { - createAMotor(newLock); - } - else - { - if (Amotor != IntPtr.Zero) - { - d.JointDestroy(Amotor); - Amotor = IntPtr.Zero; - } - } - } - } - // Store this for later in case we get turned into a separate body - m_angularlock = newLock; - } - - private void changeLink(OdePrim NewParent) - { - if (_parent == null && NewParent != null) - { - NewParent.ParentPrim(this); - } - else if (_parent != null) - { - if (_parent is OdePrim) - { - if (NewParent != _parent) - { - (_parent as OdePrim).ChildDelink(this, false); // for now... - childPrim = false; - - if (NewParent != null) - { - NewParent.ParentPrim(this); - } - } - } - } - _parent = NewParent; - } - - - private void Stop() - { - if (!childPrim) - { -// m_force = Vector3.Zero; - m_forceacc = Vector3.Zero; - m_angularForceacc = Vector3.Zero; -// m_torque = Vector3.Zero; - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - _target_velocity = Vector3.Zero; - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Stop(); - - _zeroFlag = false; - base.RequestPhysicsterseUpdate(); - } - - if (Body != IntPtr.Zero) - { - d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetTorque(Body, 0f, 0f, 0f); - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetAngularVel(Body, 0f, 0f, 0f); - } - } - - private void changePhantomStatus(bool newval) - { - m_isphantom = newval; - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - -/* not in use - internal void ChildSelectedChange(bool childSelect) - { - if(childPrim) - return; - - if (childSelect == m_isSelected) - return; - - if (childSelect) - { - DoSelectedStatus(true); - } - - else - { - foreach (OdePrim prm in childrenPrim) - { - if (prm.m_isSelected) - return; - } - DoSelectedStatus(false); - } - } -*/ - private void changeSelectedStatus(bool newval) - { - if (m_lastdoneSelected == newval) - return; - - m_lastdoneSelected = newval; - DoSelectedStatus(newval); - } - - private void CheckDelaySelect() - { - if (m_delaySelect) - { - DoSelectedStatus(m_isSelected); - } - } - - private void DoSelectedStatus(bool newval) - { - m_isSelected = newval; - Stop(); - - if (newval) - { - if (!childPrim && Body != IntPtr.Zero) - d.BodyDisable(Body); - - if (m_delaySelect || m_isphysical) - { - m_collisionCategories = CollisionCategories.Selected; - m_collisionFlags = 0; - - if (!childPrim) - { - foreach (OdePrim prm in childrenPrim) - { - prm.m_collisionCategories = m_collisionCategories; - prm.m_collisionFlags = m_collisionFlags; - - if (prm.prim_geom != IntPtr.Zero) - { - - if (prm.m_NoColide) - { - d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, 0); - } - else - { - d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); - } - } - prm.m_delaySelect = false; - } - } -// else if (_parent != null) -// ((OdePrim)_parent).ChildSelectedChange(true); - - - if (prim_geom != IntPtr.Zero) - { - if (m_NoColide) - { - d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, 0); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, 0); - d.GeomSetCollideBits(collide_geom, 0); - } - - } - else - { - d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); - if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) - { - d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); - d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); - } - } - } - - m_delaySelect = false; - } - else if(!m_isphysical) - { - m_delaySelect = true; - } - } - else - { - if (!childPrim) - { - if (Body != IntPtr.Zero && !m_disabled) - d.BodyEnable(Body); - } -// else if (_parent != null) -// ((OdePrim)_parent).ChildSelectedChange(false); - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - - m_delaySelect = false; - } - - resetCollisionAccounting(); - } - - private void changePosition(Vector3 newPos) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _position = newPos; - } - - else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) - { - FixInertia(newPos); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; -// changeSelectedStatus(); - resetCollisionAccounting(); - } - - private void changeOrientation(Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim) // inertia is messed, must rebuild - { - if (m_building) - { - _orientation = newOri; - } -/* - else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) - { - FixInertia(_position, newOri); - if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } -*/ - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - } - } - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - resetCollisionAccounting(); - } - - private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) - { - CheckDelaySelect(); - if (m_isphysical) - { - if (childPrim && m_building) // inertia is messed, must rebuild - { - _position = newPos; - _orientation = newOri; - } - else - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - if (_position != newPos) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - } - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - else - { - // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - - if (prim_geom != IntPtr.Zero) - { - if (newOri != _orientation) - { - d.Quaternion myrot = new d.Quaternion(); - myrot.X = newOri.X; - myrot.Y = newOri.Y; - myrot.Z = newOri.Z; - myrot.W = newOri.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - _orientation = newOri; - } - - if (newPos != _position) - { - d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); - _position = newPos; - - m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); - } - } - } - givefakepos--; - if (givefakepos < 0) - givefakepos = 0; - givefakeori--; - if (givefakeori < 0) - givefakeori = 0; - resetCollisionAccounting(); - } - - private void changeDisable(bool disable) - { - if (disable) - { - if (!m_disabled) - disableBodySoft(); - } - else - { - if (m_disabled) - enableBodySoft(); - } - } - - private void changePhysicsStatus(bool NewStatus) - { - CheckDelaySelect(); - - m_isphysical = NewStatus; - - if (!childPrim) - { - if (NewStatus) - { - if (Body == IntPtr.Zero) - MakeBody(); - } - else - { - if (Body != IntPtr.Zero) - { - DestroyBody(); - } - Stop(); - } - } - - resetCollisionAccounting(); - } - - private void changeSize(Vector3 newSize) - { - } - - private void changeShape(PrimitiveBaseShape newShape) - { - } - - private void changeAddPhysRep(ODEPhysRepData repData) - { - _size = repData.size; //?? - _pbs = repData.pbs; - m_shapetype = repData.shapetype; - - m_mesh = repData.mesh; - - m_assetID = repData.assetID; - m_meshState = repData.meshState; - - m_hasOBB = repData.hasOBB; - m_OBBOffset = repData.OBBOffset; - m_OBB = repData.OBB; - - primVolume = repData.volume; - - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - if (!m_isphysical) - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - else - MakeBody(); - - if ((m_meshState & MeshState.NeedMask) != 0) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMesh(repData); - } - } - - private void changePhysRepData(ODEPhysRepData repData) - { - CheckDelaySelect(); - - OdePrim parent = (OdePrim)_parent; - - bool chp = childPrim; - - if (chp) - { - if (parent != null) - { - parent.DestroyBody(); - } - } - else - { - DestroyBody(); - } - - RemoveGeom(); - - _size = repData.size; - _pbs = repData.pbs; - m_shapetype = repData.shapetype; - - m_mesh = repData.mesh; - - m_assetID = repData.assetID; - m_meshState = repData.meshState; - - m_hasOBB = repData.hasOBB; - m_OBBOffset = repData.OBBOffset; - m_OBB = repData.OBB; - - primVolume = repData.volume; - - CreateGeom(); - - if (prim_geom != IntPtr.Zero) - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.X = _orientation.X; - myrot.Y = _orientation.Y; - myrot.Z = _orientation.Z; - myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - - if (m_isphysical) - { - if (chp) - { - if (parent != null) - { - parent.MakeBody(); - } - } - else - MakeBody(); - } - else - { - SetInStaticSpace(this); - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - - resetCollisionAccounting(); - - if ((m_meshState & MeshState.NeedMask) != 0) - { - repData.size = _size; - repData.pbs = _pbs; - repData.shapetype = m_shapetype; - _parent_scene.m_meshWorker.RequestMesh(repData); - } - } - - private void changeFloatOnWater(bool newval) - { - m_collidesWater = newval; - - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - - private void changeSetTorque(Vector3 newtorque) - { - if (!m_isSelected) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - } - m_torque = newtorque; - } - } - - private void changeForce(Vector3 force) - { - m_force = force; - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - - private void changeAddForce(Vector3 theforce) - { - m_forceacc += theforce; - if (!m_isSelected) - { - lock (this) - { - //m_log.Info("[PHYSICS]: dequeing forcelist"); - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } - } - - // actually angular impulse - private void changeAddAngularImpulse(Vector3 aimpulse) - { - m_angularForceacc += aimpulse * m_invTimeStep; - if (!m_isSelected) - { - lock (this) - { - if (m_isphysical && Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - } - } - m_collisionscore = 0; - } - } - - private void changevelocity(Vector3 newVel) - { - float len = newVel.LengthSquared(); - if (len > 100000.0f) // limit to 100m/s - { - len = 100.0f / (float)Math.Sqrt(len); - newVel *= len; - } - - if (!m_isSelected) - { - if (Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); - } - //resetCollisionAccounting(); - } - _velocity = newVel; - } - - private void changeangvelocity(Vector3 newAngVel) - { - float len = newAngVel.LengthSquared(); - if (len > 144.0f) // limit to 12rad/s - { - len = 12.0f / (float)Math.Sqrt(len); - newAngVel *= len; - } - - if (!m_isSelected) - { - if (Body != IntPtr.Zero) - { - if (m_disabled) - enableBodySoft(); - else if (!d.BodyIsEnabled(Body)) - d.BodyEnable(Body); - - - d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); - } - //resetCollisionAccounting(); - } - m_rotationalVelocity = newAngVel; - } - - private void changeVolumedetetion(bool newVolDtc) - { - m_isVolumeDetect = newVolDtc; - m_fakeisVolumeDetect = newVolDtc; - UpdateCollisionCatFlags(); - ApplyCollisionCatFlags(); - } - - protected void changeBuilding(bool newbuilding) - { - // Check if we need to do anything - if (newbuilding == m_building) - return; - - if ((bool)newbuilding) - { - m_building = true; - if (!childPrim) - DestroyBody(); - } - else - { - m_building = false; - CheckDelaySelect(); - if (!childPrim) - MakeBody(); - } - if (!childPrim && childrenPrim.Count > 0) - { - foreach (OdePrim prm in childrenPrim) - prm.changeBuilding(m_building); // call directly - } - } - - public void changeSetVehicle(VehicleData vdata) - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - m_vehicle.DoSetVehicle(vdata); - } - - private void changeVehicleType(int value) - { - if (value == (int)Vehicle.TYPE_NONE) - { - if (m_vehicle != null) - m_vehicle = null; - } - else - { - if (m_vehicle == null) - m_vehicle = new ODEDynamics(this); - - m_vehicle.ProcessTypeChange((Vehicle)value); - } - } - - private void changeVehicleFloatParam(strVehicleFloatParam fp) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); - } - - private void changeVehicleVectorParam(strVehicleVectorParam vp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); - } - - private void changeVehicleRotationParam(strVehicleQuatParam qp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); - } - - private void changeVehicleFlags(strVehicleBoolParam bp) - { - if (m_vehicle == null) - return; - m_vehicle.ProcessVehicleFlags(bp.param, bp.value); - } - - private void changeBuoyancy(float b) - { - m_buoyancy = b; - } - - private void changePIDTarget(Vector3 trg) - { - m_PIDTarget = trg; - } - - private void changePIDTau(float tau) - { - m_PIDTau = tau; - } - - private void changePIDActive(bool val) - { - m_usePID = val; - } - - private void changePIDHoverHeight(float val) - { - m_PIDHoverHeight = val; - if (val == 0) - m_useHoverPID = false; - } - - private void changePIDHoverType(PIDHoverType type) - { - m_PIDHoverType = type; - } - - private void changePIDHoverTau(float tau) - { - m_PIDHoverTau = tau; - } - - private void changePIDHoverActive(bool active) - { - m_useHoverPID = active; - } - - #endregion - - public void Move() - { - if (!childPrim && m_isphysical && Body != IntPtr.Zero && - !m_disabled && !m_isSelected && !m_building && !m_outbounds) - { - if (!d.BodyIsEnabled(Body)) - { - // let vehicles sleep - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - return; - - if (++bodydisablecontrol < 20) - return; - - d.BodyEnable(Body); - } - - bodydisablecontrol = 0; - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator - - if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(); - return; - } - - float fx = 0; - float fy = 0; - float fz = 0; - - float m_mass = _mass; - - if (m_usePID && m_PIDTau > 0) - { - // for now position error - _target_velocity = - new Vector3( - (m_PIDTarget.X - lpos.X), - (m_PIDTarget.Y - lpos.Y), - (m_PIDTarget.Z - lpos.Z) - ); - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) - { - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - return; - } - else - { - _zeroFlag = false; - - float tmp = 1 / m_PIDTau; - _target_velocity *= tmp; - - // apply limits - tmp = _target_velocity.Length(); - if (tmp > 50.0f) - { - tmp = 50 / tmp; - _target_velocity *= tmp; - } - else if (tmp < 0.05f) - { - tmp = 0.05f / tmp; - _target_velocity *= tmp; - } - - d.Vector3 vel = d.BodyGetLinearVel(Body); - fx = (_target_velocity.X - vel.X) * m_invTimeStep; - fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; - fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; -// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) - { - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); - - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - - case PIDHoverType.GroundAndWater: - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - else - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - break; - } // end switch (m_PIDHoverType) - - // don't go underground unless volumedetector - - if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) - { - d.Vector3 vel = d.BodyGetLinearVel(Body); - - fz = (m_targetHoverHeight - lpos.Z); - - // if error is zero, use position control; otherwise, velocity control - if (Math.Abs(fz) < 0.01f) - { - d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - } - else - { - _zeroFlag = false; - fz /= m_PIDHoverTau; - - float tmp = Math.Abs(fz); - if (tmp > 50) - fz = 50 * Math.Sign(fz); - else if (tmp < 0.1) - fz = 0.1f * Math.Sign(fz); - - fz = ((fz - vel.Z) * m_invTimeStep); - } - } - } - else - { - float b = (1.0f - m_buoyancy) * m_gravmod; - fx = _parent_scene.gravityx * b; - fy = _parent_scene.gravityy * b; - fz = _parent_scene.gravityz * b; - } - - fx *= m_mass; - fy *= m_mass; - fz *= m_mass; - - // constant force - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - fx += m_forceacc.X; - fy += m_forceacc.Y; - fz += m_forceacc.Z; - - m_forceacc = Vector3.Zero; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - d.BodyAddForce(Body, fx, fy, fz); - //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - - Vector3 trq; - - trq = m_torque; - trq += m_angularForceacc; - m_angularForceacc = Vector3.Zero; - if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) - { - d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); - } - } - else - { // is not physical, or is not a body or is selected - // _zeroPosition = d.BodyGetPosition(Body); - return; - //Console.WriteLine("Nothing " + Name); - - } - } - - public void UpdatePositionAndVelocity(int frame) - { - if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) - { - bool bodyenabled = d.BodyIsEnabled(Body); - if (bodyenabled || !_zeroFlag) - { - bool lastZeroFlag = _zeroFlag; - - d.Vector3 lpos = d.GeomGetPosition(prim_geom); - - // check outside region - if (lpos.Z < -100 || lpos.Z > 100000f) - { - m_outbounds = true; - - lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); - _acceleration.X = 0; - _acceleration.Y = 0; - _acceleration.Z = 0; - - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); // stop it - d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere - m_lastposition = _position; - m_lastorientation = _orientation; - - base.RequestPhysicsterseUpdate(); - -// throttleCounter = 0; - _zeroFlag = true; - - disableBodySoft(); // disable it and colisions - base.RaiseOutOfBounds(_position); - return; - } - - if (lpos.X < 0f) - { - _position.X = Util.Clip(lpos.X, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.X > _parent_scene.WorldExtents.X) - { - _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); - m_outbounds = true; - } - if (lpos.Y < 0f) - { - _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); - m_outbounds = true; - } - else if (lpos.Y > _parent_scene.WorldExtents.Y) - { - _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); - m_outbounds = true; - } - - if (m_outbounds) - { - m_lastposition = _position; - m_lastorientation = _orientation; - - d.Vector3 dtmp = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = dtmp.X; - m_rotationalVelocity.Y = dtmp.Y; - m_rotationalVelocity.Z = dtmp.Z; - - dtmp = d.BodyGetLinearVel(Body); - _velocity.X = dtmp.X; - _velocity.Y = dtmp.Y; - _velocity.Z = dtmp.Z; - - d.BodySetLinearVel(Body, 0, 0, 0); // stop it - d.BodySetAngularVel(Body, 0, 0, 0); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - disableBodySoft(); // stop collisions - UnSubscribeEvents(); - - base.RequestPhysicsterseUpdate(); - return; - } - - d.Quaternion ori; - d.GeomCopyQuaternion(prim_geom, out ori); - - // decide if moving - // use positions since this are integrated quantities - // tolerance values depende a lot on simulation noise... - // use simple math.abs since we dont need to be exact - - if (!bodyenabled || - (Math.Abs(_position.X - lpos.X) < 0.005f) - && (Math.Abs(_position.Y - lpos.Y) < 0.005f) - && (Math.Abs(_position.Z - lpos.Z) < 0.005f) - && (Math.Abs(_orientation.X - ori.X) < 0.0005f) - && (Math.Abs(_orientation.Y - ori.Y) < 0.0005f) - && (Math.Abs(_orientation.Z - ori.Z) < 0.0005f) // ignore W - ) - { - _zeroFlag = true; - } - else - _zeroFlag = false; - - // update velocities and aceleration - if (!(_zeroFlag && lastZeroFlag)) - { - d.Vector3 vel = d.BodyGetLinearVel(Body); - - _acceleration = _velocity; - - if ((Math.Abs(vel.X) < 0.005f) && - (Math.Abs(vel.Y) < 0.005f) && - (Math.Abs(vel.Z) < 0.005f)) - { - _velocity = Vector3.Zero; - float t = -m_invTimeStep; - _acceleration = _acceleration * t; - } - else - { - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - _acceleration = (_velocity - _acceleration) * m_invTimeStep; - } - - if ((Math.Abs(_acceleration.X) < 0.01f) && - (Math.Abs(_acceleration.Y) < 0.01f) && - (Math.Abs(_acceleration.Z) < 0.01f)) - { - _acceleration = Vector3.Zero; - } - - if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && - (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && - (Math.Abs(_orientation.Z - ori.Z) < 0.0001) - ) - { - m_rotationalVelocity = Vector3.Zero; - } - else - { - vel = d.BodyGetAngularVel(Body); - m_rotationalVelocity.X = vel.X; - m_rotationalVelocity.Y = vel.Y; - m_rotationalVelocity.Z = vel.Z; - } - // } - - _position.X = lpos.X; - _position.Y = lpos.Y; - _position.Z = lpos.Z; - - _orientation.X = ori.X; - _orientation.Y = ori.Y; - _orientation.Z = ori.Z; - _orientation.W = ori.W; - } - if (_zeroFlag) - { - if (lastZeroFlag) - { - _velocity = Vector3.Zero; - _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; - } - - if (!m_lastUpdateSent) - { - base.RequestPhysicsterseUpdate(); - if (lastZeroFlag) - m_lastUpdateSent = true; - } - return; - } - - base.RequestPhysicsterseUpdate(); - m_lastUpdateSent = false; - } - } - } - - internal static bool QuaternionIsFinite(Quaternion q) - { - if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) - return false; - if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) - return false; - if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) - return false; - if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) - return false; - return true; - } - - internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) - { - // assumes object center of mass is zero - float smass = part.mass; - theobj.mass -= smass; - - smass *= 1.0f / (theobj.mass); ; - - theobj.c.X -= part.c.X * smass; - theobj.c.Y -= part.c.Y * smass; - theobj.c.Z -= part.c.Z * smass; - - theobj.I.M00 -= part.I.M00; - theobj.I.M01 -= part.I.M01; - theobj.I.M02 -= part.I.M02; - theobj.I.M10 -= part.I.M10; - theobj.I.M11 -= part.I.M11; - theobj.I.M12 -= part.I.M12; - theobj.I.M20 -= part.I.M20; - theobj.I.M21 -= part.I.M21; - theobj.I.M22 -= part.I.M22; - } - - private void donullchange() - { - } - - public bool DoAChange(changes what, object arg) - { - if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) - { - return false; - } - - // nasty switch - switch (what) - { - case changes.Add: - changeadd(); - break; - - case changes.AddPhysRep: - changeAddPhysRep((ODEPhysRepData)arg); - break; - - case changes.Remove: - //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... - //When we return true, it destroys all of the prims in the linkset anyway - if (_parent != null) - { - OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this, false); - } - else - ChildRemove(this, false); - - m_vehicle = null; - RemoveGeom(); - m_targetSpace = IntPtr.Zero; - UnSubscribeEvents(); - return true; - - case changes.Link: - OdePrim tmp = (OdePrim)arg; - changeLink(tmp); - break; - - case changes.DeLink: - changeLink(null); - break; - - case changes.Position: - changePosition((Vector3)arg); - break; - - case changes.Orientation: - changeOrientation((Quaternion)arg); - break; - - case changes.PosOffset: - donullchange(); - break; - - case changes.OriOffset: - donullchange(); - break; - - case changes.Velocity: - changevelocity((Vector3)arg); - break; - -// case changes.Acceleration: -// changeacceleration((Vector3)arg); -// break; - - case changes.AngVelocity: - changeangvelocity((Vector3)arg); - break; - - case changes.Force: - changeForce((Vector3)arg); - break; - - case changes.Torque: - changeSetTorque((Vector3)arg); - break; - - case changes.AddForce: - changeAddForce((Vector3)arg); - break; - - case changes.AddAngForce: - changeAddAngularImpulse((Vector3)arg); - break; - - case changes.AngLock: - changeAngularLock((Vector3)arg); - break; - - case changes.Size: - changeSize((Vector3)arg); - break; - - case changes.Shape: - changeShape((PrimitiveBaseShape)arg); - break; - - case changes.PhysRepData: - changePhysRepData((ODEPhysRepData) arg); - break; - - case changes.CollidesWater: - changeFloatOnWater((bool)arg); - break; - - case changes.VolumeDtc: - changeVolumedetetion((bool)arg); - break; - - case changes.Phantom: - changePhantomStatus((bool)arg); - break; - - case changes.Physical: - changePhysicsStatus((bool)arg); - break; - - case changes.Selected: - changeSelectedStatus((bool)arg); - break; - - case changes.disabled: - changeDisable((bool)arg); - break; - - case changes.building: - changeBuilding((bool)arg); - break; - - case changes.VehicleType: - changeVehicleType((int)arg); - break; - - case changes.VehicleFlags: - changeVehicleFlags((strVehicleBoolParam) arg); - break; - - case changes.VehicleFloatParam: - changeVehicleFloatParam((strVehicleFloatParam) arg); - break; - - case changes.VehicleVectorParam: - changeVehicleVectorParam((strVehicleVectorParam) arg); - break; - - case changes.VehicleRotationParam: - changeVehicleRotationParam((strVehicleQuatParam) arg); - break; - - case changes.SetVehicle: - changeSetVehicle((VehicleData) arg); - break; - - case changes.Buoyancy: - changeBuoyancy((float)arg); - break; - - case changes.PIDTarget: - changePIDTarget((Vector3)arg); - break; - - case changes.PIDTau: - changePIDTau((float)arg); - break; - - case changes.PIDActive: - changePIDActive((bool)arg); - break; - - case changes.PIDHoverHeight: - changePIDHoverHeight((float)arg); - break; - - case changes.PIDHoverType: - changePIDHoverType((PIDHoverType)arg); - break; - - case changes.PIDHoverTau: - changePIDHoverTau((float)arg); - break; - - case changes.PIDHoverActive: - changePIDHoverActive((bool)arg); - break; - - case changes.Null: - donullchange(); - break; - - - - default: - donullchange(); - break; - } - return false; - } - - public void AddChange(changes what, object arg) - { - _parent_scene.AddChange((PhysicsActor) this, what, arg); - } - - - private struct strVehicleBoolParam - { - public int param; - public bool value; - } - - private struct strVehicleFloatParam - { - public int param; - public float value; - } - - private struct strVehicleQuatParam - { - public int param; - public Quaternion value; - } - - private struct strVehicleVectorParam - { - public int param; - public Vector3 value; - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs deleted file mode 100644 index 6b3672a..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs +++ /dev/null @@ -1,683 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Text; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; -using OdeAPI; -using log4net; -using OpenMetaverse; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - /// - /// Processes raycast requests as ODE is in a state to be able to do them. - /// This ensures that it's thread safe and there will be no conflicts. - /// Requests get returned by a different thread then they were requested by. - /// - public class ODERayCastRequestManager - { - /// - /// Pending ray requests - /// - protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); - - /// - /// Scene that created this object. - /// - private ODEScene m_scene; - - IntPtr ray; // the ray. we only need one for our lifetime - IntPtr Sphere; - IntPtr Box; - IntPtr Plane; - - private int CollisionContactGeomsPerTest = 25; - private const int DefaultMaxCount = 25; - private const int MaxTimePerCallMS = 30; - - /// - /// ODE near callback delegate - /// - private d.NearCallback nearCallback; - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - private List m_contactResults = new List(); - private RayFilterFlags CurrentRayFilter; - private int CurrentMaxCount; - - public ODERayCastRequestManager(ODEScene pScene) - { - m_scene = pScene; - nearCallback = near; - ray = d.CreateRay(IntPtr.Zero, 1.0f); - d.GeomSetCategoryBits(ray, 0); - Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); - d.GeomSetCategoryBits(Box, 0); - Sphere = d.CreateSphere(IntPtr.Zero,1.0f); - d.GeomSetCategoryBits(Sphere, 0); - Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); - d.GeomSetCategoryBits(Sphere, 0); - } - - public void QueueRequest(ODERayRequest req) - { - if (req.Count == 0) - req.Count = DefaultMaxCount; - - m_PendingRequests.Enqueue(req); - } - - /// - /// Process all queued raycast requests - /// - /// Time in MS the raycasts took to process. - public int ProcessQueuedRequests() - { - - if (m_PendingRequests.Count <= 0) - return 0; - - if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) - // oops something got wrong or scene isn't ready still - { - m_PendingRequests.Clear(); - return 0; - } - - int time = Util.EnvironmentTickCount(); - - ODERayRequest req; - int closestHit; - int backfacecull; - CollisionCategories catflags; - - while (m_PendingRequests.Dequeue(out req)) - { - if (req.callbackMethod != null) - { - IntPtr geom = IntPtr.Zero; - if (req.actor != null) - { - if (m_scene.haveActor(req.actor)) - { - if (req.actor is OdePrim) - geom = ((OdePrim)req.actor).prim_geom; - else if (req.actor is OdeCharacter) - geom = ((OdePrim)req.actor).prim_geom; - } - if (geom == IntPtr.Zero) - { - NoContacts(req); - continue; - } - } - - - CurrentRayFilter = req.filter; - CurrentMaxCount = req.Count; - - CollisionContactGeomsPerTest = req.Count & 0xffff; - - closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); - backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); - - if (req.callbackMethod is ProbeBoxCallback) - { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); - d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); - d.Quaternion qtmp; - qtmp.X = req.orientation.X; - qtmp.Y = req.orientation.Y; - qtmp.Z = req.orientation.Z; - qtmp.W = req.orientation.W; - d.GeomSetQuaternion(Box, ref qtmp); - } - else if (req.callbackMethod is ProbeSphereCallback) - { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - - d.GeomSphereSetRadius(Sphere, req.length); - d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); - } - else if (req.callbackMethod is ProbePlaneCallback) - { - if (CollisionContactGeomsPerTest > 80) - CollisionContactGeomsPerTest = 80; - - d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); - } - - else - { - if (CollisionContactGeomsPerTest > 25) - CollisionContactGeomsPerTest = 25; - - d.GeomRaySetLength(ray, req.length); - d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); - d.GeomRaySetParams(ray, 0, backfacecull); - d.GeomRaySetClosestHit(ray, closestHit); - - if (req.callbackMethod is RaycastCallback) - { - // if we only want one get only one per Collision pair saving memory - CurrentRayFilter |= RayFilterFlags.ClosestHit; - d.GeomRaySetClosestHit(ray, 1); - } - else - d.GeomRaySetClosestHit(ray, closestHit); - } - - if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) - unchecked - { - CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; - } - - if (geom == IntPtr.Zero) - { - // translate ray filter to Collision flags - catflags = 0; - if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) - catflags |= CollisionCategories.VolumeDtc; - if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) - catflags |= CollisionCategories.Phantom; - if ((CurrentRayFilter & RayFilterFlags.agent) != 0) - catflags |= CollisionCategories.Character; - if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0) - catflags |= CollisionCategories.Geom; - if ((CurrentRayFilter & RayFilterFlags.land) != 0) - catflags |= CollisionCategories.Land; - if ((CurrentRayFilter & RayFilterFlags.water) != 0) - catflags |= CollisionCategories.Water; - - if (catflags != 0) - { - if (req.callbackMethod is ProbeBoxCallback) - { - catflags |= CollisionCategories.Space; - d.GeomSetCollideBits(Box, (uint)catflags); - d.GeomSetCategoryBits(Box, (uint)catflags); - doProbe(req, Box); - } - else if (req.callbackMethod is ProbeSphereCallback) - { - catflags |= CollisionCategories.Space; - d.GeomSetCollideBits(Sphere, (uint)catflags); - d.GeomSetCategoryBits(Sphere, (uint)catflags); - doProbe(req, Sphere); - } - else if (req.callbackMethod is ProbePlaneCallback) - { - catflags |= CollisionCategories.Space; - d.GeomSetCollideBits(Plane, (uint)catflags); - d.GeomSetCategoryBits(Plane, (uint)catflags); - doPlane(req,IntPtr.Zero); - } - else - { - d.GeomSetCollideBits(ray, (uint)catflags); - doSpaceRay(req); - } - } - } - else - { - // if we select a geom don't use filters - - if (req.callbackMethod is ProbePlaneCallback) - { - d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); - doPlane(req,geom); - } - else - { - d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); - doGeomRay(req,geom); - } - } - } - - if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) - break; - } - - lock (m_contactResults) - m_contactResults.Clear(); - - return Util.EnvironmentTickCountSubtract(time); - } - /// - /// Method that actually initiates the raycast with spaces - /// - /// - /// - - private void NoContacts(ODERayRequest req) - { - if (req.callbackMethod is RaycastCallback) - { - ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); - return; - } - List cresult = new List(); - - if (req.callbackMethod is RayCallback) - ((RayCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeBoxCallback) - ((ProbeBoxCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeSphereCallback) - ((ProbeSphereCallback)req.callbackMethod)(cresult); - } - - private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom; -// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; - private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; - - private void doSpaceRay(ODERayRequest req) - { - // Collide tests - if ((CurrentRayFilter & FilterActiveSpace) != 0) - { - d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); - } - if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) - d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); - if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) - { - // current ode land to ray collisions is very bad - // so for now limit its range badly - - if (req.length > 30.0f) - d.GeomRaySetLength(ray, 30.0f); - - d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); - } - - if (req.callbackMethod is RaycastCallback) - { - // Define default results - bool hitYN = false; - uint hitConsumerID = 0; - float distance = float.MaxValue; - Vector3 closestcontact = Vector3.Zero; - Vector3 snormal = Vector3.Zero; - - // Find closest contact and object. - lock (m_contactResults) - { - foreach (ContactResult cResult in m_contactResults) - { - if(cResult.Depth < distance) - { - closestcontact = cResult.Pos; - hitConsumerID = cResult.ConsumerID; - distance = cResult.Depth; - snormal = cResult.Normal; - } - } - m_contactResults.Clear(); - } - - if (distance > 0 && distance < float.MaxValue) - hitYN = true; - ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); - } - else - { - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - ((RayCallback)req.callbackMethod)(cresult); - } - } - - private void doProbe(ODERayRequest req, IntPtr probe) - { - // Collide tests - if ((CurrentRayFilter & FilterActiveSpace) != 0) - { - d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); - } - if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) - d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); - if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) - d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); - - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - if (req.callbackMethod is ProbeBoxCallback) - ((ProbeBoxCallback)req.callbackMethod)(cresult); - else if (req.callbackMethod is ProbeSphereCallback) - ((ProbeSphereCallback)req.callbackMethod)(cresult); - } - - private void doPlane(ODERayRequest req,IntPtr geom) - { - // Collide tests - if (geom == IntPtr.Zero) - { - if ((CurrentRayFilter & FilterActiveSpace) != 0) - { - d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); - d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); - } - if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) - d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); - if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) - d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); - } - else - { - d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); - } - - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - - ((ProbePlaneCallback)req.callbackMethod)(cresult); - } - - /// - /// Method that actually initiates the raycast with a geom - /// - /// - private void doGeomRay(ODERayRequest req, IntPtr geom) - { - // Collide test - d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test - - if (req.callbackMethod is RaycastCallback) - { - // Define default results - bool hitYN = false; - uint hitConsumerID = 0; - float distance = float.MaxValue; - Vector3 closestcontact = Vector3.Zero; - Vector3 snormal = Vector3.Zero; - - // Find closest contact and object. - lock (m_contactResults) - { - foreach (ContactResult cResult in m_contactResults) - { - if(cResult.Depth < distance ) - { - closestcontact = cResult.Pos; - hitConsumerID = cResult.ConsumerID; - distance = cResult.Depth; - snormal = cResult.Normal; - } - } - m_contactResults.Clear(); - } - - if (distance > 0 && distance < float.MaxValue) - hitYN = true; - - ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); - } - else - { - List cresult = new List(m_contactResults.Count); - lock (m_PendingRequests) - { - cresult.AddRange(m_contactResults); - m_contactResults.Clear(); - } - ((RayCallback)req.callbackMethod)(cresult); - } - } - - private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) - { - IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; - if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) - return false; - - IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); - newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); - return true; - } - - // This is the standard Near. g1 is the ray - private void near(IntPtr space, IntPtr g1, IntPtr g2) - { - if (g2 == IntPtr.Zero || g1 == g2) - return; - - if (m_contactResults.Count >= CurrentMaxCount) - return; - - if (d.GeomIsSpace(g2)) - { - try - { - d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); - } - return; - } - - int count = 0; - try - { - count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); - return; - } - - if (count == 0) - return; -/* - uint cat1 = d.GeomGetCategoryBits(g1); - uint cat2 = d.GeomGetCategoryBits(g2); - uint col1 = d.GeomGetCollideBits(g1); - uint col2 = d.GeomGetCollideBits(g2); -*/ - - uint ID = 0; - PhysicsActor p2 = null; - - m_scene.actor_name_map.TryGetValue(g2, out p2); - - if (p2 == null) - return; - - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Prim: - - RayFilterFlags thisFlags; - - if (p2.IsPhysical) - thisFlags = RayFilterFlags.physical; - else - thisFlags = RayFilterFlags.nonphysical; - - if (p2.Phantom) - thisFlags |= RayFilterFlags.phantom; - - if (p2.IsVolumeDtc) - thisFlags |= RayFilterFlags.volumedtc; - - if ((thisFlags & CurrentRayFilter) == 0) - return; - - ID = ((OdePrim)p2).LocalID; - break; - - case (int)ActorTypes.Agent: - - if ((CurrentRayFilter & RayFilterFlags.agent) == 0) - return; - else - ID = ((OdeCharacter)p2).LocalID; - break; - - case (int)ActorTypes.Ground: - - if ((CurrentRayFilter & RayFilterFlags.land) == 0) - return; - break; - - case (int)ActorTypes.Water: - - if ((CurrentRayFilter & RayFilterFlags.water) == 0) - return; - break; - - default: - break; - } - - d.ContactGeom curcontact = new d.ContactGeom(); - - // closestHit for now only works for meshs, so must do it for others - if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) - { - // Loop all contacts, build results. - for (int i = 0; i < count; i++) - { - if (!GetCurContactGeom(i, ref curcontact)) - break; - - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; - lock (m_contactResults) - { - m_contactResults.Add(collisionresult); - if (m_contactResults.Count >= CurrentMaxCount) - return; - } - } - } - else - { - // keep only closest contact - ContactResult collisionresult = new ContactResult(); - collisionresult.ConsumerID = ID; - collisionresult.Depth = float.MaxValue; - - for (int i = 0; i < count; i++) - { - if (!GetCurContactGeom(i, ref curcontact)) - break; - - if (curcontact.depth < collisionresult.Depth) - { - collisionresult.Pos.X = curcontact.pos.X; - collisionresult.Pos.Y = curcontact.pos.Y; - collisionresult.Pos.Z = curcontact.pos.Z; - collisionresult.Depth = curcontact.depth; - collisionresult.Normal.X = curcontact.normal.X; - collisionresult.Normal.Y = curcontact.normal.Y; - collisionresult.Normal.Z = curcontact.normal.Z; - } - } - - if (collisionresult.Depth != float.MaxValue) - { - lock (m_contactResults) - m_contactResults.Add(collisionresult); - } - } - } - - /// - /// Dereference the creator scene so that it can be garbage collected if needed. - /// - internal void Dispose() - { - m_scene = null; - if (ray != IntPtr.Zero) - { - d.GeomDestroy(ray); - ray = IntPtr.Zero; - } - if (Box != IntPtr.Zero) - { - d.GeomDestroy(Box); - Box = IntPtr.Zero; - } - if (Sphere != IntPtr.Zero) - { - d.GeomDestroy(Sphere); - Sphere = IntPtr.Zero; - } - if (Plane != IntPtr.Zero) - { - d.GeomDestroy(Plane); - Plane = IntPtr.Zero; - } - } - } - - public struct ODERayRequest - { - public PhysicsActor actor; - public Vector3 Origin; - public Vector3 Normal; - public int Count; - public float length; - public object callbackMethod; - public RayFilterFlags filter; - public Quaternion orientation; - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs deleted file mode 100644 index ef828de..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs +++ /dev/null @@ -1,356 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ -// Ubit 2012 -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Text; -using OpenSim.Framework; -using OpenSim.Region.PhysicsModules.SharedBase; -using OdeAPI; -using log4net; -using OpenMetaverse; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - /// - /// - public class ODESitAvatar - { - private ODEScene m_scene; - private ODERayCastRequestManager m_raymanager; - - public ODESitAvatar(ODEScene pScene, ODERayCastRequestManager raymanager) - { - m_scene = pScene; - m_raymanager = raymanager; - } - - private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); - private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; - - private void RotAroundZ(float x, float y, ref Quaternion ori) - { - double ang = Math.Atan2(y, x); - ang *= 0.5d; - float s = (float)Math.Sin(ang); - float c = (float)Math.Cos(ang); - - ori.X = 0; - ori.Y = 0; - ori.Z = s; - ori.W = c; - } - - - public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) - { - if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) - { - PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); - return; - } - - IntPtr geom = ((OdePrim)actor).prim_geom; - - Vector3 geopos = d.GeomGetPositionOMV(geom); - Quaternion geomOri = d.GeomGetQuaternionOMV(geom); - -// Vector3 geopos = actor.Position; -// Quaternion geomOri = actor.Orientation; - - Quaternion geomInvOri = Quaternion.Conjugate(geomOri); - - Quaternion ori = Quaternion.Identity; - - Vector3 rayDir = geopos + offset - avCameraPosition; - - float raylen = rayDir.Length(); - if (raylen < 0.001f) - { - PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); - return; - } - float t = 1 / raylen; - rayDir.X *= t; - rayDir.Y *= t; - rayDir.Z *= t; - - raylen += 30f; // focal point may be far - List rayResults; - - rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); - if (rayResults.Count == 0) - { -/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim - d.AABB aabb; - d.GeomGetAABB(geom, out aabb); - offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); - ori = geomInvOri; - offset *= geomInvOri; - PhysicsSitResponse(1, actor.LocalID, offset, ori); -*/ - PhysicsSitResponse(0, actor.LocalID, offset, ori); - return; - } - - int status = 1; - - offset = rayResults[0].Pos - geopos; - - d.GeomClassID geoclass = d.GeomGetClass(geom); - - if (geoclass == d.GeomClassID.SphereClass) - { - float r = d.GeomSphereGetRadius(geom); - - offset.Normalize(); - offset *= r; - - RotAroundZ(offset.X, offset.Y, ref ori); - - if (r < 0.4f) - { - offset = new Vector3(0, 0, r); - } - else - { - if (offset.Z < 0.4f) - { - t = offset.Z; - float rsq = r * r; - - t = 1.0f / (rsq - t * t); - offset.X *= t; - offset.Y *= t; - offset.Z = 0.4f; - t = rsq - 0.16f; - offset.X *= t; - offset.Y *= t; - } - else if (r > 0.8f && offset.Z > 0.8f * r) - { - status = 3; - avOffset.X = -avOffset.X; - avOffset.Z *= 1.6f; - } - } - - offset += avOffset * ori; - - ori = geomInvOri * ori; - offset *= geomInvOri; - - PhysicsSitResponse(status, actor.LocalID, offset, ori); - return; - } - - Vector3 norm = rayResults[0].Normal; - - if (norm.Z < -0.4f) - { - PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); - return; - } - - - float SitNormX = -rayDir.X; - float SitNormY = -rayDir.Y; - - Vector3 pivot = geopos + offset; - - float edgeNormalX = norm.X; - float edgeNormalY = norm.Y; - float edgeDirX = -rayDir.X; - float edgeDirY = -rayDir.Y; - Vector3 edgePos = rayResults[0].Pos; - float edgeDist = float.MaxValue; - - bool foundEdge = false; - - if (norm.Z < 0.5f) - { - float rayDist = 4.0f; - - for (int i = 0; i < 6; i++) - { - pivot.X -= 0.01f * norm.X; - pivot.Y -= 0.01f * norm.Y; - pivot.Z -= 0.01f * norm.Z; - - rayDir.X = -norm.X * norm.Z; - rayDir.Y = -norm.Y * norm.Z; - rayDir.Z = 1.0f - norm.Z * norm.Z; - rayDir.Normalize(); - - rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); - if (rayResults.Count == 0) - break; - - if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) - { - rayDist -= rayResults[0].Depth; - if (rayDist < 0f) - break; - - pivot = rayResults[0].Pos; - norm = rayResults[0].Normal; - edgeNormalX = norm.X; - edgeNormalY = norm.Y; - edgeDirX = -rayDir.X; - edgeDirY = -rayDir.Y; - } - else - { - foundEdge = true; - edgePos = rayResults[0].Pos; - break; - } - } - - if (!foundEdge) - { - PhysicsSitResponse(0, actor.LocalID, offset, ori); - return; - } - avOffset.X *= 0.5f; - } - - else if (norm.Z > 0.866f) - { - float toCamBaseX = avCameraPosition.X - pivot.X; - float toCamBaseY = avCameraPosition.Y - pivot.Y; - float toCamX = toCamBaseX; - float toCamY = toCamBaseY; - - for (int j = 0; j < 4; j++) - { - float rayDist = 1.0f; - float curEdgeDist = 0.0f; - - for (int i = 0; i < 3; i++) - { - pivot.Z -= 0.01f; - rayDir.X = toCamX; - rayDir.Y = toCamY; - rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; - rayDir.Normalize(); - - rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); - if (rayResults.Count == 0) - break; - - curEdgeDist += rayResults[0].Depth; - - if (rayResults[0].Normal.Z > 0.5f) - { - rayDist -= rayResults[0].Depth; - if (rayDist < 0f) - break; - - pivot = rayResults[0].Pos; - norm = rayResults[0].Normal; - } - else - { - foundEdge = true; - if (curEdgeDist < edgeDist) - { - edgeDist = curEdgeDist; - edgeNormalX = rayResults[0].Normal.X; - edgeNormalY = rayResults[0].Normal.Y; - edgeDirX = rayDir.X; - edgeDirY = rayDir.Y; - edgePos = rayResults[0].Pos; - } - break; - } - } - if (foundEdge && edgeDist < 0.2f) - break; - - pivot = geopos + offset; - - switch (j) - { - case 0: - toCamX = -toCamBaseY; - toCamY = toCamBaseX; - break; - case 1: - toCamX = toCamBaseY; - toCamY = -toCamBaseX; - break; - case 2: - toCamX = -toCamBaseX; - toCamY = -toCamBaseY; - break; - default: - break; - } - } - - if (!foundEdge) - { - avOffset.X = -avOffset.X; - avOffset.Z *= 1.6f; - - RotAroundZ(SitNormX, SitNormY, ref ori); - - offset += avOffset * ori; - - ori = geomInvOri * ori; - offset *= geomInvOri; - - PhysicsSitResponse(3, actor.LocalID, offset, ori); - return; - } - avOffset.X *= 0.5f; - } - - SitNormX = edgeNormalX; - SitNormY = edgeNormalY; - if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) - { - SitNormX = -SitNormX; - SitNormY = -SitNormY; - } - - RotAroundZ(SitNormX, SitNormY, ref ori); - - offset = edgePos + avOffset * ori; - offset -= geopos; - - ori = geomInvOri * ori; - offset *= geomInvOri; - - PhysicsSitResponse(1, actor.LocalID, offset, ori); - return; - } - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs b/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs deleted file mode 100644 index 10d7d50..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs +++ /dev/null @@ -1,2025 +0,0 @@ -/* - * based on: - * Ode.NET - .NET bindings for ODE - * Jason Perkins (starkos@industriousone.com) - * Licensed under the New BSD - * Part of the OpenDynamicsEngine -Open Dynamics Engine -Copyright (c) 2001-2007, Russell L. Smith. -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. - -Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the following disclaimer in the documentation -and/or other materials provided with the distribution. - -Neither the names of ODE's copyright owner 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. - * - * changes by opensim team; - * changes by Aurora team http://www.aurora-sim.org/ - - * Revision/fixs by Ubit Umarov - */ - -using System; -using System.Runtime.InteropServices; -using System.Security; -using OMV = OpenMetaverse; -namespace OdeAPI -{ -//#if dDOUBLE -// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim -// at least we save same memory and memory access time, FPU performance on intel usually is similar -// using dReal = System.Double; -//#else - using dReal = System.Single; -//#endif - - public static class d - { - public static dReal Infinity = dReal.MaxValue; - public static int NTotalBodies = 0; - public static int NTotalGeoms = 0; - - public const uint CONTACTS_UNIMPORTANT = 0x80000000; - - #region Flags and Enumerations - - [Flags] - public enum AllocateODEDataFlags : uint - { - BasicData = 0, - CollisionData = 0x00000001, - All = ~0u - } - - [Flags] - public enum IniteODEFlags : uint - { - dInitFlagManualThreadCleanup = 0x00000001 - } - - [Flags] - public enum ContactFlags : int - { - Mu2 = 0x001, - FDir1 = 0x002, - Bounce = 0x004, - SoftERP = 0x008, - SoftCFM = 0x010, - Motion1 = 0x020, - Motion2 = 0x040, - MotionN = 0x080, - Slip1 = 0x100, - Slip2 = 0x200, - Approx0 = 0x0000, - Approx1_1 = 0x1000, - Approx1_2 = 0x2000, - Approx1 = 0x3000 - } - - public enum GeomClassID : int - { - SphereClass, - BoxClass, - CapsuleClass, - CylinderClass, - PlaneClass, - RayClass, - ConvexClass, - GeomTransformClass, - TriMeshClass, - HeightfieldClass, - FirstSpaceClass, - SimpleSpaceClass = FirstSpaceClass, - HashSpaceClass, - QuadTreeSpaceClass, - LastSpaceClass = QuadTreeSpaceClass, - UbitTerrainClass, - FirstUserClass, - LastUserClass = FirstUserClass + MaxUserClasses - 1, - NumClasses, - MaxUserClasses = 5 - } - - public enum JointType : int - { - None, - Ball, - Hinge, - Slider, - Contact, - Universal, - Hinge2, - Fixed, - Null, - AMotor, - LMotor, - Plane2D - } - - public enum JointParam : int - { - LoStop, - HiStop, - Vel, - FMax, - FudgeFactor, - Bounce, - CFM, - StopERP, - StopCFM, - SuspensionERP, - SuspensionCFM, - LoStop2 = 256, - HiStop2, - Vel2, - FMax2, - FudgeFactor2, - Bounce2, - CFM2, - StopERP2, - StopCFM2, - SuspensionERP2, - SuspensionCFM2, - LoStop3 = 512, - HiStop3, - Vel3, - FMax3, - FudgeFactor3, - Bounce3, - CFM3, - StopERP3, - StopCFM3, - SuspensionERP3, - SuspensionCFM3 - } - - public enum dSweepAndPruneAxis : int - { - XYZ = ((0)|(1<<2)|(2<<4)), - XZY = ((0)|(2<<2)|(1<<4)), - YXZ = ((1)|(0<<2)|(2<<4)), - YZX = ((1)|(2<<2)|(0<<4)), - ZXY = ((2)|(0<<2)|(1<<4)), - ZYX = ((2)|(1<<2)|(0<<4)) - } - - #endregion - - #region Callbacks - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate void GetAABBFn(IntPtr geom, out AABB aabb); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate ColliderFn GetColliderFnFn(int num); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate void GeomDtorFn(IntPtr o); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); - - #endregion - - #region Structs - - [StructLayout(LayoutKind.Sequential)] - public struct AABB - { - public dReal MinX, MaxX; - public dReal MinY, MaxY; - public dReal MinZ, MaxZ; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Contact - { - public SurfaceParameters surface; - public ContactGeom geom; - public Vector3 fdir1; - public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); - } - - - [StructLayout(LayoutKind.Sequential)] - public struct ContactGeom - { - - public Vector3 pos; - public Vector3 normal; - public dReal depth; - public IntPtr g1; - public IntPtr g2; - public int side1; - public int side2; - public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); - } - - [StructLayout(LayoutKind.Sequential)] - public struct GeomClass - { - public int bytes; - public GetColliderFnFn collider; - public GetAABBFn aabb; - public AABBTestFn aabb_test; - public GeomDtorFn dtor; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct JointFeedback - { - public Vector3 f1; - public Vector3 t1; - public Vector3 f2; - public Vector3 t2; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Mass - { - public dReal mass; - public Vector4 c; - public Matrix3 I; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Matrix3 - { - public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) - { - M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; - M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; - M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; - } - public dReal M00, M10, M20; - private dReal _m30; - public dReal M01, M11, M21; - private dReal _m31; - public dReal M02, M12, M22; - private dReal _m32; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Matrix4 - { - public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, - dReal m01, dReal m11, dReal m21, dReal m31, - dReal m02, dReal m12, dReal m22, dReal m32, - dReal m03, dReal m13, dReal m23, dReal m33) - { - M00 = m00; M10 = m10; M20 = m20; M30 = m30; - M01 = m01; M11 = m11; M21 = m21; M31 = m31; - M02 = m02; M12 = m12; M22 = m22; M32 = m32; - M03 = m03; M13 = m13; M23 = m23; M33 = m33; - } - public dReal M00, M10, M20, M30; - public dReal M01, M11, M21, M31; - public dReal M02, M12, M22, M32; - public dReal M03, M13, M23, M33; - } - - [StructLayout(LayoutKind.Sequential)] - public struct Quaternion - { - public dReal W, X, Y, Z; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct SurfaceParameters - { - public ContactFlags mode; - public dReal mu; - public dReal mu2; - public dReal bounce; - public dReal bounce_vel; - public dReal soft_erp; - public dReal soft_cfm; - public dReal motion1; - public dReal motion2; - public dReal motionN; - public dReal slip1; - public dReal slip2; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Vector3 - { - public Vector3(dReal x, dReal y, dReal z) - { - X = x; Y = y; Z = z; _w = 0.0f; - } - public dReal X, Y, Z; - private dReal _w; - } - - - [StructLayout(LayoutKind.Sequential)] - public struct Vector4 - { - public Vector4(dReal x, dReal y, dReal z, dReal w) - { - X = x; Y = y; Z = z; W = w; - } - public dReal X, Y, Z, W; - } - - #endregion - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] - public static extern int AllocateODEDataForThread(uint ODEInitFlags); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] - public static extern bool AreConnected(IntPtr b1, IntPtr b2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] - public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] - public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyPosition(IntPtr body, out dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] - public static extern void BodyCopyRotation(IntPtr body, out dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr BodyiCreate(IntPtr world); - public static IntPtr BodyCreate(IntPtr world) - { - NTotalBodies++; - return BodyiCreate(world); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void BodyiDestroy(IntPtr body); - public static void BodyDestroy(IntPtr body) - { - NTotalBodies--; - BodyiDestroy(body); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] - public static extern void BodyDisable(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] - public static extern void BodyEnable(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] - public static extern bool BodyGetAutoDisableFlag(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetAutoDisableDefaults(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] - public static extern int BodyGetAutoDisableSteps(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetAutoDisableTime(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); - public static Vector3 BodyGetAngularVel(IntPtr body) - { - unsafe { return *(BodyGetAngularVelUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr BodyGetData(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] - public static extern int BodyGetFiniteRotationMode(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); - public static Vector3 BodyGetForce(IntPtr body) - { - unsafe { return *(BodyGetForceUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] - public static extern bool BodyGetGravityMode(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] - public static extern int BodyGetGyroscopicMode(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr BodyGetJoint(IntPtr body, int index); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); - public static Vector3 BodyGetLinearVel(IntPtr body) - { - unsafe { return *(BodyGetLinearVelUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetMass(IntPtr body, out Mass mass); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] - public static extern int BodyGetNumJoints(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); - public static Vector3 BodyGetPosition(IntPtr body) - { - unsafe { return *(BodyGetPositionUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); - public static Quaternion BodyGetQuaternion(IntPtr body) - { - unsafe { return *(BodyGetQuaternionUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] - public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); - public static Matrix3 BodyGetRotation(IntPtr body) - { - unsafe { return *(BodyGetRotationUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); - public static Vector3 BodyGetTorque(IntPtr body) - { - unsafe { return *(BodyGetTorqueUnsafe(body)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr BodyGetWorld(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr BodyGetFirstGeom(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] - public static extern bool BodyIsEnabled(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableDefaults(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetData(IntPtr body, IntPtr data); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetLinearDamping(IntPtr body, dReal scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAngularDamping(IntPtr body, dReal scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetLinearDamping(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetAngularDamping(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetGravityMode(IntPtr body, bool mode); - - /// - /// Sets the Gyroscopic term status on the body specified. - /// - /// Pointer to body - /// NonZero enabled, Zero disabled - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] - public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetMass(IntPtr body, ref Mass mass); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetQuaternion(IntPtr body, ref dReal w); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetRotation(IntPtr body, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] - public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] - public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] - public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] - public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, - ref Vector3 side1, ref Vector3 p2, - ref Matrix3 R2, ref Vector3 side2, - ref Vector3 normal, out dReal depth, out int return_code, - int maxc, out ContactGeom contact, int skip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] - public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, - ref Vector3 side1, ref Vector3 _p2, - ref Matrix3 R2, ref Vector3 side2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] - public static extern void CleanupODEAllDataForThread(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] - public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, - ref Vector3 b1, ref Vector3 b2, - ref Vector3 cp1, ref Vector3 cp2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] - public static extern void CloseODE(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] - public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] - public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); - public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) - { - NTotalGeoms++; - return CreateiBox(space, lx, ly, lz); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); - public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) - { - NTotalGeoms++; - return CreateiCapsule(space, radius, length); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); - public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) - { - NTotalGeoms++; - return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); - public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) - { - NTotalGeoms++; - return CreateiCylinder(space, radius, length); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); - public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) - { - NTotalGeoms++; - return CreateiHeightfield(space, data, bPlaceable); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable); - public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable) - { - NTotalGeoms++; - return CreateiUbitTerrain(space, data, bPlaceable); - } - - - - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiGeom(int classnum); - public static IntPtr CreateGeom(int classnum) - { - NTotalGeoms++; - return CreateiGeom(classnum); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] - public static extern int CreateGeomClass(ref GeomClass classptr); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateGeomTransform(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); - public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) - { - NTotalGeoms++; - return CreateiPlane(space, a, b, c, d); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiRay(IntPtr space, dReal length); - public static IntPtr CreateRay(IntPtr space, dReal length) - { - NTotalGeoms++; - return CreateiRay(space, length); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); - public static IntPtr CreateSphere(IntPtr space, dReal radius) - { - NTotalGeoms++; - return CreateiSphere(space, radius); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, - TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); - public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, - TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) - { - NTotalGeoms++; - return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); - } - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] - public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] - public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] - public static extern int FactorCholesky(ref dReal A00, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] - public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] - public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] - public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] - public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] - public static extern void GeomClearOffset(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyPosition(IntPtr geom, out dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void GeomiDestroy(IntPtr geom); - public static void GeomDestroy(IntPtr geom) - { - NTotalGeoms--; - GeomiDestroy(geom); - } - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] - public static extern void GeomDisable(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] - public static extern void GeomEnable(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] - public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] - public static extern void GeomGetAABB(IntPtr geom, out dReal minX); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomGetBody(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] - public static extern uint GeomGetCategoryBits(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomGetClassData(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] - public static extern uint GeomGetCollideBits(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] - public static extern GeomClassID GeomGetClass(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomGetData(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); - public static Vector3 GeomGetOffsetPosition(IntPtr geom) - { - unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); - public static Matrix3 GeomGetOffsetRotation(IntPtr geom) - { - unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); - public static Vector3 GeomGetPosition(IntPtr geom) - { - unsafe { return *(GeomGetPositionUnsafe(geom)); } - } - public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) - { - Vector3 vtmp = GeomGetPosition(geom); - return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); - public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) - { - Quaternion qtmp; - GeomCopyQuaternion(geom, out qtmp); - return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); - public static Matrix3 GeomGetRotation(IntPtr geom) - { - unsafe { return *(GeomGetRotationUnsafe(geom)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomGetSpace(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, - dReal width, dReal depth, int widthSamples, int depthSamples, - dReal scale, dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomHeightfieldDataCreate(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataDestroy(IntPtr d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] - public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] - public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, - dReal sampleSize, int widthSamples, int depthSamples, - dReal offset, dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] - public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, - dReal sampleSize, int widthSamples, int depthSamples, - dReal thickness, int bWrap); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomUbitTerrainDataCreate(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void GeomUbitTerrainDataDestroy(IntPtr d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] - public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] - public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d); - - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] - public static extern bool GeomIsEnabled(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] - public static extern bool GeomIsOffset(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] - public static extern bool GeomIsSpace(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] - public static extern int GeomRayGetClosestHit(IntPtr ray); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomRayGetLength(IntPtr ray); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRaySetLength(IntPtr ray, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] - public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetBody(IntPtr geom, IntPtr body); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetCategoryBits(IntPtr geom, uint bits); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetCollideBits(IntPtr geom, uint bits); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetData(IntPtr geom, IntPtr data); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomSphereGetRadius(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] - public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] - public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] - public static extern int GeomTransformGetCleanup(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomTransformGetGeom(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] - public static extern int GeomTransformGetInfo(IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTransformSetInfo(IntPtr geom, int info); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildDouble(IntPtr d, - double[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildDouble(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, - double[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride, - double[] normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildDouble(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride, - IntPtr normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSingle(IntPtr d, - dReal[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSingle(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, - dReal[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride, - dReal[] normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride, - IntPtr normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSimple(IntPtr d, - float[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSimple(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, - float[] vertices, int vertexStride, int vertexCount, - int[] indices, int indexCount, int triStride, - float[] normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, - IntPtr vertices, int vertexStride, int vertexCount, - IntPtr indices, int indexCount, int triStride, - IntPtr normals); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshClearTCCache(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomTriMeshDataCreate(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataDestroy(IntPtr d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataPreprocess(IntPtr d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshDataUpdate(IntPtr d); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] - public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] - public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomTriMeshGetData(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] - public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); - public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) - { - unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] - public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] - public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] - public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] - public extern static int GeomTriMeshGetTriangleCount(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] - public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] - public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr iGetConfiguration(); - - public static string GetConfiguration() - { - IntPtr ptr = iGetConfiguration(); - string s = Marshal.PtrToStringAnsi(ptr); - return s; - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr HashSpaceCreate(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] - public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] - public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] - public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] - public static extern void InitODE(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] - public static extern int InitODE2(uint ODEInitFlags); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] - public static extern int IsPositiveDefinite(ref dReal A, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] - public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddPRTorque(IntPtr joint, dReal torque); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] - public static extern void JointAddSliderForce(IntPtr joint, dReal force); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] - public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void JointDestroy(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] - public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] - public static extern int JointGetAMotorMode(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] - public static extern int JointGetAMotorNumAxes(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointGetBody(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointGetData(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] - public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); - public static JointFeedback JointGetFeedback(IntPtr j) - { - unsafe { return *(JointGetFeedbackUnsafe(j)); } - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHingeAngle(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHingeAngleRate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHingeParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHinge2Angle1(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] - public static extern int JointGetLMotorNumAxes(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetPRParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetPRPosition(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetPRPositionRate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetSliderParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetSliderPosition(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetSliderPositionRate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] - public static extern JointType JointGetType(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetUniversalAngle1(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetUniversalAngle2(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] - public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr JointGroupCreate(int max_size); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void JointGroupDestroy(IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] - public static extern void JointGroupEmpty(IntPtr group); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetAMotorMode(IntPtr j, int mode); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetAMotorNumAxes(IntPtr group, int num); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetData(IntPtr j, IntPtr data); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetFixed(IntPtr j); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetLMotorNumAxes(IntPtr j, int num); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] - public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] - public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] - public static extern void MassAdd(ref Mass a, ref Mass b); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] - public static extern void MassAdjust(ref Mass m, dReal newmass); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] - public static extern bool MassCheck(ref Mass m); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] - public static extern void MassRotate(ref Mass mass, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] - public static extern void MassRotate(ref Mass mass, ref dReal M00); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetParameters(out Mass mass, dReal themass, - dReal cgx, dReal cgy, dReal cgz, - dReal i11, dReal i22, dReal i33, - dReal i12, dReal i13, dReal i23); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] - public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] - public static extern void MassSetZero(out Mass mass); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] - public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] - public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] - private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); - public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) - { - MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); - } - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] - public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] - public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] - public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] - public static extern void QfromR(out Quaternion q, ref Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] - public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] - public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] - public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] - public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] - public static extern void QSetIdentity(out Quaternion q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] - public static extern dReal RandReal(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] - public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] - public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] - public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] - public static extern void RfromQ(out Matrix3 R, ref Quaternion q); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] - public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] - public static extern void RSetIdentity(out Matrix3 R); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] - public static extern void SetValue(out dReal a, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] - public static extern void SetZero(out dReal a, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr SimpleSpaceCreate(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] - public static extern void SolveCholesky(ref dReal L, out dReal b, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] - public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] - public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] - public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceAdd(IntPtr space, IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] - public static extern bool SpaceLockQuery(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceClean(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceDestroy(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] - public static extern bool SpaceGetCleanup(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] - public static extern int SpaceGetNumGeoms(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr SpaceGetGeom(IntPtr space, int i); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] - public static extern int SpaceGetSublevel(IntPtr space); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] - public static extern bool SpaceQuery(IntPtr space, IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceRemove(IntPtr space, IntPtr geom); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceSetCleanup(IntPtr space, bool mode); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] - public static extern void SpaceSetSublevel(IntPtr space, int sublevel); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] - public static extern void VectorScale(out dReal a, ref dReal d, int n); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr WorldCreate(); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] - public static extern void WorldDestroy(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] - public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] - public static extern bool WorldGetAutoDisableFlag(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] - public static extern int WorldGetAutoDisableSteps(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetAutoDisableTime(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] - public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetCFM(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetERP(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] - public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] - public static extern void WorldGetGravity(IntPtr world, out dReal X); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetAngularDamping(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetLinearDamping(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] - public static extern int WorldGetQuickStepNumIterations(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetQuickStepW(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] - public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] - public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] - public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] - public static extern void WorldQuickStep(IntPtr world, dReal stepsize); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetCFM(IntPtr world, dReal cfm); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetERP(IntPtr world, dReal erp); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] - public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] - public static extern void WorldStep(IntPtr world, dReal stepsize); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] - public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] - public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs b/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs deleted file mode 100644 index bd0e372..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs +++ /dev/null @@ -1,2818 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. - */ - -// Revision 2011/12/13 by Ubit Umarov -//#define SPAM - -using System; -using System.Collections.Generic; -using System.Reflection; -using System.Runtime.InteropServices; -using System.Threading; -using System.IO; -using System.Diagnostics; -using log4net; -using Nini.Config; -using Mono.Addins; -using OdeAPI; -using OpenSim.Framework; -using OpenSim.Region.Framework.Scenes; -using OpenSim.Region.Framework.Interfaces; -using OpenSim.Region.PhysicsModules.SharedBase; -using OpenMetaverse; - -namespace OpenSim.Region.PhysicsModule.UbitOde -{ - // colision flags of things others can colide with - // rays, sensors, probes removed since can't be colided with - // The top space where things are placed provided further selection - // ie physical are in active space nonphysical in static - // this should be exclusive as possible - - [Flags] - public enum CollisionCategories : uint - { - Disabled = 0, - //by 'things' types - Space = 0x01, - Geom = 0x02, // aka prim/part - Character = 0x04, - Land = 0x08, - Water = 0x010, - - // by state - Phantom = 0x01000, - VolumeDtc = 0x02000, - Selected = 0x04000, - NoShape = 0x08000, - - - All = 0xffffffff - } - - /// - /// Material type for a primitive - /// - public enum Material : int - { - /// - Stone = 0, - /// - Metal = 1, - /// - Glass = 2, - /// - Wood = 3, - /// - Flesh = 4, - /// - Plastic = 5, - /// - Rubber = 6, - - light = 7 // compatibility with old viewers - } - - public enum changes : int - { - Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) - Remove, - Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root - // or removes from a object if arg is null - DeLink, - Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child - Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child - PosOffset, // not in use - // arg Vector3 new position in local coords. Changes prim position in object - OriOffset, // not in use - // arg Vector3 new position in local coords. Changes prim position in object - Velocity, - AngVelocity, - Acceleration, - Force, - Torque, - Momentum, - - AddForce, - AddAngForce, - AngLock, - - Buoyancy, - - PIDTarget, - PIDTau, - PIDActive, - - PIDHoverHeight, - PIDHoverType, - PIDHoverTau, - PIDHoverActive, - - Size, - AvatarSize, - Shape, - PhysRepData, - AddPhysRep, - - CollidesWater, - VolumeDtc, - - Physical, - Phantom, - Selected, - disabled, - building, - - VehicleType, - VehicleFloatParam, - VehicleVectorParam, - VehicleRotationParam, - VehicleFlags, - SetVehicle, - - Null //keep this last used do dim the methods array. does nothing but pulsing the prim - } - - public struct ODEchangeitem - { - public PhysicsActor actor; - public OdeCharacter character; - public changes what; - public Object arg; - } - - public class ODEScene : PhysicsScene - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - public bool m_odeUbitLib = false; - public bool m_suportCombine = false; // mega suport not tested - public Scene m_frameWorkScene = null; - -// private int threadid = 0; - -// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; - - const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; - const float comumContactERP = 0.7f; - const float comumContactCFM = 0.0001f; - const float comumContactSLIP = 0f; - - float frictionMovementMult = 0.8f; - - float TerrainBounce = 0.1f; - float TerrainFriction = 0.3f; - - public float AvatarFriction = 0;// 0.9f * 0.5f; - - // this netx dimensions are only relevant for terrain partition (mega regions) - // WorldExtents below has the simulation dimensions - // they should be identical except on mega regions - private uint m_regionWidth = Constants.RegionSize; - private uint m_regionHeight = Constants.RegionSize; - - public float ODE_STEPSIZE = 0.020f; - public float HalfOdeStep = 0.01f; - public int odetimestepMS = 20; // rounded - private float metersInSpace = 25.6f; - private float m_timeDilation = 1.0f; - - private DateTime m_lastframe; - private DateTime m_lastMeshExpire; - - public float gravityx = 0f; - public float gravityy = 0f; - public float gravityz = -9.8f; - - private float waterlevel = 0f; - private int framecount = 0; - -// private int m_meshExpireCntr; - - private float avDensity = 3f; - private float avMovementDivisorWalk = 1.3f; - private float avMovementDivisorRun = 0.8f; - private float minimumGroundFlightOffset = 3f; - public float maximumMassObject = 10000.01f; - - - public float geomDefaultDensity = 10.000006836f; - - public float bodyPIDD = 35f; - public float bodyPIDG = 25; - - public int bodyFramesAutoDisable = 5; - - private d.NearCallback nearCallback; - - private HashSet _characters = new HashSet(); - private HashSet _prims = new HashSet(); - private HashSet _activeprims = new HashSet(); - private HashSet _activegroups = new HashSet(); - - public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); - - /// - /// A list of actors that should receive collision events. - /// - private List _collisionEventPrim = new List(); - private List _collisionEventPrimRemove = new List(); - - private HashSet _badCharacter = new HashSet(); -// public Dictionary geom_name_map = new Dictionary(); - public Dictionary actor_name_map = new Dictionary(); - - private float contactsurfacelayer = 0.001f; - - private int contactsPerCollision = 80; - internal IntPtr ContactgeomsArray = IntPtr.Zero; - private IntPtr GlobalContactsArray = IntPtr.Zero; - private d.Contact SharedTmpcontact = new d.Contact(); - - const int maxContactsbeforedeath = 4000; - private volatile int m_global_contactcount = 0; - - private IntPtr contactgroup; - - public ContactData[] m_materialContactsData = new ContactData[8]; - - private Dictionary RegionTerrain = new Dictionary(); - private Dictionary TerrainHeightFieldHeights = new Dictionary(); - private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); - - private int m_physicsiterations = 15; - private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag -// private PhysicsActor PANull = new NullPhysicsActor(); - private float step_time = 0.0f; - - public IntPtr world; - - - // split the spaces acording to contents type - // ActiveSpace contains characters and active prims - // StaticSpace contains land and other that is mostly static in enviroment - // this can contain subspaces, like the grid in staticspace - // as now space only contains this 2 top spaces - - public IntPtr TopSpace; // the global space - public IntPtr ActiveSpace; // space for active prims - public IntPtr CharsSpace; // space for active prims - public IntPtr StaticSpace; // space for the static things around - public IntPtr GroundSpace; // space for ground - - // some speedup variables - private int spaceGridMaxX; - private int spaceGridMaxY; - private float spacesPerMeterX; - private float spacesPerMeterY; - - // split static geometry collision into a grid as before - private IntPtr[,] staticPrimspace; - private IntPtr[] staticPrimspaceOffRegion; - - public Object OdeLock; - public static Object SimulationLock; - - public IMesher mesher; - - public IConfigSource m_config; - - public bool physics_logging = false; - public int physics_logging_interval = 0; - public bool physics_logging_append_existing_logfile = false; - - private Vector3 m_worldOffset = Vector3.Zero; - public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); - private PhysicsScene m_parentScene = null; - - private ODERayCastRequestManager m_rayCastManager; - public ODEMeshWorker m_meshWorker; - - /* maybe needed if ode uses tls - private void checkThread() - { - - int th = Thread.CurrentThread.ManagedThreadId; - if(th != threadid) - { - threadid = th; - d.AllocateODEDataForThread(~0U); - } - } - */ - - IConfig physicsconfig = null; - - public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, bool podeUbitLib) - { - OdeLock = new Object(); - - EngineType = pname; - PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName; - - m_config = psourceconfig; - m_odeUbitLib = podeUbitLib; - m_frameWorkScene = pscene; - - m_frameWorkScene.RegisterModuleInterface(this); - - Initialization(); - - base.Initialise(m_frameWorkScene.PhysicsRequestAsset, - (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]), - (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight); - } - - // core hack this just means all modules where loaded - // so now we can look for dependencies - public void RegionLoaded() - { - mesher = m_frameWorkScene.RequestModuleInterface(); - if (mesher == null) - { - m_log.WarnFormat("[UbitODE] No mesher. module disabled"); - return; - } - - m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig); - m_frameWorkScene.PhysicsEnabled = true; - } - /// - /// Initiailizes the scene - /// Sets many properties that ODE requires to be stable - /// These settings need to be tweaked 'exactly' right or weird stuff happens. - /// - private void Initialization() - { - // checkThread(); - SimulationLock = new Object(); - - nearCallback = near; - - m_rayCastManager = new ODERayCastRequestManager(this); - - WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX; - m_regionWidth = (uint)WorldExtents.X; - WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY; - m_regionHeight = (uint)WorldExtents.Y; - - m_suportCombine = false; - - lock (OdeLock) - { - // Create the world and the first space - try - { - world = d.WorldCreate(); - TopSpace = d.HashSpaceCreate(IntPtr.Zero); - - // now the major subspaces - ActiveSpace = d.HashSpaceCreate(TopSpace); - CharsSpace = d.HashSpaceCreate(TopSpace); - StaticSpace = d.HashSpaceCreate(TopSpace); - GroundSpace = d.HashSpaceCreate(TopSpace); - } - catch - { - // i must RtC#FM - // i did! - } - - d.HashSpaceSetLevels(TopSpace, -2, 8); - d.HashSpaceSetLevels(ActiveSpace, -2, 8); - d.HashSpaceSetLevels(CharsSpace, -4, 3); - d.HashSpaceSetLevels(StaticSpace, -2, 8); - d.HashSpaceSetLevels(GroundSpace, 0, 8); - - // demote to second level - d.SpaceSetSublevel(ActiveSpace, 1); - d.SpaceSetSublevel(CharsSpace, 1); - d.SpaceSetSublevel(StaticSpace, 1); - d.SpaceSetSublevel(GroundSpace, 1); - - d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Character | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(CharsSpace, 0); - - d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - // CollisionCategories.Land | - // CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(StaticSpace, 0); - - d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundSpace, 0); - - contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); - //contactgroup - - d.WorldSetAutoDisableFlag(world, false); - } - - - // checkThread(); - - - // Defaults - - int contactsPerCollision = 80; - - physicsconfig = null; - - if (m_config != null) - { - physicsconfig = m_config.Configs["ODEPhysicsSettings"]; - if (physicsconfig != null) - { - gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); - gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); - gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); - - metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); - - // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); - - ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); - - avDensity = physicsconfig.GetFloat("av_density", avDensity); - avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); - avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); - - contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); - - geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); - bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); - - physics_logging = physicsconfig.GetBoolean("physics_logging", false); - physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); - physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); - - minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); - maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); - } - } - - d.WorldSetCFM(world, comumContactCFM); - d.WorldSetERP(world, comumContactERP); - - d.WorldSetGravity(world, gravityx, gravityy, gravityz); - - d.WorldSetLinearDamping(world, 0.002f); - d.WorldSetAngularDamping(world, 0.002f); - d.WorldSetAngularDampingThreshold(world, 0f); - d.WorldSetLinearDampingThreshold(world, 0f); - d.WorldSetMaxAngularSpeed(world, 100f); - - d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - - d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); - d.WorldSetContactMaxCorrectingVel(world, 60.0f); - - HalfOdeStep = ODE_STEPSIZE * 0.5f; - odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f); - - ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); - GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); - - SharedTmpcontact.geom.g1 = IntPtr.Zero; - SharedTmpcontact.geom.g2 = IntPtr.Zero; - - SharedTmpcontact.geom.side1 = -1; - SharedTmpcontact.geom.side2 = -1; - - SharedTmpcontact.surface.mode = comumContactFlags; - SharedTmpcontact.surface.mu = 0; - SharedTmpcontact.surface.bounce = 0; - SharedTmpcontact.surface.soft_cfm = comumContactCFM; - SharedTmpcontact.surface.soft_erp = comumContactERP; - SharedTmpcontact.surface.slip1 = comumContactSLIP; - SharedTmpcontact.surface.slip2 = comumContactSLIP; - - m_materialContactsData[(int)Material.Stone].mu = 0.8f; - m_materialContactsData[(int)Material.Stone].bounce = 0.4f; - - m_materialContactsData[(int)Material.Metal].mu = 0.3f; - m_materialContactsData[(int)Material.Metal].bounce = 0.4f; - - m_materialContactsData[(int)Material.Glass].mu = 0.2f; - m_materialContactsData[(int)Material.Glass].bounce = 0.7f; - - m_materialContactsData[(int)Material.Wood].mu = 0.6f; - m_materialContactsData[(int)Material.Wood].bounce = 0.5f; - - m_materialContactsData[(int)Material.Flesh].mu = 0.9f; - m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; - - m_materialContactsData[(int)Material.Plastic].mu = 0.4f; - m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; - - m_materialContactsData[(int)Material.Rubber].mu = 0.9f; - m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; - - m_materialContactsData[(int)Material.light].mu = 0.0f; - m_materialContactsData[(int)Material.light].bounce = 0.0f; - - - spacesPerMeterX = 1.0f / metersInSpace; - spacesPerMeterY = spacesPerMeterX; - spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); - spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); - - if (spaceGridMaxX > 24) - { - spaceGridMaxX = 24; - spacesPerMeterX = spaceGridMaxX / WorldExtents.X; - } - - if (spaceGridMaxY > 24) - { - spaceGridMaxY = 24; - spacesPerMeterY = spaceGridMaxY / WorldExtents.Y; - } - - staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; - - // create all spaces now - int i, j; - IntPtr newspace; - - for (i = 0; i < spaceGridMaxX; i++) - for (j = 0; j < spaceGridMaxY; j++) - { - newspace = d.HashSpaceCreate(StaticSpace); - d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); - waitForSpaceUnlock(newspace); - d.SpaceSetSublevel(newspace, 2); - d.HashSpaceSetLevels(newspace, -2, 8); - d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(newspace, 0); - - staticPrimspace[i, j] = newspace; - } - - // let this now be index limit - spaceGridMaxX--; - spaceGridMaxY--; - - // create 4 off world spaces (x<0,x>max,y<0,y>max) - staticPrimspaceOffRegion = new IntPtr[4]; - - for (i = 0; i < 4; i++) - { - newspace = d.HashSpaceCreate(StaticSpace); - d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); - waitForSpaceUnlock(newspace); - d.SpaceSetSublevel(newspace, 2); - d.HashSpaceSetLevels(newspace, -2, 8); - d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | - CollisionCategories.Geom | - CollisionCategories.Land | - CollisionCategories.Water | - CollisionCategories.Phantom | - CollisionCategories.VolumeDtc - )); - d.GeomSetCollideBits(newspace, 0); - - staticPrimspaceOffRegion[i] = newspace; - } - - m_lastframe = DateTime.UtcNow; - m_lastMeshExpire = m_lastframe; - } - - internal void waitForSpaceUnlock(IntPtr space) - { - //if (space != IntPtr.Zero) - //while (d.SpaceLockQuery(space)) { } // Wait and do nothing - } - - #region Collision Detection - - // sets a global contact for a joint for contactgeom , and base contact description) - - - - private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom) - { - if (m_global_contactcount >= maxContactsbeforedeath) - return IntPtr.Zero; - - m_global_contactcount++; - - SharedTmpcontact.geom.depth = contactGeom.depth; - SharedTmpcontact.geom.pos = contactGeom.pos; - SharedTmpcontact.geom.normal = contactGeom.normal; - - IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); - Marshal.StructureToPtr(SharedTmpcontact, contact, true); - return d.JointCreateContactPtr(world, contactgroup, contact); - } - - private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) - { - if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) - return false; - - IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); - newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); - return true; - } - - /// - /// This is our near callback. A geometry is near a body - /// - /// The space that contains the geoms. Remember, spaces are also geoms - /// a geometry or space - /// another geometry or space - /// - - private void near(IntPtr space, IntPtr g1, IntPtr g2) - { - // no lock here! It's invoked from within Simulate(), which is thread-locked - - if (m_global_contactcount >= maxContactsbeforedeath) - return; - - // Test if we're colliding a geom with a space. - // If so we have to drill down into the space recursively - - if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) - return; - - if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) - { - // We'll be calling near recursivly if one - // of them is a space to find all of the - // contact points in the space - try - { - d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to collide test a space"); - return; - } - //here one should check collisions of geoms inside a space - // but on each space we only should have geoms that not colide amoung each other - // so we don't dig inside spaces - return; - } - - // get geom bodies to check if we already a joint contact - // guess this shouldn't happen now - IntPtr b1 = d.GeomGetBody(g1); - IntPtr b2 = d.GeomGetBody(g2); - - // d.GeomClassID id = d.GeomGetClass(g1); - - // Figure out how many contact points we have - int count = 0; - try - { - // Colliding Geom To Geom - // This portion of the function 'was' blatantly ripped off from BoxStack.cs - - if (g1 == g2) - return; // Can't collide with yourself - - if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) - return; - /* - // debug - PhysicsActor dp2; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) - { - d.AABB aabb; - d.GeomGetAABB(g2, out aabb); - float x = aabb.MaxX - aabb.MinX; - float y = aabb.MaxY - aabb.MinY; - float z = aabb.MaxZ - aabb.MinZ; - if (x > 60.0f || y > 60.0f || z > 60.0f) - { - if (!actor_name_map.TryGetValue(g2, out dp2)) - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - else - m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", - dp2.Name, dp2.Size, x, y, z, - dp2.Position.ToString(), - dp2.Orientation.ToString(), - dp2.Orientation.Length()); - return; - } - } - // - */ - - - if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || - d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) - { - int cflags; - unchecked - { - cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); - } - count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); - } - else - count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); - } - catch (SEHException) - { - m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); - // ode.drelease(world); - base.TriggerPhysicsBasedRestart(); - } - catch (Exception e) - { - m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); - return; - } - - // contacts done - if (count == 0) - return; - - // try get physical actors - PhysicsActor p1; - PhysicsActor p2; - - if (!actor_name_map.TryGetValue(g1, out p1)) - { - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); - return; - } - - if (!actor_name_map.TryGetValue(g2, out p2)) - { - m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); - return; - } - - // update actors collision score - if (p1.CollisionScore >= float.MaxValue - count) - p1.CollisionScore = 0; - p1.CollisionScore += count; - - if (p2.CollisionScore >= float.MaxValue - count) - p2.CollisionScore = 0; - p2.CollisionScore += count; - - // get first contact - d.ContactGeom curContact = new d.ContactGeom(); - - if (!GetCurContactGeom(0, ref curContact)) - return; - - ContactPoint maxDepthContact = new ContactPoint(); - - // do volume detection case - if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) - { - maxDepthContact = new ContactPoint( - new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), - new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), - curContact.depth, false - ); - - collision_accounting_events(p1, p2, maxDepthContact); - return; - } - - // big messy collision analises - - float mu = 0; - float bounce = 0; -// bool IgnoreNegSides = false; - - ContactData contactdata1 = new ContactData(0, 0, false); - ContactData contactdata2 = new ContactData(0, 0, false); - - bool dop1ava = false; - bool dop2ava = false; - bool ignore = false; - - switch (p1.PhysicsActorType) - { - case (int)ActorTypes.Agent: - { - dop1ava = true; - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Agent: - case (int)ActorTypes.Prim: - break; - - default: - ignore = true; // avatar to terrain and water ignored - break; - } - break; - } - - case (int)ActorTypes.Prim: - { - switch (p2.PhysicsActorType) - { - case (int)ActorTypes.Agent: - dop2ava = true; - break; - - case (int)ActorTypes.Prim: - Vector3 relV = p1.Velocity - p2.Velocity; - float relVlenSQ = relV.LengthSquared(); - if (relVlenSQ > 0.0001f) - { - p1.CollidingObj = true; - p2.CollidingObj = true; - } - p1.getContactData(ref contactdata1); - p2.getContactData(ref contactdata2); - bounce = contactdata1.bounce * contactdata2.bounce; - mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); - - if (relVlenSQ > 0.01f) - mu *= frictionMovementMult; - - break; - - case (int)ActorTypes.Ground: - p1.getContactData(ref contactdata1); - bounce = contactdata1.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); - - if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - p1.CollidingGround = true; - /* - if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) - { - if (curContact.side1 > 0) - IgnoreNegSides = true; - } - */ - break; - - case (int)ActorTypes.Water: - default: - ignore = true; - break; - } - } - break; - - case (int)ActorTypes.Ground: - if (p2.PhysicsActorType == (int)ActorTypes.Prim) - { - p2.CollidingGround = true; - p2.getContactData(ref contactdata2); - bounce = contactdata2.bounce * TerrainBounce; - mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); - -// if (curContact.side1 > 0) // should be 2 ? -// IgnoreNegSides = true; - - if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) - mu *= frictionMovementMult; - } - else - ignore = true; - break; - - case (int)ActorTypes.Water: - default: - break; - } - - if (ignore) - return; - - IntPtr Joint; - bool FeetCollision = false; - int ncontacts = 0; - - int i = 0; - - maxDepthContact = new ContactPoint(); - maxDepthContact.PenetrationDepth = float.MinValue; - ContactPoint minDepthContact = new ContactPoint(); - minDepthContact.PenetrationDepth = float.MaxValue; - - SharedTmpcontact.geom.depth = 0; - SharedTmpcontact.surface.mu = mu; - SharedTmpcontact.surface.bounce = bounce; - - d.ContactGeom altContact = new d.ContactGeom(); - bool useAltcontact = false; - bool noskip = true; - - while (true) - { -// if (!(IgnoreNegSides && curContact.side1 < 0)) - { - noskip = true; - useAltcontact = false; - - if (dop1ava) - { - if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) - { - if (p2.PhysicsActorType == (int)ActorTypes.Agent) - { - p1.CollidingObj = true; - p2.CollidingObj = true; - } - else if (p2.Velocity.LengthSquared() > 0.0f) - p2.CollidingObj = true; - } - else - noskip = false; - } - else if (dop2ava) - { - if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) - { - if (p1.PhysicsActorType == (int)ActorTypes.Agent) - { - p1.CollidingObj = true; - p2.CollidingObj = true; - } - else if (p2.Velocity.LengthSquared() > 0.0f) - p1.CollidingObj = true; - } - else - noskip = false; - } - - if (noskip) - { - if(useAltcontact) - Joint = CreateContacJoint(ref altContact); - else - Joint = CreateContacJoint(ref curContact); - - if (Joint == IntPtr.Zero) - break; - - d.JointAttach(Joint, b1, b2); - - ncontacts++; - - if (curContact.depth > maxDepthContact.PenetrationDepth) - { - maxDepthContact.Position.X = curContact.pos.X; - maxDepthContact.Position.Y = curContact.pos.Y; - maxDepthContact.Position.Z = curContact.pos.Z; - maxDepthContact.PenetrationDepth = curContact.depth; - maxDepthContact.CharacterFeet = FeetCollision; - } - - if (curContact.depth < minDepthContact.PenetrationDepth) - { - minDepthContact.PenetrationDepth = curContact.depth; - minDepthContact.SurfaceNormal.X = curContact.normal.X; - minDepthContact.SurfaceNormal.Y = curContact.normal.Y; - minDepthContact.SurfaceNormal.Z = curContact.normal.Z; - } - } - } - if (++i >= count) - break; - - if (!GetCurContactGeom(i, ref curContact)) - break; - } - - if (ncontacts > 0) - { - maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; - maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; - maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; - - collision_accounting_events(p1, p2, maxDepthContact); - } - } - - private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) - { - uint obj2LocalID = 0; - - bool p1events = p1.SubscribedEvents(); - bool p2events = p2.SubscribedEvents(); - - if (p1.IsVolumeDtc) - p2events = false; - if (p2.IsVolumeDtc) - p1events = false; - - if (!p2events && !p1events) - return; - - Vector3 vel = Vector3.Zero; - if (p2 != null && p2.IsPhysical) - vel = p2.Velocity; - - if (p1 != null && p1.IsPhysical) - vel -= p1.Velocity; - - contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); - - switch ((ActorTypes)p1.PhysicsActorType) - { - case ActorTypes.Agent: - case ActorTypes.Prim: - { - switch ((ActorTypes)p2.PhysicsActorType) - { - case ActorTypes.Agent: - case ActorTypes.Prim: - if (p2events) - { - AddCollisionEventReporting(p2); - p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); - } - obj2LocalID = p2.ParentActor.LocalID; - break; - - case ActorTypes.Ground: - case ActorTypes.Unknown: - default: - obj2LocalID = 0; - break; - } - if (p1events) - { - contact.SurfaceNormal = -contact.SurfaceNormal; - AddCollisionEventReporting(p1); - p1.AddCollisionEvent(obj2LocalID, contact); - } - break; - } - case ActorTypes.Ground: - case ActorTypes.Unknown: - default: - { - if (p2events && !p2.IsVolumeDtc) - { - AddCollisionEventReporting(p2); - p2.AddCollisionEvent(0, contact); - } - break; - } - } - } - - /// - /// This is our collision testing routine in ODE - /// - /// - private void collision_optimized() - { - lock (_characters) - { - try - { - foreach (OdeCharacter chr in _characters) - { - if (chr == null || chr.Body == IntPtr.Zero) - continue; - - chr.IsColliding = false; - // chr.CollidingGround = false; not done here - chr.CollidingObj = false; - // do colisions with static space - d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); - - // no coll with gnd - } - // chars with chars - d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); - - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); - } - - } - - lock (_activeprims) - { - foreach (OdePrim aprim in _activeprims) - { - aprim.CollisionScore = 0; - aprim.IsColliding = false; - } - } - - // collide active prims with static enviroment - lock (_activegroups) - { - try - { - foreach (OdePrim prm in _activegroups) - { - if (!prm.m_outbounds) - { - if (d.BodyIsEnabled(prm.Body)) - { - d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); - d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); - } - } - } - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); - } - } - // colide active amoung them - try - { - d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space"); - } - // and with chars - try - { - d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); - } - catch (AccessViolationException) - { - m_log.Warn("[PHYSICS]: Unable to collide in Active space"); - } - // _perloopContact.Clear(); - } - - #endregion - /// - /// Add actor to the list that should receive collision events in the simulate loop. - /// - /// - public void AddCollisionEventReporting(PhysicsActor obj) - { - if (!_collisionEventPrim.Contains(obj)) - _collisionEventPrim.Add(obj); - } - - /// - /// Remove actor from the list that should receive collision events in the simulate loop. - /// - /// - public void RemoveCollisionEventReporting(PhysicsActor obj) - { - if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) - _collisionEventPrimRemove.Add(obj); - } - - public override float TimeDilation - { - get { return m_timeDilation; } - } - - public override bool SupportsNINJAJoints - { - get { return false; } - } - - #region Add/Remove Entities - - public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) - { - return null; - } - - public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) - { - OdeCharacter newAv = new OdeCharacter(localID, avName, this, position, - size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); - newAv.Flying = isFlying; - newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; - - return newAv; - } - - public void AddCharacter(OdeCharacter chr) - { - lock (_characters) - { - if (!_characters.Contains(chr)) - { - _characters.Add(chr); - if (chr.bad) - m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); - } - } - } - - public void RemoveCharacter(OdeCharacter chr) - { - lock (_characters) - { - if (_characters.Contains(chr)) - { - _characters.Remove(chr); - } - } - } - - public void BadCharacter(OdeCharacter chr) - { - lock (_badCharacter) - { - if (!_badCharacter.Contains(chr)) - _badCharacter.Add(chr); - } - } - - public override void RemoveAvatar(PhysicsActor actor) - { - //m_log.Debug("[PHYSICS]:ODELOCK"); - ((OdeCharacter) actor).Destroy(); - } - - - public void addActivePrim(OdePrim activatePrim) - { - // adds active prim.. - lock (_activeprims) - { - if (!_activeprims.Contains(activatePrim)) - _activeprims.Add(activatePrim); - } - } - - public void addActiveGroups(OdePrim activatePrim) - { - lock (_activegroups) - { - if (!_activegroups.Contains(activatePrim)) - _activegroups.Add(activatePrim); - } - } - - private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, - PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) - { - OdePrim newPrim; - lock (OdeLock) - { - newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); - lock (_prims) - _prims.Add(newPrim); - } - return newPrim; - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) - { - return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); - } - - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, uint localid) - { - return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); - } - - public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, - Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) - { - - return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); - } - - public void remActivePrim(OdePrim deactivatePrim) - { - lock (_activeprims) - { - _activeprims.Remove(deactivatePrim); - } - } - public void remActiveGroup(OdePrim deactivatePrim) - { - lock (_activegroups) - { - _activegroups.Remove(deactivatePrim); - } - } - - public override void RemovePrim(PhysicsActor prim) - { - // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be - // removed in the next physics simulate pass. - if (prim is OdePrim) - { -// lock (OdeLock) - { - - OdePrim p = (OdePrim)prim; - p.setPrimForRemoval(); - } - } - } - - public void RemovePrimThreadLocked(OdePrim prim) - { - //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); - lock (prim) - { -// RemoveCollisionEventReporting(prim); - lock (_prims) - _prims.Remove(prim); - } - - } - - public bool havePrim(OdePrim prm) - { - lock (_prims) - return _prims.Contains(prm); - } - - public bool haveActor(PhysicsActor actor) - { - if (actor is OdePrim) - { - lock (_prims) - return _prims.Contains((OdePrim)actor); - } - else if (actor is OdeCharacter) - { - lock (_characters) - return _characters.Contains((OdeCharacter)actor); - } - return false; - } - - #endregion - - #region Space Separation Calculation - - /// - /// Called when a static prim moves or becomes static - /// Places the prim in a space one the static sub-spaces grid - /// - /// the pointer to the geom that moved - /// the position that the geom moved to - /// a pointer to the space it was in before it was moved. - /// a pointer to the new space it's in - public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) - { - // moves a prim into another static sub-space or from another space into a static sub-space - - // Called ODEPrim so - // it's already in locked space. - - if (geom == IntPtr.Zero) // shouldn't happen - return IntPtr.Zero; - - // get the static sub-space for current position - IntPtr newspace = calculateSpaceForGeom(pos); - - if (newspace == currentspace) // if we are there all done - return newspace; - - // else remove it from its current space - if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) - { - if (d.GeomIsSpace(currentspace)) - { - waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); - - if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) - { - d.SpaceDestroy(currentspace); - } - } - else - { - m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + - " Geom:" + geom); - } - } - else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space - { - currentspace = d.GeomGetSpace(geom); - if (currentspace != IntPtr.Zero) - { - if (d.GeomIsSpace(currentspace)) - { - waitForSpaceUnlock(currentspace); - d.SpaceRemove(currentspace, geom); - - if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) - { - d.SpaceDestroy(currentspace); - } - - } - } - } - - // put the geom in the newspace - waitForSpaceUnlock(newspace); - d.SpaceAdd(newspace, geom); - - // let caller know this newspace - return newspace; - } - - /// - /// Calculates the space the prim should be in by its position - /// - /// - /// a pointer to the space. This could be a new space or reused space. - public IntPtr calculateSpaceForGeom(Vector3 pos) - { - int x, y; - - if (pos.X < 0) - return staticPrimspaceOffRegion[0]; - - if (pos.Y < 0) - return staticPrimspaceOffRegion[2]; - - x = (int)(pos.X * spacesPerMeterX); - if (x > spaceGridMaxX) - return staticPrimspaceOffRegion[1]; - - y = (int)(pos.Y * spacesPerMeterY); - if (y > spaceGridMaxY) - return staticPrimspaceOffRegion[3]; - - return staticPrimspace[x, y]; - } - - #endregion - - - /// - /// Called to queue a change to a actor - /// to use in place of old taint mechanism so changes do have a time sequence - /// - - public void AddChange(PhysicsActor actor, changes what, Object arg) - { - ODEchangeitem item = new ODEchangeitem(); - item.actor = actor; - item.what = what; - item.arg = arg; - ChangesQueue.Enqueue(item); - } - - /// - /// Called after our prim properties are set Scale, position etc. - /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex - /// This assures us that we have no race conditions - /// - /// - public override void AddPhysicsActorTaint(PhysicsActor prim) - { - } - - // does all pending changes generated during region load process - public override void PrepareSimulation() - { - lock (OdeLock) - { - if (world == IntPtr.Zero) - { - ChangesQueue.Clear(); - return; - } - - ODEchangeitem item; - - int donechanges = 0; - if (ChangesQueue.Count > 0) - { - m_log.InfoFormat("[ODE] start processing pending actor operations"); - int tstart = Util.EnvironmentTickCount(); - - while (ChangesQueue.Dequeue(out item)) - { - if (item.actor != null) - { - try - { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); - } - catch - { - m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); - } - } - donechanges++; - } - int time = Util.EnvironmentTickCountSubtract(tstart); - m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); - } - } - } - - /// - /// This is our main simulate loop - /// It's thread locked by a Mutex in the scene. - /// It holds Collisions, it instructs ODE to step through the physical reactions - /// It moves the objects around in memory - /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) - /// - /// - /// - public override float Simulate(float timeStep) - { - DateTime now = DateTime.UtcNow; - TimeSpan timedif = now - m_lastframe; - timeStep = (float)timedif.TotalSeconds; - m_lastframe = now; - - // acumulate time so we can reduce error - step_time += timeStep; - - if (step_time < HalfOdeStep) - return 0; - - if (framecount < 0) - framecount = 0; - - framecount++; - -// int curphysiteractions; - - // if in trouble reduce step resolution -// if (step_time >= m_SkipFramesAtms) -// curphysiteractions = m_physicsiterations / 2; -// else -// curphysiteractions = m_physicsiterations; - -// checkThread(); - int nodeframes = 0; - - lock (SimulationLock) - lock(OdeLock) - { - if (world == IntPtr.Zero) - { - ChangesQueue.Clear(); - return 0; - } - - ODEchangeitem item; - -// d.WorldSetQuickStepNumIterations(world, curphysiteractions); - - int loopstartMS = Util.EnvironmentTickCount(); - int looptimeMS = 0; - - - while (step_time > HalfOdeStep) - { - try - { - // clear pointer/counter to contacts to pass into joints - m_global_contactcount = 0; - - if (ChangesQueue.Count > 0) - { - int changestartMS = Util.EnvironmentTickCount(); - int ttmp; - while (ChangesQueue.Dequeue(out item)) - { - if (item.actor != null) - { - try - { - if (item.actor is OdeCharacter) - ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); - else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) - RemovePrimThreadLocked((OdePrim)item.actor); - } - catch - { - m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", - item.actor.Name, item.what.ToString()); - } - } - ttmp = Util.EnvironmentTickCountSubtract(changestartMS); - if (ttmp > 20) - break; - } - } - - // Move characters - lock (_characters) - { - List defects = new List(); - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - actor.Move(defects); - } - if (defects.Count != 0) - { - foreach (OdeCharacter defect in defects) - { - RemoveCharacter(defect); - } - defects.Clear(); - } - } - - // Move other active objects - lock (_activegroups) - { - foreach (OdePrim aprim in _activegroups) - { - aprim.Move(); - } - } - - m_rayCastManager.ProcessQueuedRequests(); - - collision_optimized(); - - foreach (PhysicsActor obj in _collisionEventPrim) - { - if (obj == null) - continue; - - switch ((ActorTypes)obj.PhysicsActorType) - { - case ActorTypes.Agent: - OdeCharacter cobj = (OdeCharacter)obj; - cobj.AddCollisionFrameTime((int)(odetimestepMS)); - cobj.SendCollisions(); - break; - - case ActorTypes.Prim: - OdePrim pobj = (OdePrim)obj; - if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) - if (!pobj.m_outbounds) - { - pobj.AddCollisionFrameTime((int)(odetimestepMS)); - pobj.SendCollisions(); - } - break; - } - } - - foreach (PhysicsActor obj in _collisionEventPrimRemove) - _collisionEventPrim.Remove(obj); - - _collisionEventPrimRemove.Clear(); - - // do a ode simulation step - d.WorldQuickStep(world, ODE_STEPSIZE); -// d.WorldStep(world, ODE_STEPSIZE); - d.JointGroupEmpty(contactgroup); - - // update managed ideia of physical data and do updates to core - /* - lock (_characters) - { - foreach (OdeCharacter actor in _characters) - { - if (actor != null) - { - if (actor.bad) - m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); - - actor.UpdatePositionAndVelocity(); - } - } - } - */ - - lock (_activegroups) - { - { - foreach (OdePrim actor in _activegroups) - { - if (actor.IsPhysical) - { - actor.UpdatePositionAndVelocity(framecount); - } - } - } - } - } - catch (Exception e) - { - m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); -// ode.dunlock(world); - } - - step_time -= ODE_STEPSIZE; - nodeframes++; - - looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); - if (looptimeMS > 100) - break; - } - - lock (_badCharacter) - { - if (_badCharacter.Count > 0) - { - foreach (OdeCharacter chr in _badCharacter) - { - RemoveCharacter(chr); - } - - _badCharacter.Clear(); - } - } - - timedif = now - m_lastMeshExpire; - - if (timedif.Seconds > 10) - { - mesher.ExpireReleaseMeshs(); - m_lastMeshExpire = now; - } - -// information block running in debug only -/* - int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); - int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); - int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); - - int nactivegeoms = 0; - int nactivespaces = 0; - - int nstaticgeoms = 0; - int nstaticspaces = 0; - IntPtr sp; - - for (int i = 0; i < ntopactivegeoms; i++) - { - sp = d.SpaceGetGeom(ActiveSpace, i); - if (d.GeomIsSpace(sp)) - { - nactivespaces++; - nactivegeoms += d.SpaceGetNumGeoms(sp); - } - else - nactivegeoms++; - } - - for (int i = 0; i < ntopstaticgeoms; i++) - { - sp = d.SpaceGetGeom(StaticSpace, i); - if (d.GeomIsSpace(sp)) - { - nstaticspaces++; - nstaticgeoms += d.SpaceGetNumGeoms(sp); - } - else - nstaticgeoms++; - } - - int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); - - int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray - int nbodies = d.NTotalBodies; - int ngeoms = d.NTotalGeoms; -*/ - // Finished with all sim stepping. If requested, dump world state to file for debugging. - // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? - // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? - if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) - { - string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename - string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file - - if (physics_logging_append_existing_logfile) - { - string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; - TextWriter fwriter = File.AppendText(fname); - fwriter.WriteLine(header); - fwriter.Close(); - } - - d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); - } - - // think time dilation as to do with dinamic step size that we dont' have - // even so tell something to world - if (looptimeMS < 100) // we did the requested loops - m_timeDilation = 1.0f; - else if (step_time > 0) - { - m_timeDilation = timeStep / step_time; - if (m_timeDilation > 1) - m_timeDilation = 1; - if (step_time > m_SkipFramesAtms) - step_time = 0; - m_lastframe = DateTime.UtcNow; // skip also the time lost - } - } - return (float)nodeframes * ODE_STEPSIZE / timeStep; - } - - /// - public override void GetResults() - { - } - - public override bool IsThreaded - { - // for now we won't be multithreaded - get { return (false); } - } - - public float GetTerrainHeightAtXY(float x, float y) - { - - int offsetX = 0; - int offsetY = 0; - - if (m_suportCombine) - { - offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - } - - // get region map - IntPtr heightFieldGeom = IntPtr.Zero; - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return 0f; - - if (heightFieldGeom == IntPtr.Zero) - return 0f; - - if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) - return 0f; - - // TerrainHeightField for ODE as offset 1m - x += 1f - offsetX; - y += 1f - offsetY; - - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - - // integer indexs - int ix; - int iy; - // interpolators offset - float dx; - float dy; - - int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples - int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples - int regsize = regsizeX; - - if (m_odeUbitLib) - { - if (x < regsizeX - 1) - { - ix = (int)x; - dx = x - (float)ix; - } - else // out world use external height - { - ix = regsizeX - 2; - dx = 0; - } - if (y < regsizeY - 1) - { - iy = (int)y; - dy = y - (float)iy; - } - else - { - iy = regsizeY - 2; - dy = 0; - } - } - else - { - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - - regsize = regsizeY; - - if (x < regsizeX - 1) - { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height - { - iy = regsizeX - 2; - dy = 0; - } - if (y < regsizeY - 1) - { - ix = (int)y; - dx = y - (float)ix; - } - else - { - ix = regsizeY - 2; - dx = 0; - } - } - - float h0; - float h1; - float h2; - - iy *= regsize; - iy += ix; // all indexes have iy + ix - - float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; - /* - if ((dx + dy) <= 1.0f) - { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 - } - else - { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 - } - */ - h0 = ((float)heights[iy]); // 0,0 vertice - - if (dy>dx) - { - iy += regsize; - h2 = (float)heights[iy]; // 0,1 vertice - h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 - h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 - } - else - { - iy++; - h2 = (float)heights[iy]; // vertice 1,0 - h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 - } - - return h0 + h1 + h2; - } - - public Vector3 GetTerrainNormalAtXY(float x, float y) - { - int offsetX = 0; - int offsetY = 0; - - if (m_suportCombine) - { - offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - } - - // get region map - IntPtr heightFieldGeom = IntPtr.Zero; - Vector3 norm = new Vector3(0, 0, 1); - - if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) - return norm; ; - - if (heightFieldGeom == IntPtr.Zero) - return norm; - - if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) - return norm; - - // TerrainHeightField for ODE as offset 1m - x += 1f - offsetX; - y += 1f - offsetY; - - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - - // integer indexs - int ix; - int iy; - // interpolators offset - float dx; - float dy; - - int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples - int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples - int regsize = regsizeX; - - int xstep = 1; - int ystep = regsizeX; - bool firstTri = false; - - if (m_odeUbitLib) - { - if (x < regsizeX - 1) - { - ix = (int)x; - dx = x - (float)ix; - } - else // out world use external height - { - ix = regsizeX - 2; - dx = 0; - } - if (y < regsizeY - 1) - { - iy = (int)y; - dy = y - (float)iy; - } - else - { - iy = regsizeY - 2; - dy = 0; - } - firstTri = dy > dx; - } - - else - { - xstep = regsizeY; - ystep = 1; - regsize = regsizeY; - - // we still have square fixed size regions - // also flip x and y because of how map is done for ODE fliped axis - // so ix,iy,dx and dy are inter exchanged - if (x < regsizeX - 1) - { - iy = (int)x; - dy = x - (float)iy; - } - else // out world use external height - { - iy = regsizeX - 2; - dy = 0; - } - if (y < regsizeY - 1) - { - ix = (int)y; - dx = y - (float)ix; - } - else - { - ix = regsizeY - 2; - dx = 0; - } - firstTri = dx > dy; - } - - float h0; - float h1; - float h2; - - iy *= regsize; - iy += ix; // all indexes have iy + ix - - float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; - - if (firstTri) - { - h1 = ((float)heights[iy]); // 0,0 vertice - iy += ystep; - h0 = (float)heights[iy]; // 0,1 - h2 = (float)heights[iy+xstep]; // 1,1 vertice - norm.X = h0 - h2; - norm.Y = h1 - h0; - } - else - { - h2 = ((float)heights[iy]); // 0,0 vertice - iy += xstep; - h0 = ((float)heights[iy]); // 1,0 vertice - h1 = (float)heights[iy+ystep]; // vertice 1,1 - norm.X = h2 - h0; - norm.Y = h0 - h1; - } - norm.Z = 1; - norm.Normalize(); - return norm; - } - - public override void SetTerrain(float[] heightMap) - { - if (m_worldOffset != Vector3.Zero && m_parentScene != null) - { - if (m_parentScene is ODEScene) - { - ((ODEScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); - } - } - else - { - SetTerrain(heightMap, m_worldOffset); - } - } - - public override void CombineTerrain(float[] heightMap, Vector3 pOffset) - { - if(m_suportCombine) - SetTerrain(heightMap, pOffset); - } - - public void SetTerrain(float[] heightMap, Vector3 pOffset) - { - if (m_odeUbitLib) - UbitSetTerrain(heightMap, pOffset); - else - OriSetTerrain(heightMap, pOffset); - } - - public void OriSetTerrain(float[] heightMap, Vector3 pOffset) - { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future - - float[] _heightmap; - - uint regionsizeX = m_regionWidth; - uint regionsizeY = m_regionHeight; - - // map is rotated - uint heightmapWidth = regionsizeY + 2; - uint heightmapHeight = regionsizeX + 2; - - uint heightmapWidthSamples = heightmapWidth + 1; - uint heightmapHeightSamples = heightmapHeight + 1; - - _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; - - const float scale = 1.0f; - const float offset = 0.0f; - const float thickness = 10f; - const int wrap = 0; - - - float hfmin = float.MaxValue; - float hfmax = float.MinValue; - float val; - uint xx; - uint yy; - - uint maxXX = regionsizeX - 1; - uint maxYY = regionsizeY - 1; - // flipping map adding one margin all around so things don't fall in edges - - uint xt = 0; - xx = 0; - - for (uint x = 0; x < heightmapWidthSamples; x++) - { - if (x > 1 && xx < maxXX) - xx++; - yy = 0; - for (uint y = 0; y < heightmapHeightSamples; y++) - { - if (y > 1 && y < maxYY) - yy += regionsizeX; - - val = heightMap[yy + xx]; - if (val < 0.0f) - val = 0.0f; // no neg terrain as in chode - _heightmap[xt + y] = val; - - if (hfmin > val) - hfmin = val; - if (hfmax < val) - hfmax = val; - } - xt += heightmapHeightSamples; - } - - lock (OdeLock) - { - IntPtr GroundGeom = IntPtr.Zero; - if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) - { - RegionTerrain.Remove(pOffset); - if (GroundGeom != IntPtr.Zero) - { - actor_name_map.Remove(GroundGeom); - d.GeomDestroy(GroundGeom); - - if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) - { - TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); - TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); - TerrainHeightFieldHeights.Remove(GroundGeom); - } - } - } - IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); - - GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); - - d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, - offset, thickness, wrap); - - d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - - GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); - - if (GroundGeom != IntPtr.Zero) - { - d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, 0); - - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Terrain"; - pa.PhysicsActorType = (int)ActorTypes.Ground; - actor_name_map[GroundGeom] = pa; - -// geom_name_map[GroundGeom] = "Terrain"; - - d.Matrix3 R = new d.Matrix3(); - - Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); - Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); - - - q1 = q1 * q2; - - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); - - d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); - d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); - } - } - } - - public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) - { - // assumes 1m size grid and constante size square regions - // needs to know about sims around in future - - float[] _heightmap; - - uint regionsizeX = m_regionWidth; - uint regionsizeY = m_regionHeight; - - uint heightmapWidth = regionsizeX + 2; - uint heightmapHeight = regionsizeY + 2; - - uint heightmapWidthSamples = heightmapWidth + 1; - uint heightmapHeightSamples = heightmapHeight + 1; - - _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; - - - float hfmin = float.MaxValue; -// float hfmax = float.MinValue; - float val; - - - uint maxXX = regionsizeX - 1; - uint maxYY = regionsizeY - 1; - // adding one margin all around so things don't fall in edges - - uint xx; - uint yy = 0; - uint yt = 0; - - for (uint y = 0; y < heightmapHeightSamples; y++) - { - if (y > 1 && y < maxYY) - yy += regionsizeX; - xx = 0; - for (uint x = 0; x < heightmapWidthSamples; x++) - { - if (x > 1 && x < maxXX) - xx++; - - val = heightMap[yy + xx]; - if (val < 0.0f) - val = 0.0f; // no neg terrain as in chode - _heightmap[yt + x] = val; - - if (hfmin > val) - hfmin = val; -// if (hfmax < val) -// hfmax = val; - } - yt += heightmapWidthSamples; - } - lock (OdeLock) - { - IntPtr GroundGeom = IntPtr.Zero; - if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) - { - RegionTerrain.Remove(pOffset); - if (GroundGeom != IntPtr.Zero) - { - actor_name_map.Remove(GroundGeom); - d.GeomDestroy(GroundGeom); - - if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) - { - if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) - TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); - TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); - TerrainHeightFieldHeights.Remove(GroundGeom); - } - } - } - IntPtr HeightmapData = d.GeomUbitTerrainDataCreate(); - - const int wrap = 0; - float thickness = hfmin; - if (thickness < 0) - thickness = 1; - - GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); - - d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, - thickness, wrap); - -// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); - GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); - if (GroundGeom != IntPtr.Zero) - { - d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); - d.GeomSetCollideBits(GroundGeom, 0); - - - PhysicsActor pa = new NullPhysicsActor(); - pa.Name = "Terrain"; - pa.PhysicsActorType = (int)ActorTypes.Ground; - actor_name_map[GroundGeom] = pa; - -// geom_name_map[GroundGeom] = "Terrain"; - - d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); - RegionTerrain.Add(pOffset, GroundGeom); - TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); - TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); - } - } - } - - public override void DeleteTerrain() - { - } - - public float GetWaterLevel() - { - return waterlevel; - } - - public override bool SupportsCombining() - { - return m_suportCombine; - } - - public override void SetWaterLevel(float baseheight) - { - waterlevel = baseheight; - } - - public override void Dispose() - { - if (m_meshWorker != null) - m_meshWorker.Stop(); - - lock (OdeLock) - { - m_rayCastManager.Dispose(); - m_rayCastManager = null; - - lock (_prims) - { - ChangesQueue.Clear(); - foreach (OdePrim prm in _prims) - { - prm.DoAChange(changes.Remove, null); - _collisionEventPrim.Remove(prm); - } - _prims.Clear(); - } - - OdeCharacter[] chtorem; - lock (_characters) - { - chtorem = new OdeCharacter[_characters.Count]; - _characters.CopyTo(chtorem); - } - - ChangesQueue.Clear(); - foreach (OdeCharacter ch in chtorem) - ch.DoAChange(changes.Remove, null); - - - foreach (IntPtr GroundGeom in RegionTerrain.Values) - { - if (GroundGeom != IntPtr.Zero) - d.GeomDestroy(GroundGeom); - } - - - RegionTerrain.Clear(); - - if (TerrainHeightFieldHeightsHandlers.Count > 0) - { - foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) - { - if (gch.IsAllocated) - gch.Free(); - } - } - - TerrainHeightFieldHeightsHandlers.Clear(); - TerrainHeightFieldHeights.Clear(); - - if (ContactgeomsArray != IntPtr.Zero) - Marshal.FreeHGlobal(ContactgeomsArray); - if (GlobalContactsArray != IntPtr.Zero) - Marshal.FreeHGlobal(GlobalContactsArray); - - - d.WorldDestroy(world); - world = IntPtr.Zero; - //d.CloseODE(); - } - } - - public override Dictionary GetTopColliders() - { - Dictionary returncolliders = new Dictionary(); - int cnt = 0; - lock (_prims) - { - foreach (OdePrim prm in _prims) - { - if (prm.CollisionScore > 0) - { - returncolliders.Add(prm.LocalID, prm.CollisionScore); - cnt++; - prm.CollisionScore = 0f; - if (cnt > 25) - { - break; - } - } - } - } - return returncolliders; - } - - public override bool SupportsRayCast() - { - return true; - } - - public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) - { - if (retMethod != null) - { - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = 0; - req.filter = RayFilterFlags.AllPrims; - - m_rayCastManager.QueueRequest(req); - } - } - - public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) - { - if (retMethod != null) - { - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = Count; - req.filter = RayFilterFlags.AllPrims; - - m_rayCastManager.QueueRequest(req); - } - } - - - public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) - { - List ourresults = new List(); - object SyncObject = new object(); - - RayCallback retMethod = delegate(List results) - { - lock (SyncObject) - { - ourresults = results; - Monitor.PulseAll(SyncObject); - } - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = Count; - req.filter = RayFilterFlags.AllPrims; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return null; - else - return ourresults; - } - } - - public override bool SupportsRaycastWorldFiltered() - { - return true; - } - - public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) - { - object SyncObject = new object(); - List ourresults = new List(); - - RayCallback retMethod = delegate(List results) - { - lock (SyncObject) - { - ourresults = results; - Monitor.PulseAll(SyncObject); - } - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = Count; - req.filter = filter; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return null; - else - return ourresults; - } - } - - public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) - { - if (actor == null) - return new List(); - - IntPtr geom; - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - else - return new List(); - - if (geom == IntPtr.Zero) - return new List(); - - List ourResults = null; - object SyncObject = new object(); - - RayCallback retMethod = delegate(List results) - { - lock (SyncObject) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - } - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = actor; - req.callbackMethod = retMethod; - req.length = length; - req.Normal = direction; - req.Origin = position; - req.Count = Count; - req.filter = flags; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) - { - List ourResults = null; - object SyncObject = new object(); - - ProbeBoxCallback retMethod = delegate(List results) - { - lock (SyncObject) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - } - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.Normal = size; - req.Origin = position; - req.orientation = orientation; - req.Count = Count; - req.filter = flags; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) - { - List ourResults = null; - object SyncObject = new object(); - - ProbeSphereCallback retMethod = delegate(List results) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = radius; - req.Origin = position; - req.Count = Count; - req.filter = flags; - - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) - { - IntPtr geom = IntPtr.Zero;; - - if (actor != null) - { - if (actor is OdePrim) - geom = ((OdePrim)actor).prim_geom; - else if (actor is OdeCharacter) - geom = ((OdePrim)actor).prim_geom; - } - - List ourResults = null; - object SyncObject = new object(); - - ProbePlaneCallback retMethod = delegate(List results) - { - ourResults = results; - Monitor.PulseAll(SyncObject); - }; - - ODERayRequest req = new ODERayRequest(); - req.actor = null; - req.callbackMethod = retMethod; - req.length = plane.W; - req.Normal.X = plane.X; - req.Normal.Y = plane.Y; - req.Normal.Z = plane.Z; - req.Count = Count; - req.filter = flags; - - lock (SyncObject) - { - m_rayCastManager.QueueRequest(req); - if (!Monitor.Wait(SyncObject, 500)) - return new List(); - } - - if (ourResults == null) - return new List(); - return ourResults; - } - - public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) - { - Util.FireAndForget( delegate - { - ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); - if(sitAvatar != null) - sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); - }); - return 1; - } - - } -} diff --git a/OpenSim/Region/PhysicsModules/UbitOde/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/UbitOde/Properties/AssemblyInfo.cs deleted file mode 100644 index 01c8470..0000000 --- a/OpenSim/Region/PhysicsModules/UbitOde/Properties/AssemblyInfo.cs +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * 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. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Reflection; -using System.Runtime.InteropServices; -using Mono.Addins; - -// Information about this assembly is defined by the following -// attributes. -// -// change them to the information which is associated with the assembly -// you compile. - -[assembly : AssemblyTitle("OpenSim.Region.PhysicsModule.UbitOde")] -[assembly : AssemblyDescription("Ubit Variation")] -[assembly : AssemblyConfiguration("")] -[assembly : AssemblyCompany("http://opensimulator.org")] -[assembly : AssemblyProduct("UbitOde")] -[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] -[assembly : AssemblyTrademark("")] -[assembly : AssemblyCulture("")] - -// This sets the default COM visibility of types in the assembly to invisible. -// If you need to expose a type to COM, use [ComVisible(true)] on that type. - -[assembly : ComVisible(false)] - -// The assembly version has following format : -// -// Major.Minor.Build.Revision -// -// You can specify all values by your own or you can build default build and revision -// numbers with the '*' character (the default): - -[assembly : AssemblyVersion("0.8.2.*")] -[assembly: Addin("OpenSim.Region.PhysicsModule.UbitOde", OpenSim.VersionInfo.VersionNumber)] -[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs new file mode 100644 index 0000000..2ebd964 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -0,0 +1,2024 @@ +/* + * based on: + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +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. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of ODE's copyright owner 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. + * + * changes by opensim team; + * changes by Aurora team http://www.aurora-sim.org/ + * changes by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; +using OMV = OpenMetaverse; +namespace OdeAPI +{ +//#if dDOUBLE +// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim +// at least we save same memory and memory access time, FPU performance on intel usually is similar +// using dReal = System.Double; +//#else + using dReal = System.Single; +//#endif + + public static class d + { + public static dReal Infinity = dReal.MaxValue; + public static int NTotalBodies = 0; + public static int NTotalGeoms = 0; + + public const uint CONTACTS_UNIMPORTANT = 0x80000000; + + #region Flags and Enumerations + + [Flags] + public enum AllocateODEDataFlags : uint + { + BasicData = 0, + CollisionData = 0x00000001, + All = ~0u + } + + [Flags] + public enum IniteODEFlags : uint + { + dInitFlagManualThreadCleanup = 0x00000001 + } + + [Flags] + public enum ContactFlags : int + { + Mu2 = 0x001, + FDir1 = 0x002, + Bounce = 0x004, + SoftERP = 0x008, + SoftCFM = 0x010, + Motion1 = 0x020, + Motion2 = 0x040, + MotionN = 0x080, + Slip1 = 0x100, + Slip2 = 0x200, + Approx0 = 0x0000, + Approx1_1 = 0x1000, + Approx1_2 = 0x2000, + Approx1 = 0x3000 + } + + public enum GeomClassID : int + { + SphereClass, + BoxClass, + CapsuleClass, + CylinderClass, + PlaneClass, + RayClass, + ConvexClass, + GeomTransformClass, + TriMeshClass, + HeightfieldClass, + FirstSpaceClass, + SimpleSpaceClass = FirstSpaceClass, + HashSpaceClass, + QuadTreeSpaceClass, + LastSpaceClass = QuadTreeSpaceClass, + ubtTerrainClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 5 + } + + public enum JointType : int + { + None, + Ball, + Hinge, + Slider, + Contact, + Universal, + Hinge2, + Fixed, + Null, + AMotor, + LMotor, + Plane2D + } + + public enum JointParam : int + { + LoStop, + HiStop, + Vel, + FMax, + FudgeFactor, + Bounce, + CFM, + StopERP, + StopCFM, + SuspensionERP, + SuspensionCFM, + LoStop2 = 256, + HiStop2, + Vel2, + FMax2, + FudgeFactor2, + Bounce2, + CFM2, + StopERP2, + StopCFM2, + SuspensionERP2, + SuspensionCFM2, + LoStop3 = 512, + HiStop3, + Vel3, + FMax3, + FudgeFactor3, + Bounce3, + CFM3, + StopERP3, + StopCFM3, + SuspensionERP3, + SuspensionCFM3 + } + + public enum dSweepAndPruneAxis : int + { + XYZ = ((0)|(1<<2)|(2<<4)), + XZY = ((0)|(2<<2)|(1<<4)), + YXZ = ((1)|(0<<2)|(2<<4)), + YZX = ((1)|(2<<2)|(0<<4)), + ZXY = ((2)|(0<<2)|(1<<4)), + ZYX = ((2)|(1<<2)|(0<<4)) + } + + #endregion + + #region Callbacks + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GetAABBFn(IntPtr geom, out AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate ColliderFn GetColliderFnFn(int num); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GeomDtorFn(IntPtr o); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal ubTerrainGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); + + #endregion + + #region Structs + + [StructLayout(LayoutKind.Sequential)] + public struct AABB + { + public dReal MinX, MaxX; + public dReal MinY, MaxY; + public dReal MinZ, MaxZ; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Contact + { + public SurfaceParameters surface; + public ContactGeom geom; + public Vector3 fdir1; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); + } + + + [StructLayout(LayoutKind.Sequential)] + public struct ContactGeom + { + + public Vector3 pos; + public Vector3 normal; + public dReal depth; + public IntPtr g1; + public IntPtr g2; + public int side1; + public int side2; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); + } + + [StructLayout(LayoutKind.Sequential)] + public struct GeomClass + { + public int bytes; + public GetColliderFnFn collider; + public GetAABBFn aabb; + public AABBTestFn aabb_test; + public GeomDtorFn dtor; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct JointFeedback + { + public Vector3 f1; + public Vector3 t1; + public Vector3 f2; + public Vector3 t2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Mass + { + public dReal mass; + public Vector4 c; + public Matrix3 I; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3 + { + public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) + { + M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + } + public dReal M00, M10, M20; + private dReal _m30; + public dReal M01, M11, M21; + private dReal _m31; + public dReal M02, M12, M22; + private dReal _m32; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, + dReal m01, dReal m11, dReal m21, dReal m31, + dReal m02, dReal m12, dReal m22, dReal m32, + dReal m03, dReal m13, dReal m23, dReal m33) + { + M00 = m00; M10 = m10; M20 = m20; M30 = m30; + M01 = m01; M11 = m11; M21 = m21; M31 = m31; + M02 = m02; M12 = m12; M22 = m22; M32 = m32; + M03 = m03; M13 = m13; M23 = m23; M33 = m33; + } + public dReal M00, M10, M20, M30; + public dReal M01, M11, M21, M31; + public dReal M02, M12, M22, M32; + public dReal M03, M13, M23, M33; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public dReal W, X, Y, Z; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct SurfaceParameters + { + public ContactFlags mode; + public dReal mu; + public dReal mu2; + public dReal bounce; + public dReal bounce_vel; + public dReal soft_erp; + public dReal soft_cfm; + public dReal motion1; + public dReal motion2; + public dReal motionN; + public dReal slip1; + public dReal slip2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public Vector3(dReal x, dReal y, dReal z) + { + X = x; Y = y; Z = z; _w = 0.0f; + } + public dReal X, Y, Z; + private dReal _w; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 + { + public Vector4(dReal x, dReal y, dReal z, dReal w) + { + X = x; Y = y; Z = z; W = w; + } + public dReal X, Y, Z, W; + } + + #endregion + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern int AllocateODEDataForThread(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnected(IntPtr b1, IntPtr b2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyiCreate(IntPtr world); + public static IntPtr BodyCreate(IntPtr world) + { + NTotalBodies++; + return BodyiCreate(world); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void BodyiDestroy(IntPtr body); + public static void BodyDestroy(IntPtr body) + { + NTotalBodies--; + BodyiDestroy(body); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDisable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyEnable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetAutoDisableFlag(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetAutoDisableSteps(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableTime(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + public static Vector3 BodyGetAngularVel(IntPtr body) + { + unsafe { return *(BodyGetAngularVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetData(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetFiniteRotationMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); + public static Vector3 BodyGetForce(IntPtr body) + { + unsafe { return *(BodyGetForceUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetGravityMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetGyroscopicMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetJoint(IntPtr body, int index); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + public static Vector3 BodyGetLinearVel(IntPtr body) + { + unsafe { return *(BodyGetLinearVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetMass(IntPtr body, out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetNumJoints(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + public static Vector3 BodyGetPosition(IntPtr body) + { + unsafe { return *(BodyGetPositionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + public static Quaternion BodyGetQuaternion(IntPtr body) + { + unsafe { return *(BodyGetQuaternionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); + public static Matrix3 BodyGetRotation(IntPtr body) + { + unsafe { return *(BodyGetRotationUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); + public static Vector3 BodyGetTorque(IntPtr body) + { + unsafe { return *(BodyGetTorqueUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetWorld(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetFirstGeom(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyIsEnabled(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetData(IntPtr body, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetGravityMode(IntPtr body, bool mode); + + /// + /// Sets the Gyroscopic term status on the body specified. + /// + /// Pointer to body + /// NonZero enabled, Zero disabled + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetMass(IntPtr body, ref Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 p2, + ref Matrix3 R2, ref Vector3 side2, + ref Vector3 normal, out dReal depth, out int return_code, + int maxc, out ContactGeom contact, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 _p2, + ref Matrix3 R2, ref Vector3 side2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern void CleanupODEAllDataForThread(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] + public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, + ref Vector3 b1, ref Vector3 b2, + ref Vector3 cp1, ref Vector3 cp2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] + public static extern void CloseODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); + public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) + { + NTotalGeoms++; + return CreateiBox(space, lx, ly, lz); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCapsule(space, radius, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) + { + NTotalGeoms++; + return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCylinder(space, radius, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiHeightfield(space, data, bPlaceable); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateubTerrain"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiubTerrain(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateubTerrain(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiubTerrain(space, data, bPlaceable); + } + + + + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiGeom(int classnum); + public static IntPtr CreateGeom(int classnum) + { + NTotalGeoms++; + return CreateiGeom(classnum); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] + public static extern int CreateGeomClass(ref GeomClass classptr); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeomTransform(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) + { + NTotalGeoms++; + return CreateiPlane(space, a, b, c, d); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiRay(IntPtr space, dReal length); + public static IntPtr CreateRay(IntPtr space, dReal length) + { + NTotalGeoms++; + return CreateiRay(space, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); + public static IntPtr CreateSphere(IntPtr space, dReal radius) + { + NTotalGeoms++; + return CreateiSphere(space, radius); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); + public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) + { + NTotalGeoms++; + return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] + public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] + public static extern int FactorCholesky(ref dReal A00, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] + public static extern void GeomClearOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomiDestroy(IntPtr geom); + public static void GeomDestroy(IntPtr geom) + { + NTotalGeoms--; + GeomiDestroy(geom); + } + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDisable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomEnable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out dReal minX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetBody(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern uint GeomGetCategoryBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetClassData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern uint GeomGetCollideBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] + public static extern GeomClassID GeomGetClass(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetOffsetPosition(IntPtr geom) + { + unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetOffsetRotation(IntPtr geom) + { + unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetPosition(IntPtr geom) + { + unsafe { return *(GeomGetPositionUnsafe(geom)); } + } + public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) + { + Vector3 vtmp = GeomGetPosition(geom); + return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) + { + Quaternion qtmp; + GeomCopyQuaternion(geom, out qtmp); + return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetRotation(IntPtr geom) + { + unsafe { return *(GeomGetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomubTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomubTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomubTerrainDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomubTerrainDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomubTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomubTerrainGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomubTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomubTerrainSetHeightfieldData(IntPtr g, IntPtr d); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsEnabled(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern int GeomRayGetClosestHit(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetLength(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetLength(IntPtr ray, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetBody(IntPtr geom, IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCategoryBits(IntPtr geom, uint bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, uint bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSphereGetRadius(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetCleanup(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTransformGetGeom(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetInfo(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetInfo(IntPtr geom, int info); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + double[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + dReal[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + float[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshClearTCCache(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataPreprocess(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataUpdate(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); + public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) + { + unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] + public extern static int GeomTriMeshGetTriangleCount(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr iGetConfiguration(); + + public static string GetConfiguration() + { + IntPtr ptr = iGetConfiguration(); + string s = Marshal.PtrToStringAnsi(ptr); + return s; + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr HashSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] + public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] + public static extern void InitODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] + public static extern int InitODE2(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] + public static extern int IsPositiveDefinite(ref dReal A, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] + public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddPRTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddSliderForce(IntPtr joint, dReal force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] + public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointDestroy(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorMode(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetBody(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetData(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] + public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); + public static JointFeedback JointGetFeedback(IntPtr j) + { + unsafe { return *(JointGetFeedbackUnsafe(j)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngle(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngleRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetLMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] + public static extern JointType JointGetType(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGroupCreate(int max_size); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupDestroy(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupEmpty(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorMode(IntPtr j, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorNumAxes(IntPtr group, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetData(IntPtr j, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFixed(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorNumAxes(IntPtr j, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] + public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdd(ref Mass a, ref Mass b); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdjust(ref Mass m, dReal newmass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] + public static extern bool MassCheck(ref Mass m); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetParameters(out Mass mass, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal i11, dReal i22, dReal i33, + dReal i12, dReal i13, dReal i23); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] + public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetZero(out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] + public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); + public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) + { + MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] + public static extern void QfromR(out Quaternion q, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void QSetIdentity(out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] + public static extern dReal RandReal(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] + public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] + public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + public static extern void RfromQ(out Matrix3 R, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] + public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void RSetIdentity(out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] + public static extern void SetValue(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void SetZero(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SimpleSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] + public static extern void SolveCholesky(ref dReal L, out dReal b, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceAdd(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceLockQuery(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceClean(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceDestroy(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceGetCleanup(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetNumGeoms(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SpaceGetGeom(IntPtr space, int i); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetSublevel(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceQuery(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceRemove(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetCleanup(IntPtr space, bool mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetSublevel(IntPtr space, int sublevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] + public static extern void VectorScale(out dReal a, ref dReal d, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr WorldCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void WorldDestroy(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool WorldGetAutoDisableFlag(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableSteps(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableTime(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetCFM(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetERP(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetQuickStepNumIterations(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetQuickStepW(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldQuickStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetCFM(IntPtr world, dReal cfm); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetERP(IntPtr world, dReal erp); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] + public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs new file mode 100644 index 0000000..cc37ef5 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -0,0 +1,1847 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ + + +// Revision by Ubit 2011/12 + +using System; +using System.Collections.Generic; +using System.Reflection; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using log4net; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public enum dParam : int + { + LowStop = 0, + HiStop = 1, + Vel = 2, + FMax = 3, + FudgeFactor = 4, + Bounce = 5, + CFM = 6, + StopERP = 7, + StopCFM = 8, + LoStop2 = 256, + HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, + StopERP2 = 7 + 256, + StopCFM2 = 8 + 256, + LoStop3 = 512, + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515, + StopERP3 = 7 + 512, + StopCFM3 = 8 + 512 + } + + public class OdeCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private Vector3 _zeroPosition; + private Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private Vector3 m_size; + private Vector3 m_collideNormal; + private Quaternion m_orientation; + private Quaternion m_orientation2D; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + + const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) + const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) + public float PID_D; + public float PID_P; + + private float timeStep; + private float invtimeStep; + + private float m_feetOffset = 0; + private float feetOff = 0; + private float boneOff = 0; + private float AvaAvaSizeXsq = 0.3f; + private float AvaAvaSizeYsq = 0.2f; + + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + + private bool _zeroFlag = false; + + + private uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public float MinimumGroundFlightOffset = 3f; + + private float m_buoyancy = 0f; + + private bool m_freemove = false; + // private CollisionLocker ode; + +// private string m_name = String.Empty; + // other filter control + int m_colliderfilter = 0; + int m_colliderGroundfilter = 0; + int m_colliderObjectfilter = 0; + + // Default we're a Character + private CollisionCategories m_collisionCategories = (CollisionCategories.Character); + + // Default, Collide with Other Geometries, spaces, bodies and characters. + private CollisionCategories m_collisionFlags = (CollisionCategories.Character + | CollisionCategories.Geom + | CollisionCategories.VolumeDtc + ); + // we do land collisions not ode | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private ODEScene _parent_scene; + private IntPtr capsule = IntPtr.Zero; + public IntPtr collider = IntPtr.Zero; + + public IntPtr Amotor = IntPtr.Zero; + + public d.Mass ShellMass; + + public int m_eventsubscription = 0; + private int m_cureventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + float mu; + + public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + m_localID = localID; + + timeStep = parent_scene.ODE_STEPSIZE; + invtimeStep = 1 / timeStep; + + if (pos.IsFinite()) + { + if (pos.Z > 99999f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (pos.Z < -100f) // shouldn't this be 0 ? + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + _position = pos; + } + else + { + _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + _parent_scene = parent_scene; + + + m_size.X = pSize.X; + m_size.Y = pSize.Y; + m_size.Z = pSize.Z; + + if(m_size.X <0.01f) + m_size.X = 0.01f; + if(m_size.Y <0.01f) + m_size.Y = 0.01f; + if(m_size.Z <0.01f) + m_size.Z = 0.01f; + + m_feetOffset = pfeetOffset; + m_orientation = Quaternion.Identity; + m_orientation2D = Quaternion.Identity; + m_density = density; + + // force lower density for testing + m_density = 3.0f; + + mu = parent_scene.AvatarFriction; + + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default + + PID_D = basePID_D * m_mass * invtimeStep; + PID_P = basePID_P * m_mass * invtimeStep; + + m_isPhysical = false; // current status: no ODE information exists + + Name = avName; + + AddChange(changes.Add, null); + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = 0; + cdata.softcolide = false; + } + + public override bool Building { get; set; } + + /// + /// If this is set, the avatar will move faster + /// + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + public override uint LocalID + { + get { return m_localID; } + set { m_localID = value; } + } + + public override PhysicsActor ParentActor + { + get { return (PhysicsActor)this; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override bool FloatOnWater + { + set { return; } + } + + public override bool IsPhysical + { + get { return m_isPhysical; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return flying; } + set + { + flying = value; +// m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); + } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return (m_iscolliding || m_iscollidingGround); } + set + { + if (value) + { + m_colliderfilter += 3; + if (m_colliderfilter > 3) + m_colliderfilter = 3; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + { + m_pidControllerActive = true; + m_iscolliding = true; + m_freemove = false; + } + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { +/* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } + + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ + } + + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + // Ubit filter this also + if (value) + { + m_colliderObjectfilter += 2; + if (m_colliderObjectfilter > 2) + m_colliderObjectfilter = 2; + } + else + { + m_colliderObjectfilter--; + if (m_colliderObjectfilter < 0) + m_colliderObjectfilter = 0; + } + + if (m_colliderObjectfilter == 0) + m_iscollidingObj = false; + else + m_iscollidingObj = true; + +// m_iscollidingObj = value; + + if (m_iscollidingObj) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + /// + /// This 'puts' an avatar somewhere in the physics space. + /// Not really a good choice unless you 'know' it's a good + /// spot otherwise you're likely to orbit the avatar. + /// + public override Vector3 Position + { + get { return _position; } + set + { + if (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -100f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + AddChange(changes.Position, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + } + } + } + + public override Vector3 RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + /// + /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight + /// and use it to offset landings properly + /// + public override Vector3 Size + { + get + { + return m_size; + } + set + { + if (value.IsFinite()) + { + if(value.X <0.01f) + value.X = 0.01f; + if(value.Y <0.01f) + value.Y = 0.01f; + if(value.Z <0.01f) + value.Z = 0.01f; + + AddChange(changes.Size, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + public override void setAvatarSize(Vector3 size, float feetOffset) + { + if (size.IsFinite()) + { + if (size.X < 0.01f) + size.X = 0.01f; + if (size.Y < 0.01f) + size.Y = 0.01f; + if (size.Z < 0.01f) + size.Z = 0.01f; + + strAvatarSize st = new strAvatarSize(); + st.size = size; + st.offset = feetOffset; + AddChange(changes.AvatarSize, st); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character"); + } + + } + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // + /// + /// Uses the capped cyllinder volume formula to calculate the avatar's mass. + /// This may be used in calculations in the scene/scenepresence + /// + public override float Mass + { + get + { + return m_mass; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(Vector3 axis) + { + + } + + + public override Vector3 Force + { + get { return _target_velocity; } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void VehicleFlags(int param, bool remove) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override Vector3 CenterOfMass + { + get + { + Vector3 pos = _position; + return pos; + } + } + + public override Vector3 GeometricCenter + { + get + { + Vector3 pos = _position; + return pos; + } + } + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 Velocity + { + get + { + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); + } + } + } + + public override Vector3 Torque + { + get { return Vector3.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return m_orientation; } + set + { +// fakeori = value; +// givefakeori++; + value.Normalize(); + AddChange(changes.Orientation, value); + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public void SetAcceleration(Vector3 accel) + { + m_pidControllerActive = true; + _acceleration = accel; + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if (pushforce) + { + AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f)); + } + else + { + AddChange(changes.Velocity, force); + } + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); + } + //m_lastUpdateSent = false; + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + + } + + public override void SetMomentum(Vector3 momentum) + { + if (momentum.IsFinite()) + AddChange(changes.Momentum, momentum); + } + + + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + // sizes one day should came from visual parameters + float sx = m_size.X; + float sy = m_size.Y; + float sz = m_size.Z; + + float bot = -sz * 0.5f + m_feetOffset; + boneOff = bot + 0.3f; + + float feetsz = sz * 0.45f; + if (feetsz > 0.6f) + feetsz = 0.6f; + + feetOff = bot + feetsz; + + AvaAvaSizeXsq = 0.4f * sx; + AvaAvaSizeXsq *= AvaAvaSizeXsq; + AvaAvaSizeYsq = 0.5f * sy; + AvaAvaSizeYsq *= AvaAvaSizeYsq; + + _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); + + collider = d.HashSpaceCreate(_parent_scene.CharsSpace); + d.HashSpaceSetLevels(collider, -4, 3); + d.SpaceSetSublevel(collider, 3); + d.SpaceSetCleanup(collider, false); + d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); + d.GeomSetCollideBits(collider, (uint)m_collisionFlags); + + float r = m_size.X; + if (m_size.Y > r) + r = m_size.Y; + float l = m_size.Z - r; + r *= 0.5f; + + capsule = d.CreateCapsule(collider, r, l); + + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass + + d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); + + PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE; + PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + _zeroFlag = false; + m_pidControllerActive = true; + m_freemove = false; + + _velocity = Vector3.Zero; + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(capsule, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geoms + if (capsule != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(capsule); + _parent_scene.waitForSpaceUnlock(collider); + d.GeomDestroy(capsule); + capsule = IntPtr.Zero; + } + + if (collider != IntPtr.Zero) + { + d.SpaceDestroy(collider); + collider = IntPtr.Zero; + } + + } + + //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z + public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos + y * sin; + y = -tx * sin + y * cos; + } + + public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = - 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + + public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, + ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) + { + feetcollision = false; + useAltcontact = false; + + if (me == capsule) + { + Vector3 offset; + + float h = contact.pos.Z - _position.Z; + offset.Z = h - feetOff; + + offset.X = contact.pos.X - _position.X; + offset.Y = contact.pos.Y - _position.Y; + + d.GeomClassID gtype = d.GeomGetClass(other); + if (gtype == d.GeomClassID.CapsuleClass) + { + Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); + float r = roff.X *roff.X / AvaAvaSizeXsq; + r += (roff.Y * roff.Y) / AvaAvaSizeYsq; + if (r > 1.0f) + return false; + + float dp = 1.0f -(float)Math.Sqrt((double)r); + if (dp > 0.05f) + dp = 0.05f; + + contact.depth = dp; + + if (offset.Z < 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + } + return true; + } +/* + d.AABB aabb; + d.GeomGetAABB(other,out aabb); + float othertop = aabb.MaxZ - _position.Z; +*/ +// if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) + if (offset.Z > 0 || contact.normal.Z > 0.35f) + { + if (offset.Z <= 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + } + return true; + } + + altContact = contact; + useAltcontact = true; + + offset.Z -= 0.2f; + + offset.Normalize(); + + if (contact.depth > 0.1f) + contact.depth = 0.1f; + + if (reverse) + { + altContact.normal.X = offset.X; + altContact.normal.Y = offset.Y; + altContact.normal.Z = offset.Z; + } + else + { + altContact.normal.X = -offset.X; + altContact.normal.Y = -offset.Y; + altContact.normal.Z = -offset.Z; + } + + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + return true; + } + return false; + } + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(List defects) + { + if (Body == IntPtr.Zero) + return; + + d.Vector3 dtmp = d.BodyGetPosition(Body); + Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + // the Amotor still lets avatar rotation to drift during colisions + // so force it back to identity + + d.Quaternion qtmp; + qtmp.W = m_orientation2D.W; + qtmp.X = m_orientation2D.X; + qtmp.Y = m_orientation2D.Y; + qtmp.Z = m_orientation2D.Z; + d.BodySetQuaternion(Body, ref qtmp); + + if (m_pidControllerActive == false) + { + _zeroPosition = localpos; + } + + if (!localpos.IsFinite()) + { + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + return; + } + + // check outbounds forcing to be in world + bool fixbody = false; + if (localpos.X < 0.0f) + { + fixbody = true; + localpos.X = 0.1f; + } + else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + localpos.X = _parent_scene.WorldExtents.X - 0.1f; + } + if (localpos.Y < 0.0f) + { + fixbody = true; + localpos.Y = 0.1f; + } + else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; + } + if (fixbody) + { + m_freemove = false; + d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); + } + + float breakfactor; + + Vector3 vec = Vector3.Zero; + dtmp = d.BodyGetLinearVel(Body); + Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + float velLengthSquared = vel.LengthSquared(); + + + Vector3 ctz = _target_velocity; + + float movementdivisor = 1f; + //Ubit change divisions into multiplications below + if (!m_alwaysRun) + movementdivisor = 1 / walkDivisor; + else + movementdivisor = 1 / runDivisor; + + ctz.X *= movementdivisor; + ctz.Y *= movementdivisor; + + //****************************************** + // colide with land + + d.AABB aabb; +// d.GeomGetAABB(feetbox, out aabb); + d.GeomGetAABB(capsule, out aabb); + float chrminZ = aabb.MinZ; // move up a bit + Vector3 posch = localpos; + + float ftmp; + + if (flying) + { + ftmp = timeStep; + posch.X += vel.X * ftmp; + posch.Y += vel.Y * ftmp; + } + + float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + if (chrminZ < terrainheight) + { + if (ctz.Z < 0) + ctz.Z = 0; + + Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); + float depth = terrainheight - chrminZ; + + vec.Z = depth * PID_P * 50; + + if (!flying) + vec.Z += -vel.Z * PID_D; + + if (depth < 0.2f) + { + m_colliderGroundfilter++; + if (m_colliderGroundfilter > 2) + { + m_iscolliding = true; + m_colliderfilter = 2; + + if (m_colliderGroundfilter > 10) + { + m_colliderGroundfilter = 10; + m_freemove = false; + } + + m_collideNormal.X = n.X; + m_collideNormal.Y = n.Y; + m_collideNormal.Z = n.Z; + + m_iscollidingGround = true; + + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = terrainheight; + contact.SurfaceNormal.X = -n.X; + contact.SurfaceNormal.Y = -n.Y; + contact.SurfaceNormal.Z = -n.Z; + contact.RelativeSpeed = -vel.Z; + contact.CharacterFeet = true; + AddCollisionEvent(0, contact); + +// vec.Z *= 0.5f; + } + } + + else + { + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } + } + } + else + { + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } + } + + + //****************************************** + if (!m_iscolliding) + m_collideNormal.Z = 0; + + bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); + + + + if (!tviszero) + { + m_freemove = false; + + // movement relative to surface if moving on it + // dont disturbe vertical movement, ie jumps + if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) + { + float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; + ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); + ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); + ctz.Z -= p; + if (ctz.Z < 0) + ctz.Z *= 2; + + } + + } + + + if (!m_freemove) + { + + // if velocity is zero, use position control; otherwise, velocity control + if (tviszero && m_iscolliding && !flying) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = localpos; + } + if (m_pidControllerActive) + { + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); + vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); + if(vel.Z > 0) + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + else + vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; +/* + if (flying) + { + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + } +*/ + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + _zeroFlag = false; + + if (m_iscolliding) + { + if (!flying) + { + // we are on a surface + if (ctz.Z > 0f) + { + // moving up or JUMPING + vec.Z += (ctz.Z - vel.Z) * PID_D * 2f; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + else + { + // we are moving down on a surface + if (ctz.Z == 0) + { + if (vel.Z > 0) + vec.Z -= vel.Z * PID_D * 2f; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + // intencionally going down + else + { + if (ctz.Z < vel.Z) + vec.Z += (ctz.Z - vel.Z) * PID_D; + else + { + } + + if (Math.Abs(ctz.X) > Math.Abs(vel.X)) + vec.X += (ctz.X - vel.X) * (PID_D); + if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + } + + // We're standing on something + } + else + { + // We're flying and colliding with something + vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); + vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); + vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); + } + } + else // ie not colliding + { + if (flying) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + vec.Z += (ctz.Z - vel.Z) * (PID_D); + } + + else + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + vec.X += (ctz.X - vel.X) * PID_D * 0.833f; + vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; + // hack for breaking on fall + if (ctz.Z == -9999f) + vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass; + } + } + } + + if (velLengthSquared > 2500.0f) // 50m/s apply breaks + { + breakfactor = 0.16f * m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + vec.Z -= breakfactor * vel.Z; + } + } + else + { + breakfactor = m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + if (flying) + vec.Z -= 0.5f * breakfactor * vel.Z; + else + vec.Z -= .16f* m_mass * vel.Z; + } + + if (flying) + { + vec.Z -= _parent_scene.gravityz * m_mass; + + //Added for auto fly height. Kitto Flora + float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; + + if (localpos.Z < target_altitude) + { + vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; + } + // end add Kitto Flora + } + + if (vec.IsFinite()) + { + if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) + d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + return; + } + + // update our local ideia of position velocity and aceleration + // _position = localpos; + _position = localpos; + + if (_zeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } + else + { + Vector3 a =_velocity; // previus velocity + SetSmooth(ref _velocity, ref vel, 2); + a = (_velocity - a) * invtimeStep; + SetSmooth(ref _acceleration, ref a, 2); + + dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = 0f; + m_rotationalVelocity.Y = 0f; + m_rotationalVelocity.Z = dtmp.Z; + Math.Round(m_rotationalVelocity.Z,3); + } + } + + public void round(ref Vector3 v, int digits) + { + v.X = (float)Math.Round(v.X, digits); + v.Y = (float)Math.Round(v.Y, digits); + v.Z = (float)Math.Round(v.Z, digits); + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value) + { + dst.X = 0.1f * dst.X + 0.9f * value.X; + dst.Y = 0.1f * dst.Y + 0.9f * value.Y; + dst.Z = 0.1f * dst.Z + 0.9f * value.Z; + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) + { + dst.X = 0.4f * dst.X + 0.6f * value.X; + dst.X = (float)Math.Round(dst.X, rounddigits); + + dst.Y = 0.4f * dst.Y + 0.6f * value.Y; + dst.Y = (float)Math.Round(dst.Y, rounddigits); + + dst.Z = 0.4f * dst.Z + 0.6f * value.Z; + dst.Z = (float)Math.Round(dst.Z, rounddigits); + } + + + /// + /// Updates the reported position and velocity. + /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording + /// also outbounds checking + /// copy and outbounds now done in move(..) at ode rate + /// + /// + public void UpdatePositionAndVelocity() + { + return; + +// if (Body == IntPtr.Zero) +// return; + + } + + /// + /// Cleanup the things we use in the scene. + /// + public void Destroy() + { + AddChange(changes.Remove, null); + } + + public override void CrossingFailure() + { + } + + public override Vector3 PIDTarget { set { return; } } + public override bool PIDActive {get {return m_pidControllerActive;} set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight { set { return; } } + public override bool PIDHoverActive { set { return; } } + public override PIDHoverType PIDHoverType { set { return; } } + public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; + } + + public override void UnSubscribeEvents() + { + if (CollisionEventsThisFrame != null) + { + lock (CollisionEventsThisFrame) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } + } + m_eventsubscription = 0; + } + + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + lock (CollisionEventsThisFrame) + { + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + _parent_scene.AddCollisionEventReporting(this); + } + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + lock (CollisionEventsThisFrame) + { + if (m_cureventsubscription < m_eventsubscription) + return; + + m_cureventsubscription = 0; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); + } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + } + + internal void AddCollisionFrameTime(int t) + { + // protect it from overflow crashing + if (m_cureventsubscription < 50000) + m_cureventsubscription += t; + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + private void changePhysicsStatus(bool NewStatus) + { + if (NewStatus != m_isPhysical) + { + if (NewStatus) + { + AvatarGeomAndBodyDestroy(); + + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); + + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; + _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; + _parent_scene.AddCharacter(this); + } + else + { + _parent_scene.RemoveCollisionEventReporting(this); + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + m_freemove = false; + m_isPhysical = NewStatus; + } + } + + private void changeAdd() + { + changePhysicsStatus(true); + } + + private void changeRemove() + { + changePhysicsStatus(false); + } + + private void changeShape(PrimitiveBaseShape arg) + { + } + + private void changeAvatarSize(strAvatarSize st) + { + m_feetOffset = st.offset; + changeSize(st.size); + } + + private void changeSize(Vector3 pSize) + { + if (pSize.IsFinite()) + { + // for now only look to Z changes since viewers also don't change X and Y + if (pSize.Z != m_size.Z) + { + AvatarGeomAndBodyDestroy(); + + + float oldsz = m_size.Z; + m_size = pSize; + + + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (m_size.Z - oldsz) * 0.5f); + + Velocity = Vector3.Zero; + + + _parent_scene.actor_name_map[collider] = (PhysicsActor)this; + _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; + } + m_freemove = false; + m_pidControllerActive = true; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + + private void changePosition( Vector3 newPos) + { + if (Body != IntPtr.Zero) + d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + m_freemove = false; + m_pidControllerActive = true; + } + + private void changeOrientation(Quaternion newOri) + { + if (m_orientation != newOri) + { + m_orientation = newOri; // keep a copy for core use + // but only use rotations around Z + + m_orientation2D.W = newOri.W; + m_orientation2D.Z = newOri.Z; + + float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; + if (t > 0) + { + t = 1.0f / (float)Math.Sqrt(t); + m_orientation2D.W *= t; + m_orientation2D.Z *= t; + } + else + { + m_orientation2D.W = 1.0f; + m_orientation2D.Z = 0f; + } + m_orientation2D.Y = 0f; + m_orientation2D.X = 0f; + + d.Quaternion myrot = new d.Quaternion(); + myrot.X = m_orientation2D.X; + myrot.Y = m_orientation2D.Y; + myrot.Z = m_orientation2D.Z; + myrot.W = m_orientation2D.W; + d.BodySetQuaternion(Body, ref myrot); + } + } + + private void changeVelocity(Vector3 newVel) + { + m_pidControllerActive = true; + m_freemove = false; + _target_velocity = newVel; + } + + private void changeSetTorque(Vector3 newTorque) + { + } + + private void changeAddForce(Vector3 newForce) + { + } + + private void changeAddAngularForce(Vector3 arg) + { + } + + private void changeAngularLock(Vector3 arg) + { + } + + private void changeFloatOnWater(bool arg) + { + } + + private void changeVolumedetetion(bool arg) + { + } + + private void changeSelectedStatus(bool arg) + { + } + + private void changeDisable(bool arg) + { + } + + private void changeBuilding(bool arg) + { + } + + private void setFreeMove() + { + m_pidControllerActive = true; + _zeroFlag = false; + _target_velocity = Vector3.Zero; + m_freemove = true; + m_colliderfilter = -1; + m_colliderObjectfilter = -1; + m_colliderGroundfilter = -1; + + m_iscolliding = false; + m_iscollidingGround = false; + m_iscollidingObj = false; + + CollisionEventsThisFrame.Clear(); + } + + private void changeForce(Vector3 newForce) + { + setFreeMove(); + + if (Body != IntPtr.Zero) + { + if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) + d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); + } + } + + // for now momentum is actually velocity + private void changeMomentum(Vector3 newmomentum) + { + _velocity = newmomentum; + setFreeMove(); + + if (Body != IntPtr.Zero) + d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeAdd(); + break; + case changes.Remove: + changeRemove(); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changeVelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.AvatarSize: + changeAvatarSize((strAvatarSize)arg); + break; + + case changes.Momentum: + changeMomentum((Vector3)arg); + break; +/* not in use for now + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; +*/ + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor)this, what, arg); + } + + private struct strAvatarSize + { + public Vector3 size; + public float offset; + } + + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs new file mode 100644 index 0000000..c3b4dd8 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs @@ -0,0 +1,1096 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ + +/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +// Extensive change Ubit 2012 + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + private OdePrim rootPrim; + private ODEScene _pParentScene; + + // Vehicle properties + // WARNING this are working copies for internel use + // their values may not be the corresponding parameter + + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? + + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + private float m_linearMotorDecayTimescale = 120; + private float m_linearMotorTimescale = 1000; + private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate + + //Deflection properties + private float m_angularDeflectionEfficiency = 0; + private float m_angularDeflectionTimescale = 1000; + private float m_linearDeflectionEfficiency = 0; + private float m_linearDeflectionTimescale = 1000; + + //Banking properties + private float m_bankingEfficiency = 0; + private float m_bankingMix = 0; + private float m_bankingTimescale = 1000; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 1000f; + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. + + + // auxiliar + private float m_lmEfect = 0f; // current linear motor eficiency + private float m_lmDecay = 0f; // current linear decay + + private float m_amEfect = 0; // current angular motor eficiency + private float m_amDecay = 0f; // current linear decay + + private float m_ffactor = 1.0f; + + private float m_timestep = 0.02f; + private float m_invtimestep = 50; + + + float m_ampwr; + float m_amdampX; + float m_amdampY; + float m_amdampZ; + + float m_gravmod; + + public float FrictionFactor + { + get + { + return m_ffactor; + } + } + + public float GravMod + { + set + { + m_gravmod = value; + } + } + + + public ODEDynamics(OdePrim rootp) + { + rootPrim = rootp; + _pParentScene = rootPrim._parent_scene; + m_timestep = _pParentScene.ODE_STEPSIZE; + m_invtimestep = 1.0f / m_timestep; + m_gravmod = rootPrim.GravModifier; + } + + public void DoSetVehicle(VehicleData vd) + { + m_type = vd.m_type; + m_flags = vd.m_flags; + + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; + if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; + if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; + + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; + m_linearMotorDecayTimescale += 0.2f; + m_linearMotorDecayTimescale *= m_invtimestep; + + m_linearMotorTimescale = vd.m_linearMotorTimescale; + if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; + + m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; + + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; + m_angularMotorDecayTimescale *= m_invtimestep; + + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; + if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; + if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; + + //Deflection properties + m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; + m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; + if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; + + m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; + m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; + + //Banking properties + m_bankingEfficiency = vd.m_bankingEfficiency; + m_bankingMix = vd.m_bankingMix; + m_bankingTimescale = vd.m_bankingTimescale; + if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; + m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; + + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; + + // Axis + m_referenceFrame = vd.m_referenceFrame; + + m_lmEfect = 0; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_amEfect = 0; + m_ffactor = 1.0f; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + else if (pValue > 120) pValue = 120; + m_angularMotorDecayTimescale = pValue * m_invtimestep; + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + + m_amEfect = 1.0f ; // turn it on + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_linearMotorDirection.Length(); + if (len > 100.0f) + m_linearMotorDirection *= (100.0f / len); + + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + m_lmEfect = 1.0f; // turn it on + + m_ffactor = 0.0f; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_timestep; + + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + + m_amEfect = 1.0f; // turn it on + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_timestep; + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorDirection.Length(); + if (len > 100.0f) + m_linearMotorDirection *= (100.0f / len); + + m_lmEfect = 1.0f; // turn it on + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + + m_ffactor = 0.0f; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + case Vehicle.BLOCK_EXIT: + m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = Quaternion.Inverse(pValue); + m_referenceFrame = pValue; + break; + case Vehicle.ROLL_FRAME: + m_RollreferenceFrame = pValue; + break; + } + }//end ProcessRotationVehicleParam + + internal void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + internal void ProcessTypeChange(Vehicle pType) + { + m_lmEfect = 0; + + m_amEfect = 0; + m_ffactor = 1f; + + m_linearMotorDirection = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + + m_BlockingEndPoint = Vector3.Zero; + m_RollreferenceFrame = Quaternion.Identity; + m_linearMotorOffset = Vector3.Zero; + + m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * m_invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 1000; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 1000; + m_verticalAttractionEfficiency = 0; + m_verticalAttractionTimescale = 1000; + + m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * m_invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 1; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 10; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60 * m_invtimestep; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 2; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + m_bankingEfficiency = -0.2f; + m_bankingMix = 1; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * m_invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 3; + m_angularDeflectionEfficiency = 0.5f; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + m_bankingEfficiency = -0.3f; + m_bankingMix = 0.8f; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY); // | +// VehicleFlag.LIMIT_ROLL_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_UP_ONLY | // new sl + VehicleFlag.HOVER_WATER_ONLY); + break; + + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60 * m_invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 0.5f; + m_angularDeflectionEfficiency = 1; + m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + m_bankingEfficiency = 1; + m_bankingMix = 0.7f; + m_bankingTimescale = 2; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * m_invtimestep; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10 * m_invtimestep; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 5 * m_invtimestep; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 1000f; + m_bankingEfficiency = 0; + m_bankingMix = 0.7f; + m_bankingTimescale = 5; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | //); + VehicleFlag.LIMIT_ROLL_ONLY | // new sl + VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl + +// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | +// VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + }//end SetDefaultsForType + + internal void Stop() + { + m_lmEfect = 0; + m_lmDecay = 0f; + m_amEfect = 0; + m_amDecay = 0; + m_ffactor = 1f; + } + + public static Vector3 Xrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; + vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); + vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); + return vec; + } + + public static Vector3 Zrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); + vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); + vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; + + return vec; + } + + private const float pi = (float)Math.PI; + private const float halfpi = 0.5f * (float)Math.PI; + private const float twopi = 2.0f * pi; + + public static Vector3 ubRot2Euler(Quaternion rot) + { + // returns roll in X + // pitch in Y + // yaw in Z + Vector3 vec; + + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + vec.X = 0; + vec.Y = -halfpi; + vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); + } + else if (zX > 0.49999f) + { + vec.X = 0; + vec.Y = halfpi; + vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); + } + else + { + vec.Y = (float)Math.Asin(2 * zX); + + float sqw = rot.W * rot.W; + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + sqw - 0.5f; + + vec.X = (float)Math.Atan2(minuszY, zZ); + + float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) + float yY = rot.X * rot.X + sqw - 0.5f; + vec.Z = (float)Math.Atan2(yX, yY); + } + return vec; + } + + public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) + { + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + roll = 0; + pitch = -halfpi; + } + else if (zX > 0.49999f) + { + roll = 0; + pitch = halfpi; + } + else + { + pitch = (float)Math.Asin(2 * zX); + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; + + roll = (float)Math.Atan2(minuszY, zZ); + } + return ; + } + + internal void Step() + { + IntPtr Body = rootPrim.Body; + + d.Mass dmass; + d.BodyGetMass(Body, out dmass); + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion rotq = objrotq; // rotq = rotation of object + rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame + Quaternion irotq = Quaternion.Inverse(rotq); + + d.Vector3 dvtmp; + Vector3 tmpV; + Vector3 curVel; // velocity in world + Vector3 curAngVel; // angular velocity in world + Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame + Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame + d.Vector3 dtorque = new d.Vector3(); + + dvtmp = d.BodyGetLinearVel(Body); + curVel.X = dvtmp.X; + curVel.Y = dvtmp.Y; + curVel.Z = dvtmp.Z; + Vector3 curLocalVel = curVel * irotq; // current velocity in local + + dvtmp = d.BodyGetAngularVel(Body); + curAngVel.X = dvtmp.X; + curAngVel.Y = dvtmp.Y; + curAngVel.Z = dvtmp.Z; + Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local + + float ldampZ = 0; + + // linear motor + if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) + { + tmpV = m_linearMotorDirection - curLocalVel; // velocity error + tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep + tmpV *= rotq; // to world + + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) + tmpV.Z = 0; + + if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) + { + // have offset, do it now + tmpV *= dmass.mass; + d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); + } + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + m_lmEfect *= m_lmDecay; +// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); + m_ffactor = 0.0f; + } + else + { + m_lmEfect = 0; + m_ffactor = 1f; + } + + // hover + if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) + { + // d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); + pos.Z -= 0.21f; // minor offset that seems to be always there in sl + + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float perr; + + // default to global but don't go underground + perr = m_VhoverHeight - pos.Z; + + if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += t; + } + else + { + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + } + else if (t > m_VhoverHeight) + perr = t - pos.Z; ; + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) + { + ldampZ = m_VhoverEfficiency * m_invtimestep; + + perr *= (1.0f + ldampZ) / m_VhoverTimescale; + + // force.Z += perr - curVel.Z * tmp; + force.Z += perr; + ldampZ *= -curVel.Z; + + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; + } + else + { + // default gravity and Buoyancy + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); + } + + // linear deflection + if (m_linearDeflectionEfficiency > 0) + { + float len = curVel.Length(); + if (len > 0.01) // if moving + { + Vector3 atAxis; + atAxis = Xrot(rotq); // where are we pointing to + atAxis *= len; // make it same size as world velocity vector + + tmpV = -atAxis; // oposite direction + atAxis -= curVel; // error to one direction + len = atAxis.LengthSquared(); + + tmpV -= curVel; // error to oposite + float lens = tmpV.LengthSquared(); + + if (len > 0.01 || lens > 0.01) // do nothing if close enougth + { + if (len < lens) + tmpV = atAxis; + + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) + force.Z += tmpV.Z; + } + } + } + + // linear friction/damping + if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) + { + tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; + tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; + tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; + tmpV *= rotq; // to world + + if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) + tmpV.Z = ldampZ; + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + // vertical atractor + if (m_verticalAttractionTimescale < 300) + { + float roll; + float pitch; + + + + float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; + + float ftmp2; + ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; + m_amdampX = ftmp2; + + m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; + + GetRollPitch(irotq, out roll, out pitch); + + if (roll > halfpi) + roll = pi - roll; + else if (roll < -halfpi) + roll = -pi - roll; + + float effroll = pitch / halfpi; + effroll *= effroll; + effroll = 1 - effroll; + effroll *= roll; + + + torque.X += effroll * ftmp; + + if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) + { + float effpitch = roll / halfpi; + effpitch *= effpitch; + effpitch = 1 - effpitch; + effpitch *= pitch; + + torque.Y += effpitch * ftmp; + } + + if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) + { + + float broll = effroll; + /* + if (broll > halfpi) + broll = pi - broll; + else if (broll < -halfpi) + broll = -pi - broll; + */ + broll *= m_bankingEfficiency; + if (m_bankingMix != 0) + { + float vfact = Math.Abs(curLocalVel.X) / 10.0f; + if (vfact > 1.0f) vfact = 1.0f; + + if (curLocalVel.X >= 0) + broll *= (1 + (vfact - 1) * m_bankingMix); + else + broll *= -(1 + (vfact - 1) * m_bankingMix); + } + // make z rot be in world Z not local as seems to be in sl + + broll = broll / m_bankingTimescale; + + + tmpV = Zrot(irotq); + tmpV *= broll; + + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; + + m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; + m_amdampY = m_amdampZ; + + } + else + { + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + m_amdampY = m_amdampX; + } + } + else + { + m_ampwr = 1.0f; + m_amdampX = 1 / m_angularFrictionTimescale.X; + m_amdampY = 1 / m_angularFrictionTimescale.Y; + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + } + + // angular motor + if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) + { + tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error + tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep + torque.X += tmpV.X * m_ampwr; + torque.Y += tmpV.Y * m_ampwr; + torque.Z += tmpV.Z; + + m_amEfect *= m_amDecay; + } + else + m_amEfect = 0; + + // angular deflection + if (m_angularDeflectionEfficiency > 0) + { + Vector3 dirv; + + if (curLocalVel.X > 0.01f) + dirv = curLocalVel; + else if (curLocalVel.X < -0.01f) + // use oposite + dirv = -curLocalVel; + else + { + // make it fall into small positive x case + dirv.X = 0.01f; + dirv.Y = curLocalVel.Y; + dirv.Z = curLocalVel.Z; + } + + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; + + if (Math.Abs(dirv.Z) > 0.01) + { + torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; + } + + if (Math.Abs(dirv.Y) > 0.01) + { + torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; + } + } + + // angular friction + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + torque.X -= curLocalAngVel.X * m_amdampX; + torque.Y -= curLocalAngVel.Y * m_amdampY; + torque.Z -= curLocalAngVel.Z * m_amdampZ; + } + + + if (force.X != 0 || force.Y != 0 || force.Z != 0) + { + force *= dmass.mass; + d.BodyAddForce(Body, force.X, force.Y, force.Z); + } + + if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) + { + torque *= m_referenceFrame; // to object frame + dtorque.X = torque.X ; + dtorque.Y = torque.Y; + dtorque.Z = torque.Z; + + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); + d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame + } + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs new file mode 100644 index 0000000..40b5ef7 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs @@ -0,0 +1,933 @@ +/* + * AJLDuarte 2012 + */ + +using System; +using System.Threading; +using System.Collections.Generic; +using System.IO; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using Nini.Config; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public enum MeshState : byte + { + noNeed = 0, + + loadingAsset = 1, + + AssetOK = 0x0f, // 00001111 + + NeedMask = 0x30, // 00110000 + needMesh = 0x10, // 00010000 + needAsset = 0x20, // 00100000 + + FailMask = 0xC0, // 11000000 + AssetFailed = 0x40, // 01000000 + MeshFailed = 0x80, // 10000000 + + MeshNoColide = FailMask | needAsset + } + + public enum meshWorkerCmnds : byte + { + nop = 0, + addnew, + changefull, + changesize, + changeshapetype, + getmesh, + } + + public class ODEPhysRepData + { + public PhysicsActor actor; + public PrimitiveBaseShape pbs; + public IMesh mesh; + + public Vector3 size; + public Vector3 OBB; + public Vector3 OBBOffset; + + public float volume; + + public byte shapetype; + public bool hasOBB; + public bool hasMeshVolume; + public MeshState meshState; + public UUID? assetID; + public meshWorkerCmnds comand; + } + + public class ODEMeshWorker + { + + private ILog m_log; + private ODEScene m_scene; + private IMesher m_mesher; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 32; + + + private OpenSim.Framework.BlockingQueue createqueue = new OpenSim.Framework.BlockingQueue(); + private bool m_running; + + private Thread m_thread; + + public ODEMeshWorker(ODEScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) + { + m_scene = pScene; + m_log = pLog; + m_mesher = pMesher; + + if (pConfig != null) + { + forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); + meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); + MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); + } + m_running = true; + m_thread = new Thread(DoWork); + m_thread.Name = "OdeMeshWorker"; + m_thread.Start(); + } + + private void DoWork() + { + m_mesher.ExpireFileCache(); + + while(m_running) + { + ODEPhysRepData nextRep = createqueue.Dequeue(); + if(!m_running) + return; + if (nextRep == null) + continue; + if (m_scene.haveActor(nextRep.actor)) + { + switch (nextRep.comand) + { + case meshWorkerCmnds.changefull: + case meshWorkerCmnds.changeshapetype: + case meshWorkerCmnds.changesize: + GetMesh(nextRep); + if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) + m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); + break; + case meshWorkerCmnds.getmesh: + DoRepDataGetMesh(nextRep); + break; + } + } + } + } + + public void Stop() + { + try + { + m_thread.Abort(); + createqueue.Clear(); + } + catch + { + } + } + + public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + CheckMesh(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.PhysRepData, repData); + return; + } + + public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + CheckMesh(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.AddPhysRep, repData); + return repData; + } + + public void RequestMesh(ODEPhysRepData repData) + { + repData.mesh = null; + + if (repData.meshState == MeshState.needAsset) + { + PrimitiveBaseShape pbs = repData.pbs; + + // check if we got outdated + + if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero) + { + repData.meshState = MeshState.noNeed; + return; + } + + repData.assetID = pbs.SculptTexture; + repData.meshState = MeshState.loadingAsset; + + repData.comand = meshWorkerCmnds.getmesh; + createqueue.Enqueue(repData); + } + } + + // creates and prepares a mesh to use and calls parameters estimation + public bool CreateActorPhysRep(ODEPhysRepData repData) + { + IMesh mesh = repData.mesh; + + if (mesh != null) + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", + repData.actor.Name, repData.pbs.SculptTexture.ToString()); + repData.meshState = MeshState.MeshFailed; + repData.hasOBB = false; + repData.mesh = null; + m_scene.mesher.ReleaseMesh(mesh); + } + else + { + repData.OBBOffset = mesh.GetCentroid(); + repData.OBB = mesh.GetOBB(); + repData.hasOBB = true; + mesh.releaseSourceMeshData(); + } + } + CalcVolumeData(repData); + return true; + } + + public void AssetLoaded(ODEPhysRepData repData) + { + if (m_scene.haveActor(repData.actor)) + { + if (needsMeshing(repData.pbs)) // no need for pbs now? + { + repData.comand = meshWorkerCmnds.changefull; + createqueue.Enqueue(repData); + } + } + else + repData.pbs.SculptData = Utils.EmptyBytes; + } + + public void DoRepDataGetMesh(ODEPhysRepData repData) + { + if (!repData.pbs.SculptEntry) + return; + + if (repData.meshState != MeshState.loadingAsset) + return; + + if (repData.assetID == null || repData.assetID == UUID.Zero) + return; + + if (repData.assetID != repData.pbs.SculptTexture) + return; + + // check if it is in cache + GetMesh(repData); + if (repData.meshState != MeshState.needAsset) + { + CreateActorPhysRep(repData); + m_scene.AddChange(repData.actor, changes.PhysRepData, repData); + return; + } + + RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; + if (assetProvider == null) + return; + ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log); + } + + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // check sculpts or meshs + if (pbs.SculptEntry) + { + if (meshSculptedPrim) + return true; + + if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs + return true; + + return false; + } + + if (forceSimplePrimMeshing) + return true; + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { + return false; + } + } + + // following code doesn't give meshs to boxes and spheres ever + // and it's odd.. so for now just return true if asked to force meshs + // hopefully mesher will fail if doesn't suport so things still get basic boxes + + int iPropertiesNotSupportedDefault = 0; + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (iPropertiesNotSupportedDefault == 0) + { + return false; + } + return true; + } + + // see if we need a mesh and if so if we have a cached one + // called with a new repData + public void CheckMesh(ODEPhysRepData repData) + { + PhysicsActor actor = repData.actor; + PrimitiveBaseShape pbs = repData.pbs; + + if (!needsMeshing(pbs)) + { + repData.meshState = MeshState.noNeed; + return; + } + + IMesh mesh = null; + + Vector3 size = repData.size; + byte shapetype = repData.shapetype; + + bool convex; + + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else + { + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } + + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + + if (mesh == null) + { + if (pbs.SculptEntry) + { + if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) + { + repData.assetID = pbs.SculptTexture; + repData.meshState = MeshState.needAsset; + } + else + repData.meshState = MeshState.MeshFailed; + + return; + } + else + { + repData.meshState = MeshState.needMesh; + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + if (mesh == null) + { + repData.meshState = MeshState.MeshFailed; + return; + } + } + } + + repData.meshState = MeshState.AssetOK; + repData.mesh = mesh; + + if (pbs.SculptEntry) + { + repData.assetID = pbs.SculptTexture; + } + + pbs.SculptData = Utils.EmptyBytes; + return ; + } + + public void GetMesh(ODEPhysRepData repData) + { + PhysicsActor actor = repData.actor; + + PrimitiveBaseShape pbs = repData.pbs; + + repData.mesh = null; + repData.hasOBB = false; + + if (!needsMeshing(pbs)) + { + repData.meshState = MeshState.noNeed; + return; + } + + if (repData.meshState == MeshState.MeshFailed) + return; + + if (pbs.SculptEntry) + { + if (repData.meshState == MeshState.AssetFailed) + { + if (pbs.SculptTexture == repData.assetID) + return; + } + } + + repData.meshState = MeshState.noNeed; + + IMesh mesh = null; + Vector3 size = repData.size; + byte shapetype = repData.shapetype; + + bool convex; + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else + { + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } + + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + + if (mesh == null) + { + if (pbs.SculptEntry) + { + if (pbs.SculptTexture == UUID.Zero) + return; + + repData.assetID = pbs.SculptTexture; + + if (pbs.SculptData == null || pbs.SculptData.Length == 0) + { + repData.meshState = MeshState.needAsset; + return; + } + } + } + + repData.mesh = mesh; + repData.pbs.SculptData = Utils.EmptyBytes; + + if (mesh == null) + { + if (pbs.SculptEntry) + repData.meshState = MeshState.AssetFailed; + else + repData.meshState = MeshState.MeshFailed; + + return; + } + + repData.meshState = MeshState.AssetOK; + + return; + } + + private void CalculateBasicPrimVolume(ODEPhysRepData repData) + { + PrimitiveBaseShape _pbs = repData.pbs; + Vector3 _size = repData.size; + + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.5236f; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Circle: + case HollowShape.Triangle: // diference in sl is minor and odd + case HollowShape.Same: + break; + + case HollowShape.Square: + hollowVolume *= 0.909f; + break; + + // case HollowShape.Triangle: + // hollowVolume *= .827f; + // break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + repData.volume = volume; + } + + private void CalcVolumeData(ODEPhysRepData repData) + { + if (repData.hasOBB) + { + Vector3 OBB = repData.OBB; + } + else + { + Vector3 OBB = repData.size; + OBB.X *= 0.5f; + OBB.Y *= 0.5f; + OBB.Z *= 0.5f; + + repData.OBB = OBB; + repData.OBBOffset = Vector3.Zero; + } + + CalculateBasicPrimVolume(repData); + } + } + + public class ODEAssetRequest + { + ODEMeshWorker m_worker; + private ILog m_log; + ODEPhysRepData repData; + + public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, + ODEPhysRepData pRepData, ILog plog) + { + m_worker = pWorker; + m_log = plog; + repData = pRepData; + + repData.meshState = MeshState.AssetFailed; + if (provider == null) + return; + + if (repData.assetID == null) + return; + + UUID assetID = (UUID) repData.assetID; + if (assetID == UUID.Zero) + return; + + repData.meshState = MeshState.loadingAsset; + provider(assetID, ODEassetReceived); + } + + void ODEassetReceived(AssetBase asset) + { + repData.meshState = MeshState.AssetFailed; + if (asset != null) + { + if (asset.Data != null && asset.Data.Length > 0) + { + repData.meshState = MeshState.noNeed; + + if (!repData.pbs.SculptEntry) + return; + if (repData.pbs.SculptTexture != repData.assetID) + return; + +// repData.pbs.SculptData = new byte[asset.Data.Length]; +// asset.Data.CopyTo(repData.pbs.SculptData,0); + repData.pbs.SculptData = asset.Data; + repData.meshState = MeshState.AssetOK; + m_worker.AssetLoaded(repData); + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", + repData.actor.Name, asset.ID.ToString()); + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", + repData.actor.Name); + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs new file mode 100644 index 0000000..44bf17f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs @@ -0,0 +1,97 @@ +using System; +using System.Reflection; +using log4net; +using Nini.Config; +using Mono.Addins; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEPhysicsScene")] + class ubOdeModule : INonSharedRegionModule + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_Enabled = false; + private IConfigSource m_config; + private ODEScene m_scene; + private bool ubOdeLib; + + #region INonSharedRegionModule + + public string Name + { + get { return "ubODE"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void Initialise(IConfigSource source) + { + IConfig config = source.Configs["Startup"]; + if (config != null) + { + string physics = config.GetString("physics", string.Empty); + if (physics == Name) + { + m_config = source; + m_Enabled = true; + } + } + } + + public void Close() + { + } + + public void AddRegion(Scene scene) + { + if (!m_Enabled) + return; + + if (Util.IsWindows()) + Util.LoadArchSpecificWindowsDll("ode.dll"); + + // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to + // http://opensimulator.org/mantis/view.php?id=2750). + d.InitODE(); + + string ode_config = d.GetConfiguration(); + if (ode_config != null && ode_config != "") + { + m_log.InfoFormat("[ubODE] ode library configuration: {0}", ode_config); + // ubODE still not avaiable + if (ode_config.Contains("ubODE")) + { + ubOdeLib = true; + } + } + + m_scene = new ODEScene(scene, m_config, Name, ubOdeLib); + } + + public void RemoveRegion(Scene scene) + { + if (!m_Enabled || m_scene == null) + return; + + m_scene.Dispose(); + m_scene = null; + } + + public void RegionLoaded(Scene scene) + { + if (!m_Enabled || m_scene == null) + return; + + m_scene.RegionLoaded(); + } + #endregion + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs new file mode 100644 index 0000000..91aca26 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -0,0 +1,3896 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; + private bool m_fakeisphantom; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + protected bool m_building; + protected bool m_forcePosOrRotation; + private bool m_iscolliding; + + internal bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + internal bool m_outbounds; + + private Quaternion m_lastorientation; + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 m_torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private float m_invTimeStep; + private float m_timeStep; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private bool m_usePID; + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames; + public int bodydisablecontrol; + private float m_gravmod = 1.0f; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + // Default colide nonphysical don't try to colide with anything + private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; + + private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Land | + CollisionCategories.VolumeDtc); + +// private bool m_collidesLand = true; + private bool m_collidesWater; +// public bool m_returnCollisions; + + private bool m_NoColide; // for now only for internal use for bad meshs + + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; + + public bool m_disabled; + + private uint m_localID; + + private IMesh m_mesh; + private object m_meshlock = new object(); + private PrimitiveBaseShape _pbs; + + private UUID? m_assetID; + private MeshState m_meshState; + + public ODEScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + public float m_collisionscore; + private int m_colliderfilter = 0; + + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + + private float m_density; + private byte m_shapetype; + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body; + + private Vector3 _target_velocity; + + public Vector3 m_OBBOffset; + public Vector3 m_OBB; + public float primOOBradiusSQ; + + private bool m_hasOBB = true; + + private float m_physCost; + private float m_streamCost; + + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float primVolume; // prim own volume; + float _mass; // object mass acording to case + + public int givefakepos; + private Vector3 fakepos; + public int givefakeori; + private Quaternion fakeori; + + private int m_eventsubscription; + private int m_cureventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool IsVolumeDtc + { + get { return m_fakeisVolumeDetect; } + set + { + m_fakeisVolumeDetect = value; + AddChange(changes.VolumeDtc, value); + } + } + + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; + AddChange(changes.Phantom, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { +// if (value) +// m_building = true; + AddChange(changes.building, value); + } + } + + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; + + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; + + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; +// cdata.mu *= 0; + } + } + + public override float PhysicsCost + { + get + { + return m_physCost; + } + } + + public override float StreamCost + { + get + { + return m_streamCost; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get { return m_localID; } + set { m_localID = value; } + } + + public override PhysicsActor ParentActor + { + get + { + if (childPrim) + return _parent; + else + return (PhysicsActor)this; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + m_iscolliding = true; + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + + public override bool ThrottleUpdates {get;set;} + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return primMass; } + } + + public override Vector3 Force + { + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + public override void SetVolumeDetect(int param) + { + m_fakeisVolumeDetect = (param != 0); + AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); + } + + public override Vector3 GeometricCenter + { + // this is not real geometric center but a average of positions relative to root prim acording to + // http://wiki.secondlife.com/wiki/llGetGeometricCenter + // ignoring tortured prims details since sl also seems to ignore + // so no real use in doing it on physics + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + lock (_parent_scene.OdeLock) + { + d.Vector3 dtmp; + if (!childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 Ptot = m_OBBOffset * q; + dtmp = d.GeomGetPosition(prim_geom); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + + // if(childPrim) we only know about physical linksets + return Ptot; +/* + float tmass = _mass; + Ptot *= tmass; + + float m; + + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } + + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; + + Ptot *= tmass; + return Ptot; +*/ + } + else + return _position; + } + } + } + + public override Vector3 OOBsize + { + get + { + return m_OBB; + } + } + + public override Vector3 OOBoffset + { + get + { + return m_OBBOffset; + } + } + + public override float OOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + + public override PrimitiveBaseShape Shape + { + set + { +// AddChange(changes.Shape, value); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); + } + } + + public override byte PhysicsShapeType + { + get + { + return m_shapetype; + } + set + { + m_shapetype = value; + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); + } + } + + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return m_torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + + value.Normalize(); + + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.AngVelocity, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + AddChange(changes.Buoyancy,value); + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + AddChange(changes.PIDTarget,value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive + { + get + { + return m_usePID; + } + set + { + AddChange(changes.PIDActive,value); + } + } + + public override float PIDTau + { + set + { + float tmp = 0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDTau,tmp); + } + } + + public override float PIDHoverHeight + { + set + { + AddChange(changes.PIDHoverHeight,value); + } + } + public override bool PIDHoverActive + { + set + { + AddChange(changes.PIDHoverActive, value); + } + } + + public override PIDHoverType PIDHoverType + { + set + { + AddChange(changes.PIDHoverType,value); + } + } + + public override float PIDHoverTau + { + set + { + float tmp =0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDHoverTau, tmp); + } + } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if(pushforce) + AddChange(changes.AddForce, force); + else // a impulse + AddChange(changes.AddForce, force * m_invTimeStep); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { +// if(pushforce) for now applyrotationimpulse seems more happy applied as a force + AddChange(changes.AddAngForce, force); +// else // a impulse +// AddChange(changes.AddAngForce, force * m_invTimeStep); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public override float Density + { + get + { + return m_density * 100f; + } + set + { + m_density = value / 100f; + // for not prim mass is not updated since this implies full rebuild of body inertia TODO + } + } + public override float GravModifier + { + get + { + return m_gravmod; + } + set + { + m_gravmod = value; + if (m_vehicle != null) + m_vehicle.GravMod = m_gravmod; + } + } + public override float Friction + { + get + { + return mu; + } + set + { + mu = value; + } + } + + public override float Restitution + { + get + { + return bounce; + } + set + { + bounce = value; + } + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; +// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; + } + + public override void UnSubscribeEvents() + { + if (CollisionEventsThisFrame != null) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } + m_eventsubscription = 0; + _parent_scene.RemoveCollisionEventReporting(this); + } + + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); +// if(CollisionEventsThisFrame.Count < 32) + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + if (m_cureventsubscription < m_eventsubscription) + return; + + m_cureventsubscription = 0; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; + _parent_scene.RemoveCollisionEventReporting(this); + } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + + internal void AddCollisionFrameTime(int t) + { + if (m_cureventsubscription < 50000) + m_cureventsubscription += t; + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + public OdePrim(String primName, ODEScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) + { + Name = primName; + LocalID = plocalID; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + m_timeStep = parent_scene.ODE_STEPSIZE; + m_invTimeStep = 1f / m_timeStep; + + m_density = parent_scene.geomDefaultDensity; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + m_fakeisVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_NoColide = false; + + _triMeshData = IntPtr.Zero; + + m_shapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + m_building = true; // control must set this to false when done + + // get basic mass parameters + ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); + + primVolume = repData.volume; + m_OBB = repData.OBB; + m_OBBOffset = repData.OBBOffset; + + UpdatePrimBodyData(); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void UpdateCollisionCatFlags() + { + if(m_isphysical && m_disabled) + { + m_collisionCategories = 0; + m_collisionFlags = 0; + } + + else if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + } + + else if (m_isVolumeDetect) + { + m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + m_collisionFlags = 0; + } + else if (m_isphantom) + { + m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionFlags = m_default_collisionFlagsPhysical; + else + m_collisionFlags = m_default_collisionFlagsNotPhysical; + } + } + + private void ApplyCollisionCatFlags() + { + if (prim_geom != IntPtr.Zero) + { + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + { + if (m_isphysical && m_disabled) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = 0; + } + else + { + // preserve some + if (prm.m_isSelected) + { + prm.m_collisionCategories = CollisionCategories.Selected; + prm.m_collisionFlags = 0; + } + else if (prm.m_isVolumeDetect) + { + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + prm.m_collisionFlags = 0; + } + else if (prm.m_isphantom) + { + prm.m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Land; + else + prm.m_collisionFlags = 0; + } + else + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + } + } + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + } + } + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.BodyGetQuaternion(Body); + Quaternion curr; // crap convertion between identical things + curr.X = dcur.X; + curr.Y = dcur.Y; + curr.Z = dcur.Z; + curr.W = dcur.W; + Vector3 ax; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } + + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + + UpdatePrimBodyData(); + _parent_scene.actor_name_map[prim_geom] = this; + +/* +// debug + d.AABB aabb; + d.GeomGetAABB(prim_geom, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if( x > 60.0f || y > 60.0f || z > 60.0f) + m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + else if (x < 0.001f || y < 0.001f || z < 0.001f) + m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + +// +*/ + + } + else + m_log.Warn("Setting bad Geom"); + } + + private bool GetMeshGeom() + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + IMesh mesh = m_mesh; + + if (mesh == null) + return false; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); + + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } + + IntPtr geo = IntPtr.Zero; + + try + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + try + { + d.GeomTriMeshDataDestroy(_triMeshData); + } + catch + { + } + } + _triMeshData = IntPtr.Zero; + + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } + + m_physCost = 0.0013f * (float)indexCount; + // todo + m_streamCost = 1.0f; + + SetGeom(geo); + + return true; + } + + private void CreateGeom() + { + bool hasMesh = false; + + m_NoColide = false; + + if ((m_meshState & MeshState.MeshNoColide) != 0) + m_NoColide = true; + + else if(m_mesh != null) + { + if (GetMeshGeom()) + hasMesh = true; + else + m_NoColide = true; + } + + + if (!hasMesh) + { + IntPtr geo = IntPtr.Zero; + + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + try + { + geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + try + { + geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + m_physCost = 0.1f; + m_streamCost = 1.0f; + SetGeom(geo); + } + } + + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(prim_geom); + + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + m_targetSpace = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + + lock (m_meshlock) + { + if (m_mesh != null) + { + _parent_scene.mesher.ReleaseMesh(m_mesh); + m_mesh = null; + } + } + + Body = IntPtr.Zero; + m_hasOBB = false; + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + collide_geom = IntPtr.Zero; + } + + public void enableBodySoft() + { + m_disabled = false; + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isSelected) + m_collisionFlags = CollisionCategories.Selected; + else + m_collisionCategories = 0; + m_collisionFlags = 0; + ApplyCollisionCatFlags(); + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + DestroyBody(); + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + objdmass = primdMass; + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X; + rcm.Y = _position.Y; + rcm.Z = _position.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + tmpdmass = prm.primdMass; + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X -= rcm.X; + ppos.Y -= rcm.Y; + ppos.Z -= rcm.Z; + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.X = -myrot.X; + myrot.Y = -myrot.Y; + myrot.Z = -myrot.Z; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + d.BodySetAutoDisableAngularThreshold(Body, 0.01f); + d.BodySetAutoDisableLinearThreshold(Body, 0.01f); + d.BodySetDamping(Body, .005f, .001f); + + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; + } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + + d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(m_targetSpace, 0); + collide_geom = m_targetSpace; + } + + d.SpaceAdd(m_targetSpace, prim_geom); + + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + + m_collisionscore = 0; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + _parent_scene.addActivePrim(this); + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; + + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + + if (m_isSelected || m_disabled) + { + d.BodyDisable(Body); + } + else + { + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + } + _parent_scene.addActiveGroups(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + + collide_geom = IntPtr.Zero; + + if (m_disabled) + m_collisionCategories = 0; + else if (m_isSelected) + m_collisionCategories = CollisionCategories.Selected; + else if (m_isVolumeDetect) + m_collisionCategories = CollisionCategories.VolumeDtc; + else if (m_isphantom) + m_collisionCategories = CollisionCategories.Phantom; + else + m_collisionCategories = CollisionCategories.Geom; + + m_collisionFlags = 0; + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + + if (prm.m_isSelected) + prm.m_collisionCategories = CollisionCategories.Selected; + else if (prm.m_isVolumeDetect) + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + else if (prm.m_isphantom) + prm.m_collisionCategories = CollisionCategories.Phantom; + else + prm.m_collisionCategories = CollisionCategories.Geom; + + prm.m_collisionFlags = 0; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_collisionscore = 0; + } + + private void FixInertia(Vector3 NewPos,Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + + d.BodyGetMass(Body, out tmpdmass); + objdmass = tmpdmass; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + + // transform to object frame + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // back prim own inertia + tmpdmass = primdMass; + + // update to new position and orientation + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + + + private void FixInertia(Vector3 NewPos) + { + d.Matrix3 primmat = new d.Matrix3(); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Mass primmass = new d.Mass { }; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + primmass = primdMass; + // transform to object frame + primmat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref primmass, ref primmat); + + tmpdmass = primmass; + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new position + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref primmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref primmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + private void FixInertia(Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + // transform to object frame + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new orientation + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + } + + + #region Mass Calculation + + private void UpdatePrimBodyData() + { + primMass = m_density * primVolume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); + + d.MassTranslate(ref primdMass, + m_OBBOffset.X, + m_OBBOffset.Y, + m_OBBOffset.Z); + + primOOBradiusSQ = m_OBB.LengthSquared(); + + if (_triMeshData != IntPtr.Zero) + { + float pc = m_physCost; + float psf = primOOBradiusSQ; + psf *= 1.33f * .2f; + pc *= psf; + if (pc < 0.1f) + pc = 0.1f; + + m_physCost = pc; + } + else + m_physCost = 0.1f; + + m_streamCost = 1.0f; + } + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Quaternion qtmp; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + _orientation.W = qtmp.W; +/* +// Debug + float qlen = _orientation.Length(); + if (qlen > 1.01f || qlen < 0.99) + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); +// +*/ + _orientation.Normalize(); + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + + #region changes + + private void changeadd() + { + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if (!childPrim) + { +// m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; +// m_torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + _zeroFlag = false; + base.RequestPhysicsterseUpdate(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + } + } + + private void changePhantomStatus(bool newval) + { + m_isphantom = newval; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + +/* not in use + internal void ChildSelectedChange(bool childSelect) + { + if(childPrim) + return; + + if (childSelect == m_isSelected) + return; + + if (childSelect) + { + DoSelectedStatus(true); + } + + else + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_isSelected) + return; + } + DoSelectedStatus(false); + } + } +*/ + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); + + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); + } + } + prm.m_delaySelect = false; + } + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(true); + + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, 0); + } + + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + + m_delaySelect = false; + } + else if(!m_isphysical) + { + m_delaySelect = true; + } + } + else + { + if (!childPrim) + { + if (Body != IntPtr.Zero && !m_disabled) + d.BodyEnable(Body); + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(false); + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + m_delaySelect = false; + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + + else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + { + FixInertia(newPos); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; +// changeSelectedStatus(); + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } +/* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } +*/ + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + CheckDelaySelect(); + + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + MakeBody(); + } + else + { + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); + } + } + + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + } + + private void changeShape(PrimitiveBaseShape newShape) + { + } + + private void changeAddPhysRep(ODEPhysRepData repData) + { + _size = repData.size; //?? + _pbs = repData.pbs; + m_shapetype = repData.shapetype; + + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_meshState = repData.meshState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (!m_isphysical) + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + else + MakeBody(); + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); + } + } + + private void changePhysRepData(ODEPhysRepData repData) + { + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + _size = repData.size; + _pbs = repData.pbs; + m_shapetype = repData.shapetype; + + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_meshState = repData.meshState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (m_isphysical) + { + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + } + else + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + resetCollisionAccounting(); + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_shapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); + } + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + m_torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 theforce) + { + m_forceacc += theforce; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + } + } + + // actually angular impulse + private void changeAddAngularImpulse(Vector3 aimpulse) + { + m_angularForceacc += aimpulse * m_invTimeStep; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + float len = newVel.LengthSquared(); + if (len > 100000.0f) // limit to 100m/s + { + len = 100.0f / (float)Math.Sqrt(len); + newVel *= len; + } + + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeangvelocity(Vector3 newAngVel) + { + float len = newAngVel.LengthSquared(); + if (len > 144.0f) // limit to 12rad/s + { + len = 12.0f / (float)Math.Sqrt(len); + newAngVel *= len; + } + + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + + d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); + } + //resetCollisionAccounting(); + } + m_rotationalVelocity = newAngVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + m_fakeisVolumeDetect = newVolDtc; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + protected void changeBuilding(bool newbuilding) + { + // Check if we need to do anything + if (newbuilding == m_building) + return; + + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + public void changeSetVehicle(VehicleData vdata) + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + } + + private void changeBuoyancy(float b) + { + m_buoyancy = b; + } + + private void changePIDTarget(Vector3 trg) + { + m_PIDTarget = trg; + } + + private void changePIDTau(float tau) + { + m_PIDTau = tau; + } + + private void changePIDActive(bool val) + { + m_usePID = val; + } + + private void changePIDHoverHeight(float val) + { + m_PIDHoverHeight = val; + if (val == 0) + m_useHoverPID = false; + } + + private void changePIDHoverType(PIDHoverType type) + { + m_PIDHoverType = type; + } + + private void changePIDHoverTau(float tau) + { + m_PIDHoverTau = tau; + } + + private void changePIDHoverActive(bool active) + { + m_useHoverPID = active; + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && !m_building && !m_outbounds) + { + if (!d.BodyIsEnabled(Body)) + { + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return; + + if (++bodydisablecontrol < 20) + return; + + d.BodyEnable(Body); + } + + bodydisablecontrol = 0; + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } + + float fx = 0; + float fy = 0; + float fz = 0; + + float m_mass = _mass; + + if (m_usePID && m_PIDTau > 0) + { + // for now position error + _target_velocity = + new Vector3( + (m_PIDTarget.X - lpos.X), + (m_PIDTarget.Y - lpos.Y), + (m_PIDTarget.Z - lpos.Z) + ); + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) + { + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + return; + } + else + { + _zeroFlag = false; + + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; + + // apply limits + tmp = _target_velocity.Length(); + if (tmp > 50.0f) + { + tmp = 50 / tmp; + _target_velocity *= tmp; + } + else if (tmp < 0.05f) + { + tmp = 0.05f / tmp; + _target_velocity *= tmp; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + fx = (_target_velocity.X - vel.X) * m_invTimeStep; + fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; + fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; +// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); + + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + + case PIDHoverType.GroundAndWater: + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + else + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + break; + } // end switch (m_PIDHoverType) + + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + fz = (m_targetHoverHeight - lpos.Z); + + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) + { + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; + + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); + + fz = ((fz - vel.Z) * m_invTimeStep); + } + } + } + else + { + float b = (1.0f - m_buoyancy) * m_gravmod; + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = m_torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + public void UpdatePositionAndVelocity(int frame) + { + if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) + { + bool bodyenabled = d.BodyIsEnabled(Body); + if (bodyenabled || !_zeroFlag) + { + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + + // check outside region + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + +// throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + UnSubscribeEvents(); + + base.RequestPhysicsterseUpdate(); + return; + } + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + + // decide if moving + // use positions since this are integrated quantities + // tolerance values depende a lot on simulation noise... + // use simple math.abs since we dont need to be exact + + if (!bodyenabled || + (Math.Abs(_position.X - lpos.X) < 0.005f) + && (Math.Abs(_position.Y - lpos.Y) < 0.005f) + && (Math.Abs(_position.Z - lpos.Z) < 0.005f) + && (Math.Abs(_orientation.X - ori.X) < 0.0005f) + && (Math.Abs(_orientation.Y - ori.Y) < 0.0005f) + && (Math.Abs(_orientation.Z - ori.Z) < 0.0005f) // ignore W + ) + { + _zeroFlag = true; + } + else + _zeroFlag = false; + + // update velocities and aceleration + if (!(_zeroFlag && lastZeroFlag)) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + _acceleration = _velocity; + + if ((Math.Abs(vel.X) < 0.005f) && + (Math.Abs(vel.Y) < 0.005f) && + (Math.Abs(vel.Z) < 0.005f)) + { + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + _acceleration = _acceleration * t; + } + else + { + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + _acceleration = (_velocity - _acceleration) * m_invTimeStep; + } + + if ((Math.Abs(_acceleration.X) < 0.01f) && + (Math.Abs(_acceleration.Y) < 0.01f) && + (Math.Abs(_acceleration.Z) < 0.01f)) + { + _acceleration = Vector3.Zero; + } + + if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && + (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && + (Math.Abs(_orientation.Z - ori.Z) < 0.0001) + ) + { + m_rotationalVelocity = Vector3.Zero; + } + else + { + vel = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; + } + // } + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + } + if (_zeroFlag) + { + if (lastZeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } + + if (!m_lastUpdateSent) + { + base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + m_lastUpdateSent = true; + } + return; + } + + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = false; + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) + { + // assumes object center of mass is zero + float smass = part.mass; + theobj.mass -= smass; + + smass *= 1.0f / (theobj.mass); ; + + theobj.c.X -= part.c.X * smass; + theobj.c.Y -= part.c.Y * smass; + theobj.c.Z -= part.c.Z * smass; + + theobj.I.M00 -= part.I.M00; + theobj.I.M01 -= part.I.M01; + theobj.I.M02 -= part.I.M02; + theobj.I.M10 -= part.I.M10; + theobj.I.M11 -= part.I.M11; + theobj.I.M12 -= part.I.M12; + theobj.I.M20 -= part.I.M20; + theobj.I.M21 -= part.I.M21; + theobj.I.M22 -= part.I.M22; + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + + case changes.AddPhysRep: + changeAddPhysRep((ODEPhysRepData)arg); + break; + + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); + + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + +// case changes.Acceleration: +// changeacceleration((Vector3)arg); +// break; + + case changes.AngVelocity: + changeangvelocity((Vector3)arg); + break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularImpulse((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.PhysRepData: + changePhysRepData((ODEPhysRepData) arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Phantom: + changePhantomStatus((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + + case changes.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; + + case changes.Buoyancy: + changeBuoyancy((float)arg); + break; + + case changes.PIDTarget: + changePIDTarget((Vector3)arg); + break; + + case changes.PIDTau: + changePIDTau((float)arg); + break; + + case changes.PIDActive: + changePIDActive((bool)arg); + break; + + case changes.PIDHoverHeight: + changePIDHoverHeight((float)arg); + break; + + case changes.PIDHoverType: + changePIDHoverType((PIDHoverType)arg); + break; + + case changes.PIDHoverTau: + changePIDHoverTau((float)arg); + break; + + case changes.PIDHoverActive: + changePIDHoverActive((bool)arg); + break; + + case changes.Null: + donullchange(); + break; + + + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor) this, what, arg); + } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs new file mode 100644 index 0000000..ef1e57c --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs @@ -0,0 +1,683 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// Processes raycast requests as ODE is in a state to be able to do them. + /// This ensures that it's thread safe and there will be no conflicts. + /// Requests get returned by a different thread then they were requested by. + /// + public class ODERayCastRequestManager + { + /// + /// Pending ray requests + /// + protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); + + /// + /// Scene that created this object. + /// + private ODEScene m_scene; + + IntPtr ray; // the ray. we only need one for our lifetime + IntPtr Sphere; + IntPtr Box; + IntPtr Plane; + + private int CollisionContactGeomsPerTest = 25; + private const int DefaultMaxCount = 25; + private const int MaxTimePerCallMS = 30; + + /// + /// ODE near callback delegate + /// + private d.NearCallback nearCallback; + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private List m_contactResults = new List(); + private RayFilterFlags CurrentRayFilter; + private int CurrentMaxCount; + + public ODERayCastRequestManager(ODEScene pScene) + { + m_scene = pScene; + nearCallback = near; + ray = d.CreateRay(IntPtr.Zero, 1.0f); + d.GeomSetCategoryBits(ray, 0); + Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); + d.GeomSetCategoryBits(Box, 0); + Sphere = d.CreateSphere(IntPtr.Zero,1.0f); + d.GeomSetCategoryBits(Sphere, 0); + Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); + d.GeomSetCategoryBits(Sphere, 0); + } + + public void QueueRequest(ODERayRequest req) + { + if (req.Count == 0) + req.Count = DefaultMaxCount; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + + if (m_PendingRequests.Count <= 0) + return 0; + + if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) + // oops something got wrong or scene isn't ready still + { + m_PendingRequests.Clear(); + return 0; + } + + int time = Util.EnvironmentTickCount(); + + ODERayRequest req; + int closestHit; + int backfacecull; + CollisionCategories catflags; + + while (m_PendingRequests.Dequeue(out req)) + { + if (req.callbackMethod != null) + { + IntPtr geom = IntPtr.Zero; + if (req.actor != null) + { + if (m_scene.haveActor(req.actor)) + { + if (req.actor is OdePrim) + geom = ((OdePrim)req.actor).prim_geom; + else if (req.actor is OdeCharacter) + geom = ((OdePrim)req.actor).prim_geom; + } + if (geom == IntPtr.Zero) + { + NoContacts(req); + continue; + } + } + + + CurrentRayFilter = req.filter; + CurrentMaxCount = req.Count; + + CollisionContactGeomsPerTest = req.Count & 0xffff; + + closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); + backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); + + if (req.callbackMethod is ProbeBoxCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); + d.Quaternion qtmp; + qtmp.X = req.orientation.X; + qtmp.Y = req.orientation.Y; + qtmp.Z = req.orientation.Z; + qtmp.W = req.orientation.W; + d.GeomSetQuaternion(Box, ref qtmp); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomSphereSetRadius(Sphere, req.length); + d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); + } + + else + { + if (CollisionContactGeomsPerTest > 25) + CollisionContactGeomsPerTest = 25; + + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomRaySetParams(ray, 0, backfacecull); + d.GeomRaySetClosestHit(ray, closestHit); + + if (req.callbackMethod is RaycastCallback) + { + // if we only want one get only one per Collision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + d.GeomRaySetClosestHit(ray, 1); + } + else + d.GeomRaySetClosestHit(ray, closestHit); + } + + if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) + unchecked + { + CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; + } + + if (geom == IntPtr.Zero) + { + // translate ray filter to Collision flags + catflags = 0; + if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) + catflags |= CollisionCategories.VolumeDtc; + if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) + catflags |= CollisionCategories.Phantom; + if ((CurrentRayFilter & RayFilterFlags.agent) != 0) + catflags |= CollisionCategories.Character; + if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0) + catflags |= CollisionCategories.Geom; + if ((CurrentRayFilter & RayFilterFlags.land) != 0) + catflags |= CollisionCategories.Land; + if ((CurrentRayFilter & RayFilterFlags.water) != 0) + catflags |= CollisionCategories.Water; + + if (catflags != 0) + { + if (req.callbackMethod is ProbeBoxCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Box, (uint)catflags); + d.GeomSetCategoryBits(Box, (uint)catflags); + doProbe(req, Box); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Sphere, (uint)catflags); + d.GeomSetCategoryBits(Sphere, (uint)catflags); + doProbe(req, Sphere); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Plane, (uint)catflags); + d.GeomSetCategoryBits(Plane, (uint)catflags); + doPlane(req,IntPtr.Zero); + } + else + { + d.GeomSetCollideBits(ray, (uint)catflags); + doSpaceRay(req); + } + } + } + else + { + // if we select a geom don't use filters + + if (req.callbackMethod is ProbePlaneCallback) + { + d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); + doPlane(req,geom); + } + else + { + d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req,geom); + } + } + } + + if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) + break; + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return Util.EnvironmentTickCountSubtract(time); + } + /// + /// Method that actually initiates the raycast with spaces + /// + /// + /// + + private void NoContacts(ODERayRequest req) + { + if (req.callbackMethod is RaycastCallback) + { + ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); + return; + } + List cresult = new List(); + + if (req.callbackMethod is RayCallback) + ((RayCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom; +// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; + private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; + + private void doSpaceRay(ODERayRequest req) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + { + // current ode land to ray collisions is very bad + // so for now limit its range badly + + if (req.length > 30.0f) + d.GeomRaySetLength(ray, 30.0f); + + d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if(cResult.Depth < distance) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + if (distance > 0 && distance < float.MaxValue) + hitYN = true; + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); + } + } + + private void doProbe(ODERayRequest req, IntPtr probe) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private void doPlane(ODERayRequest req,IntPtr geom) + { + // Collide tests + if (geom == IntPtr.Zero) + { + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + else + { + d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); + } + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + + ((ProbePlaneCallback)req.callbackMethod)(cresult); + } + + /// + /// Method that actually initiates the raycast with a geom + /// + /// + private void doGeomRay(ODERayRequest req, IntPtr geom) + { + // Collide test + d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if(cResult.Depth < distance ) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + if (distance > 0 && distance < float.MaxValue) + hitYN = true; + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); + } + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; + if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + // This is the standard Near. g1 is the ray + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + if (g2 == IntPtr.Zero || g1 == g2) + return; + + if (m_contactResults.Count >= CurrentMaxCount) + return; + + if (d.GeomIsSpace(g2)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; +/* + uint cat1 = d.GeomGetCategoryBits(g1); + uint cat2 = d.GeomGetCategoryBits(g2); + uint col1 = d.GeomGetCollideBits(g1); + uint col2 = d.GeomGetCollideBits(g2); +*/ + + uint ID = 0; + PhysicsActor p2 = null; + + m_scene.actor_name_map.TryGetValue(g2, out p2); + + if (p2 == null) + return; + + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Prim: + + RayFilterFlags thisFlags; + + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; + + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; + + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; + + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p2).LocalID; + break; + + case (int)ActorTypes.Agent: + + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).LocalID; + break; + + case (int)ActorTypes.Ground: + + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; + + case (int)ActorTypes.Water: + + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: + break; + } + + d.ContactGeom curcontact = new d.ContactGeom(); + + // closestHit for now only works for meshs, so must do it for others + if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) + { + // Loop all contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; + collisionresult.Depth = curcontact.depth; + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; + lock (m_contactResults) + { + m_contactResults.Add(collisionresult); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + if (curcontact.depth < collisionresult.Depth) + { + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; + collisionresult.Depth = curcontact.depth; + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; + } + } + + if (collisionresult.Depth != float.MaxValue) + { + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + } + + /// + /// Dereference the creator scene so that it can be garbage collected if needed. + /// + internal void Dispose() + { + m_scene = null; + if (ray != IntPtr.Zero) + { + d.GeomDestroy(ray); + ray = IntPtr.Zero; + } + if (Box != IntPtr.Zero) + { + d.GeomDestroy(Box); + Box = IntPtr.Zero; + } + if (Sphere != IntPtr.Zero) + { + d.GeomDestroy(Sphere); + Sphere = IntPtr.Zero; + } + if (Plane != IntPtr.Zero) + { + d.GeomDestroy(Plane); + Plane = IntPtr.Zero; + } + } + } + + public struct ODERayRequest + { + public PhysicsActor actor; + public Vector3 Origin; + public Vector3 Normal; + public int Count; + public float length; + public object callbackMethod; + public RayFilterFlags filter; + public Quaternion orientation; + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs new file mode 100644 index 0000000..a2dd885 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -0,0 +1,2825 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ + +// Revision 2011/12/13 by Ubit Umarov +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using Mono.Addins; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + // colision flags of things others can colide with + // rays, sensors, probes removed since can't be colided with + // The top space where things are placed provided further selection + // ie physical are in active space nonphysical in static + // this should be exclusive as possible + + [Flags] + public enum CollisionCategories : uint + { + Disabled = 0, + //by 'things' types + Space = 0x01, + Geom = 0x02, // aka prim/part + Character = 0x04, + Land = 0x08, + Water = 0x010, + + // by state + Phantom = 0x01000, + VolumeDtc = 0x02000, + Selected = 0x04000, + NoShape = 0x08000, + + + All = 0xffffffff + } + + /// + /// Material type for a primitive + /// + public enum Material : int + { + /// + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6, + + light = 7 // compatibility with old viewers + } + + public enum changes : int + { + Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) + Remove, + Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root + // or removes from a object if arg is null + DeLink, + Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child + Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child + PosOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + OriOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + Velocity, + AngVelocity, + Acceleration, + Force, + Torque, + Momentum, + + AddForce, + AddAngForce, + AngLock, + + Buoyancy, + + PIDTarget, + PIDTau, + PIDActive, + + PIDHoverHeight, + PIDHoverType, + PIDHoverTau, + PIDHoverActive, + + Size, + AvatarSize, + Shape, + PhysRepData, + AddPhysRep, + + CollidesWater, + VolumeDtc, + + Physical, + Phantom, + Selected, + disabled, + building, + + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + SetVehicle, + + Null //keep this last used do dim the methods array. does nothing but pulsing the prim + } + + public struct ODEchangeitem + { + public PhysicsActor actor; + public OdeCharacter character; + public changes what; + public Object arg; + } + + public class ODEScene : PhysicsScene + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + public bool m_ubOdeLib = false; + public bool m_suportCombine = false; // mega suport not tested + public Scene m_frameWorkScene = null; + +// private int threadid = 0; + +// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + + const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; + const float comumContactERP = 0.7f; + const float comumContactCFM = 0.0001f; + const float comumContactSLIP = 0f; + + float frictionMovementMult = 0.8f; + + float TerrainBounce = 0.1f; + float TerrainFriction = 0.3f; + + public float AvatarFriction = 0;// 0.9f * 0.5f; + + // this netx dimensions are only relevant for terrain partition (mega regions) + // WorldExtents below has the simulation dimensions + // they should be identical except on mega regions + private uint m_regionWidth = Constants.RegionSize; + private uint m_regionHeight = Constants.RegionSize; + + public float ODE_STEPSIZE = 0.020f; + public float HalfOdeStep = 0.01f; + public int odetimestepMS = 20; // rounded + private float metersInSpace = 25.6f; + private float m_timeDilation = 1.0f; + + private DateTime m_lastframe; + private DateTime m_lastMeshExpire; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + private float waterlevel = 0f; + private int framecount = 0; + +// private int m_meshExpireCntr; + + private float avDensity = 80f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + + + public float geomDefaultDensity = 10.000006836f; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int bodyFramesAutoDisable = 5; + + private d.NearCallback nearCallback; + + private HashSet _characters = new HashSet(); + private HashSet _prims = new HashSet(); + private HashSet _activeprims = new HashSet(); + private HashSet _activegroups = new HashSet(); + + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + + /// + /// A list of actors that should receive collision events. + /// + private List _collisionEventPrim = new List(); + private List _collisionEventPrimRemove = new List(); + + private HashSet _badCharacter = new HashSet(); +// public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); + + private float contactsurfacelayer = 0.001f; + + private int contactsPerCollision = 80; + internal IntPtr ContactgeomsArray = IntPtr.Zero; + private IntPtr GlobalContactsArray = IntPtr.Zero; + private d.Contact SharedTmpcontact = new d.Contact(); + + const int maxContactsbeforedeath = 4000; + private volatile int m_global_contactcount = 0; + + private IntPtr contactgroup; + + public ContactData[] m_materialContactsData = new ContactData[8]; + + private Dictionary RegionTerrain = new Dictionary(); + private Dictionary TerrainHeightFieldHeights = new Dictionary(); + private Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + + private int m_physicsiterations = 15; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag +// private PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; + + public IntPtr world; + + + // split the spaces acording to contents type + // ActiveSpace contains characters and active prims + // StaticSpace contains land and other that is mostly static in enviroment + // this can contain subspaces, like the grid in staticspace + // as now space only contains this 2 top spaces + + public IntPtr TopSpace; // the global space + public IntPtr ActiveSpace; // space for active prims + public IntPtr CharsSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + public IntPtr GroundSpace; // space for ground + + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeterX; + private float spacesPerMeterY; + + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + private IntPtr[] staticPrimspaceOffRegion; + + public Object OdeLock; + public static Object SimulationLock; + + public IMesher mesher; + + public IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + private Vector3 m_worldOffset = Vector3.Zero; + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + private PhysicsScene m_parentScene = null; + + private ODERayCastRequestManager m_rayCastManager; + public ODEMeshWorker m_meshWorker; + + /* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ + + IConfig physicsconfig = null; + + public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, bool pubOdeLib) + { + OdeLock = new Object(); + + EngineType = pname; + PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName; + + m_config = psourceconfig; + m_ubOdeLib = pubOdeLib; + m_frameWorkScene = pscene; + + m_frameWorkScene.RegisterModuleInterface(this); + + Initialization(); + + base.Initialise(m_frameWorkScene.PhysicsRequestAsset, + (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]), + (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight); + } + + // core hack this just means all modules where loaded + // so now we can look for dependencies + public void RegionLoaded() + { + mesher = m_frameWorkScene.RequestModuleInterface(); + if (mesher == null) + { + m_log.WarnFormat("[ubOde] No mesher. module disabled"); + return; + } + + m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig); + m_frameWorkScene.PhysicsEnabled = true; + } + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + private void Initialization() + { + // checkThread(); + SimulationLock = new Object(); + + nearCallback = near; + + m_rayCastManager = new ODERayCastRequestManager(this); + + WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX; + m_regionWidth = (uint)WorldExtents.X; + WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY; + m_regionHeight = (uint)WorldExtents.Y; + + m_suportCombine = false; + + lock (OdeLock) + { + // Create the world and the first space + try + { + world = d.WorldCreate(); + TopSpace = d.HashSpaceCreate(IntPtr.Zero); + + // now the major subspaces + ActiveSpace = d.HashSpaceCreate(TopSpace); + CharsSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + GroundSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + // i did! + } + + d.HashSpaceSetLevels(TopSpace, -2, 8); + d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(CharsSpace, -4, 3); + d.HashSpaceSetLevels(StaticSpace, -2, 8); + d.HashSpaceSetLevels(GroundSpace, 0, 8); + + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(CharsSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + d.SpaceSetSublevel(GroundSpace, 1); + + d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(CharsSpace, 0); + + d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + // CollisionCategories.Land | + // CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(StaticSpace, 0); + + d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundSpace, 0); + + contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + } + + + // checkThread(); + + + // Defaults + + int contactsPerCollision = 80; + + physicsconfig = null; + + if (m_config != null) + { + physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); + gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); + gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); + + // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); + + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); + + physics_logging = physicsconfig.GetBoolean("physics_logging", false); + physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); + physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); + + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); + + avDensity *= 3f / 80f; // scale other engines density option to this + } + } + + d.WorldSetCFM(world, comumContactCFM); + d.WorldSetERP(world, comumContactERP); + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + + d.WorldSetLinearDamping(world, 0.002f); + d.WorldSetAngularDamping(world, 0.002f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 100f); + + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + d.WorldSetContactMaxCorrectingVel(world, 60.0f); + + HalfOdeStep = ODE_STEPSIZE * 0.5f; + odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f); + + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + + SharedTmpcontact.geom.g1 = IntPtr.Zero; + SharedTmpcontact.geom.g2 = IntPtr.Zero; + + SharedTmpcontact.geom.side1 = -1; + SharedTmpcontact.geom.side2 = -1; + + SharedTmpcontact.surface.mode = comumContactFlags; + SharedTmpcontact.surface.mu = 0; + SharedTmpcontact.surface.bounce = 0; + SharedTmpcontact.surface.soft_cfm = comumContactCFM; + SharedTmpcontact.surface.soft_erp = comumContactERP; + SharedTmpcontact.surface.slip1 = comumContactSLIP; + SharedTmpcontact.surface.slip2 = comumContactSLIP; + + m_materialContactsData[(int)Material.Stone].mu = 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; + + m_materialContactsData[(int)Material.Metal].mu = 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = 0.9f; + m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + + m_materialContactsData[(int)Material.light].mu = 0.0f; + m_materialContactsData[(int)Material.light].bounce = 0.0f; + + + spacesPerMeterX = 1.0f / metersInSpace; + spacesPerMeterY = spacesPerMeterX; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); + + if (spaceGridMaxX > 24) + { + spaceGridMaxX = 24; + spacesPerMeterX = spaceGridMaxX / WorldExtents.X; + } + + if (spaceGridMaxY > 24) + { + spaceGridMaxY = 24; + spacesPerMeterY = spaceGridMaxY / WorldExtents.Y; + } + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // create all spaces now + int i, j; + IntPtr newspace; + + for (i = 0; i < spaceGridMaxX; i++) + for (j = 0; j < spaceGridMaxY; j++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspace[i, j] = newspace; + } + + // let this now be index limit + spaceGridMaxX--; + spaceGridMaxY--; + + // create 4 off world spaces (x<0,x>max,y<0,y>max) + staticPrimspaceOffRegion = new IntPtr[4]; + + for (i = 0; i < 4; i++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspaceOffRegion[i] = newspace; + } + + m_lastframe = DateTime.UtcNow; + m_lastMeshExpire = m_lastframe; + } + + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } + + #region Collision Detection + + // sets a global contact for a joint for contactgeom , and base contact description) + + + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom) + { + if (m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; + + m_global_contactcount++; + + SharedTmpcontact.geom.depth = contactGeom.depth; + SharedTmpcontact.geom.pos = contactGeom.pos; + SharedTmpcontact.geom.normal = contactGeom.normal; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(SharedTmpcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + /// + + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + + if (m_global_contactcount >= maxContactsbeforedeath) + return; + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //here one should check collisions of geoms inside a space + // but on each space we only should have geoms that not colide amoung each other + // so we don't dig inside spaces + return; + } + + // get geom bodies to check if we already a joint contact + // guess this shouldn't happen now + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + + // d.GeomClassID id = d.GeomGetClass(g1); + + // Figure out how many contact points we have + int count = 0; + try + { + // Colliding Geom To Geom + // This portion of the function 'was' blatantly ripped off from BoxStack.cs + + if (g1 == g2) + return; // Can't collide with yourself + + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + /* + // debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); + return; + } + } + // + */ + + + if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) + { + int cflags; + unchecked + { + cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); + } + count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + else + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); + // ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + return; + } + + // contacts done + if (count == 0) + return; + + // try get physical actors + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); + return; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + return; + } + + // update actors collision score + if (p1.CollisionScore >= float.MaxValue - count) + p1.CollisionScore = 0; + p1.CollisionScore += count; + + if (p2.CollisionScore >= float.MaxValue - count) + p2.CollisionScore = 0; + p2.CollisionScore += count; + + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + + if (!GetCurContactGeom(0, ref curContact)) + return; + + ContactPoint maxDepthContact = new ContactPoint(); + + // do volume detection case + if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) + { + maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth, false + ); + + collision_accounting_events(p1, p2, maxDepthContact); + return; + } + + // big messy collision analises + + float mu = 0; + float bounce = 0; +// bool IgnoreNegSides = false; + + ContactData contactdata1 = new ContactData(0, 0, false); + ContactData contactdata2 = new ContactData(0, 0, false); + + bool dop1ava = false; + bool dop2ava = false; + bool ignore = false; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + { + dop1ava = true; + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + case (int)ActorTypes.Prim: + break; + + default: + ignore = true; // avatar to terrain and water ignored + break; + } + break; + } + + case (int)ActorTypes.Prim: + { + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + dop2ava = true; + break; + + case (int)ActorTypes.Prim: + Vector3 relV = p1.Velocity - p2.Velocity; + float relVlenSQ = relV.LengthSquared(); + if (relVlenSQ > 0.0001f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); + bounce = contactdata1.bounce * contactdata2.bounce; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if (relVlenSQ > 0.01f) + mu *= frictionMovementMult; + + break; + + case (int)ActorTypes.Ground: + p1.getContactData(ref contactdata1); + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + /* + if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + { + if (curContact.side1 > 0) + IgnoreNegSides = true; + } + */ + break; + + case (int)ActorTypes.Water: + default: + ignore = true; + break; + } + } + break; + + case (int)ActorTypes.Ground: + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + p2.CollidingGround = true; + p2.getContactData(ref contactdata2); + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + +// if (curContact.side1 > 0) // should be 2 ? +// IgnoreNegSides = true; + + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + } + else + ignore = true; + break; + + case (int)ActorTypes.Water: + default: + break; + } + + if (ignore) + return; + + IntPtr Joint; + bool FeetCollision = false; + int ncontacts = 0; + + int i = 0; + + maxDepthContact = new ContactPoint(); + maxDepthContact.PenetrationDepth = float.MinValue; + ContactPoint minDepthContact = new ContactPoint(); + minDepthContact.PenetrationDepth = float.MaxValue; + + SharedTmpcontact.geom.depth = 0; + SharedTmpcontact.surface.mu = mu; + SharedTmpcontact.surface.bounce = bounce; + + d.ContactGeom altContact = new d.ContactGeom(); + bool useAltcontact = false; + bool noskip = true; + + while (true) + { +// if (!(IgnoreNegSides && curContact.side1 < 0)) + { + noskip = true; + useAltcontact = false; + + if (dop1ava) + { + if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) + { + if (p2.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + } + else + noskip = false; + } + else if (dop2ava) + { + if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) + { + if (p1.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p2.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + } + else + noskip = false; + } + + if (noskip) + { + if(useAltcontact) + Joint = CreateContacJoint(ref altContact); + else + Joint = CreateContacJoint(ref curContact); + + if (Joint == IntPtr.Zero) + break; + + d.JointAttach(Joint, b1, b2); + + ncontacts++; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + maxDepthContact.CharacterFeet = FeetCollision; + } + + if (curContact.depth < minDepthContact.PenetrationDepth) + { + minDepthContact.PenetrationDepth = curContact.depth; + minDepthContact.SurfaceNormal.X = curContact.normal.X; + minDepthContact.SurfaceNormal.Y = curContact.normal.Y; + minDepthContact.SurfaceNormal.Z = curContact.normal.Z; + } + } + } + if (++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + } + + if (ncontacts > 0) + { + maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; + maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; + maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; + + collision_accounting_events(p1, p2, maxDepthContact); + } + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + uint obj2LocalID = 0; + + bool p1events = p1.SubscribedEvents(); + bool p2events = p2.SubscribedEvents(); + + if (p1.IsVolumeDtc) + p2events = false; + if (p2.IsVolumeDtc) + p1events = false; + + if (!p2events && !p1events) + return; + + Vector3 vel = Vector3.Zero; + if (p2 != null && p2.IsPhysical) + vel = p2.Velocity; + + if (p1 != null && p1.IsPhysical) + vel -= p1.Velocity; + + contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + case ActorTypes.Prim: + { + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + case ActorTypes.Prim: + if (p2events) + { + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); + } + obj2LocalID = p2.ParentActor.LocalID; + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1events) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + AddCollisionEventReporting(p1); + p1.AddCollisionEvent(obj2LocalID, contact); + } + break; + } + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + { + if (p2events && !p2.IsVolumeDtc) + { + AddCollisionEventReporting(p2); + p2.AddCollisionEvent(0, contact); + } + break; + } + } + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized() + { + lock (_characters) + { + try + { + foreach (OdeCharacter chr in _characters) + { + if (chr == null || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + // do colisions with static space + d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); + + // no coll with gnd + } + // chars with chars + d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); + + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); + } + + } + + lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + } + } + + // collide active prims with static enviroment + lock (_activegroups) + { + try + { + foreach (OdePrim prm in _activegroups) + { + if (!prm.m_outbounds) + { + if (d.BodyIsEnabled(prm.Body)) + { + d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); + } + } + } + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); + } + } + // colide active amoung them + try + { + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space"); + } + // and with chars + try + { + d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide in Active space"); + } + // _perloopContact.Clear(); + } + + #endregion + /// + /// Add actor to the list that should receive collision events in the simulate loop. + /// + /// + public void AddCollisionEventReporting(PhysicsActor obj) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); + } + + /// + /// Remove actor from the list that should receive collision events in the simulate loop. + /// + /// + public void RemoveCollisionEventReporting(PhysicsActor obj) + { + if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) + _collisionEventPrimRemove.Add(obj); + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) + { + return null; + } + + public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) + { + OdeCharacter newAv = new OdeCharacter(localID, avName, this, position, + size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + + return newAv; + } + + public void AddCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } + } + + public void RemoveCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } + } + } + + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) + { + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); + } + } + + public override void RemoveAvatar(PhysicsActor actor) + { + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + } + + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + } + } + + public void addActiveGroups(OdePrim activatePrim) + { + lock (_activegroups) + { + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); + } + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) + { + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); + lock (_prims) + _prims.Add(newPrim); + } + return newPrim; + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); + } + + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) + { + + return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); + } + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + public void remActiveGroup(OdePrim deactivatePrim) + { + lock (_activegroups) + { + _activegroups.Remove(deactivatePrim); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { +// lock (OdeLock) + { + + OdePrim p = (OdePrim)prim; + p.setPrimForRemoval(); + } + } + } + + public void RemovePrimThreadLocked(OdePrim prim) + { + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { +// RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim); + } + + } + + public bool havePrim(OdePrim prm) + { + lock (_prims) + return _prims.Contains(prm); + } + + public bool haveActor(PhysicsActor actor) + { + if (actor is OdePrim) + { + lock (_prims) + return _prims.Contains((OdePrim)actor); + } + else if (actor is OdeCharacter) + { + lock (_characters) + return _characters.Contains((OdeCharacter)actor); + } + return false; + } + + #endregion + + #region Space Separation Calculation + + /// + /// Called when a static prim moves or becomes static + /// Places the prim in a space one the static sub-spaces grid + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // moves a prim into another static sub-space or from another space into a static sub-space + + // Called ODEPrim so + // it's already in locked space. + + if (geom == IntPtr.Zero) // shouldn't happen + return IntPtr.Zero; + + // get the static sub-space for current position + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == currentspace) // if we are there all done + return newspace; + + // else remove it from its current space + if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + } + else + { + m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + + " Geom:" + geom); + } + } + else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space + { + currentspace = d.GeomGetSpace(geom); + if (currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + + } + } + } + + // put the geom in the newspace + waitForSpaceUnlock(newspace); + d.SpaceAdd(newspace, geom); + + // let caller know this newspace + return newspace; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) + { + int x, y; + + if (pos.X < 0) + return staticPrimspaceOffRegion[0]; + + if (pos.Y < 0) + return staticPrimspaceOffRegion[2]; + + x = (int)(pos.X * spacesPerMeterX); + if (x > spaceGridMaxX) + return staticPrimspaceOffRegion[1]; + + y = (int)(pos.Y * spacesPerMeterY); + if (y > spaceGridMaxY) + return staticPrimspaceOffRegion[3]; + + return staticPrimspace[x, y]; + } + + #endregion + + + /// + /// Called to queue a change to a actor + /// to use in place of old taint mechanism so changes do have a time sequence + /// + + public void AddChange(PhysicsActor actor, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.actor = actor; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); + } + + /// + /// Called after our prim properties are set Scale, position etc. + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions + /// + /// + public override void AddPhysicsActorTaint(PhysicsActor prim) + { + } + + // does all pending changes generated during region load process + public override void PrepareSimulation() + { + lock (OdeLock) + { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return; + } + + ODEchangeitem item; + + int donechanges = 0; + if (ChangesQueue.Count > 0) + { + m_log.InfoFormat("[ODE] start processing pending actor operations"); + int tstart = Util.EnvironmentTickCount(); + + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + donechanges++; + } + int time = Util.EnvironmentTickCountSubtract(tstart); + m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); + } + } + } + + /// + /// This is our main simulate loop + /// It's thread locked by a Mutex in the scene. + /// It holds Collisions, it instructs ODE to step through the physical reactions + /// It moves the objects around in memory + /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) + /// + /// + /// + public override float Simulate(float timeStep) + { + DateTime now = DateTime.UtcNow; + TimeSpan timedif = now - m_lastframe; + timeStep = (float)timedif.TotalSeconds; + m_lastframe = now; + + // acumulate time so we can reduce error + step_time += timeStep; + + if (step_time < HalfOdeStep) + return 0; + + if (framecount < 0) + framecount = 0; + + framecount++; + +// int curphysiteractions; + + // if in trouble reduce step resolution +// if (step_time >= m_SkipFramesAtms) +// curphysiteractions = m_physicsiterations / 2; +// else +// curphysiteractions = m_physicsiterations; + +// checkThread(); + int nodeframes = 0; + + lock (SimulationLock) + lock(OdeLock) + { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return 0; + } + + ODEchangeitem item; + +// d.WorldSetQuickStepNumIterations(world, curphysiteractions); + + int loopstartMS = Util.EnvironmentTickCount(); + int looptimeMS = 0; + + + while (step_time > HalfOdeStep) + { + try + { + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; + + if (ChangesQueue.Count > 0) + { + int changestartMS = Util.EnvironmentTickCount(); + int ttmp; + while (ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + ttmp = Util.EnvironmentTickCountSubtract(changestartMS); + if (ttmp > 20) + break; + } + } + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + defects.Clear(); + } + } + + // Move other active objects + lock (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } + + m_rayCastManager.ProcessQueuedRequests(); + + collision_optimized(); + + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime((int)(odetimestepMS)); + cobj.SendCollisions(); + break; + + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) + if (!pobj.m_outbounds) + { + pobj.AddCollisionFrameTime((int)(odetimestepMS)); + pobj.SendCollisions(); + } + break; + } + } + + foreach (PhysicsActor obj in _collisionEventPrimRemove) + _collisionEventPrim.Remove(obj); + + _collisionEventPrimRemove.Clear(); + + // do a ode simulation step + d.WorldQuickStep(world, ODE_STEPSIZE); +// d.WorldStep(world, ODE_STEPSIZE); + d.JointGroupEmpty(contactgroup); + + // update managed ideia of physical data and do updates to core + /* + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + */ + + lock (_activegroups) + { + { + foreach (OdePrim actor in _activegroups) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity(framecount); + } + } + } + } + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + nodeframes++; + + looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); + if (looptimeMS > 100) + break; + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + + _badCharacter.Clear(); + } + } + + timedif = now - m_lastMeshExpire; + + if (timedif.Seconds > 10) + { + mesher.ExpireReleaseMeshs(); + m_lastMeshExpire = now; + } + +// information block running in debug only +/* + int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); + + int nactivegeoms = 0; + int nactivespaces = 0; + + int nstaticgeoms = 0; + int nstaticspaces = 0; + IntPtr sp; + + for (int i = 0; i < ntopactivegeoms; i++) + { + sp = d.SpaceGetGeom(ActiveSpace, i); + if (d.GeomIsSpace(sp)) + { + nactivespaces++; + nactivegeoms += d.SpaceGetNumGeoms(sp); + } + else + nactivegeoms++; + } + + for (int i = 0; i < ntopstaticgeoms; i++) + { + sp = d.SpaceGetGeom(StaticSpace, i); + if (d.GeomIsSpace(sp)) + { + nstaticspaces++; + nstaticgeoms += d.SpaceGetNumGeoms(sp); + } + else + nstaticgeoms++; + } + + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + + int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray + int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; +*/ + // Finished with all sim stepping. If requested, dump world state to file for debugging. + // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? + // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? + if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) + { + string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename + string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file + + if (physics_logging_append_existing_logfile) + { + string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; + TextWriter fwriter = File.AppendText(fname); + fwriter.WriteLine(header); + fwriter.Close(); + } + + d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); + } + + // think time dilation as to do with dinamic step size that we dont' have + // even so tell something to world + if (looptimeMS < 100) // we did the requested loops + m_timeDilation = 1.0f; + else if (step_time > 0) + { + m_timeDilation = timeStep / step_time; + if (m_timeDilation > 1) + m_timeDilation = 1; + if (step_time > m_SkipFramesAtms) + step_time = 0; + m_lastframe = DateTime.UtcNow; // skip also the time lost + } + } + return (float)nodeframes * ODE_STEPSIZE / timeStep; + } + + /// + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + public float GetTerrainHeightAtXY(float x, float y) + { + + int offsetX = 0; + int offsetY = 0; + + if (m_suportCombine) + { + offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + } + + // get region map + IntPtr heightFieldGeom = IntPtr.Zero; + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; + + if (heightFieldGeom == IntPtr.Zero) + return 0f; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples + int regsize = regsizeX; + + if (m_ubOdeLib) + { + if (x < regsizeX - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsizeX - 2; + dx = 0; + } + if (y < regsizeY - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsizeY - 2; + dy = 0; + } + } + else + { + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + + regsize = regsizeY; + + if (x < regsizeX - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsizeX - 2; + dy = 0; + } + if (y < regsizeY - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsizeY - 2; + dx = 0; + } + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + /* + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + */ + h0 = ((float)heights[iy]); // 0,0 vertice + + if (dy>dx) + { + iy += regsize; + h2 = (float)heights[iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 + } + else + { + iy++; + h2 = (float)heights[iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 + } + + return h0 + h1 + h2; + } + + public Vector3 GetTerrainNormalAtXY(float x, float y) + { + int offsetX = 0; + int offsetY = 0; + + if (m_suportCombine) + { + offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + } + + // get region map + IntPtr heightFieldGeom = IntPtr.Zero; + Vector3 norm = new Vector3(0, 0, 1); + + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return norm; ; + + if (heightFieldGeom == IntPtr.Zero) + return norm; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return norm; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples + int regsize = regsizeX; + + int xstep = 1; + int ystep = regsizeX; + bool firstTri = false; + + if (m_ubOdeLib) + { + if (x < regsizeX - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsizeX - 2; + dx = 0; + } + if (y < regsizeY - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsizeY - 2; + dy = 0; + } + firstTri = dy > dx; + } + + else + { + xstep = regsizeY; + ystep = 1; + regsize = regsizeY; + + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsizeX - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsizeX - 2; + dy = 0; + } + if (y < regsizeY - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsizeY - 2; + dx = 0; + } + firstTri = dx > dy; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if (firstTri) + { + h1 = ((float)heights[iy]); // 0,0 vertice + iy += ystep; + h0 = (float)heights[iy]; // 0,1 + h2 = (float)heights[iy+xstep]; // 1,1 vertice + norm.X = h0 - h2; + norm.Y = h1 - h0; + } + else + { + h2 = ((float)heights[iy]); // 0,0 vertice + iy += xstep; + h0 = ((float)heights[iy]); // 1,0 vertice + h1 = (float)heights[iy+ystep]; // vertice 1,1 + norm.X = h2 - h0; + norm.Y = h0 - h1; + } + norm.Z = 1; + norm.Normalize(); + return norm; + } + + public override void SetTerrain(float[] heightMap) + { + if (m_worldOffset != Vector3.Zero && m_parentScene != null) + { + if (m_parentScene is ODEScene) + { + ((ODEScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); + } + } + else + { + SetTerrain(heightMap, m_worldOffset); + } + } + + public override void CombineTerrain(float[] heightMap, Vector3 pOffset) + { + if(m_suportCombine) + SetTerrain(heightMap, pOffset); + } + + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + if (m_ubOdeLib) + ubSetTerrain(heightMap, pOffset); + else + OriSetTerrain(heightMap, pOffset); + } + + public void OriSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; + + // map is rotated + uint heightmapWidth = regionsizeY + 2; + uint heightmapHeight = regionsizeX + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 10f; + const int wrap = 0; + + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + uint xx; + uint yy; + + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; + // flipping map adding one margin all around so things don't fall in edges + + uint xt = 0; + xx = 0; + + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && xx < maxXX) + xx++; + yy = 0; + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxYY) + yy += regionsizeX; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; + } + xt += heightmapHeightSamples; + } + + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + actor_name_map.Remove(GroundGeom); + d.GeomDestroy(GroundGeom); + + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + + GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); + + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; + +// geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } + } + } + + public void ubSetTerrain(float[] heightMap, Vector3 pOffset) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; + + uint heightmapWidth = regionsizeX + 2; + uint heightmapHeight = regionsizeY + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + + float hfmin = float.MaxValue; +// float hfmax = float.MinValue; + float val; + + + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; + // adding one margin all around so things don't fall in edges + + uint xx; + uint yy = 0; + uint yt = 0; + + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxYY) + yy += regionsizeX; + xx = 0; + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && x < maxXX) + xx++; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[yt + x] = val; + + if (hfmin > val) + hfmin = val; +// if (hfmax < val) +// hfmax = val; + } + yt += heightmapWidthSamples; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + actor_name_map.Remove(GroundGeom); + d.GeomDestroy(GroundGeom); + + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + } + } + IntPtr HeightmapData = d.GeomubTerrainDataCreate(); + + const int wrap = 0; + float thickness = hfmin; + if (thickness < 0) + thickness = 1; + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomubTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, + thickness, wrap); + +// d.GeomubTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateubTerrain(GroundSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, 0); + + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[GroundGeom] = pa; + +// geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); + RegionTerrain.Add(pOffset, GroundGeom); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + } + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override bool SupportsCombining() + { + return m_suportCombine; + } + + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + } + + public override void Dispose() + { + if (m_rayCastManager == null) // if this is null we already did dispose + return; + lock (OdeLock) + { + if (m_meshWorker != null) + m_meshWorker.Stop(); + + if (m_rayCastManager != null) + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + } + + lock (_prims) + { + ChangesQueue.Clear(); + foreach (OdePrim prm in _prims) + { + prm.DoAChange(changes.Remove, null); + _collisionEventPrim.Remove(prm); + } + _prims.Clear(); + } + + OdeCharacter[] chtorem; + lock (_characters) + { + chtorem = new OdeCharacter[_characters.Count]; + _characters.CopyTo(chtorem); + } + + ChangesQueue.Clear(); + foreach (OdeCharacter ch in chtorem) + ch.DoAChange(changes.Remove, null); + + + foreach (IntPtr GroundGeom in RegionTerrain.Values) + { + if (GroundGeom != IntPtr.Zero) + d.GeomDestroy(GroundGeom); + } + + + RegionTerrain.Clear(); + + if (TerrainHeightFieldHeightsHandlers.Count > 0) + { + foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) + { + if (gch.IsAllocated) + gch.Free(); + } + } + + TerrainHeightFieldHeightsHandlers.Clear(); + TerrainHeightFieldHeights.Clear(); + + if (ContactgeomsArray != IntPtr.Zero) + Marshal.FreeHGlobal(ContactgeomsArray); + if (GlobalContactsArray != IntPtr.Zero) + Marshal.FreeHGlobal(GlobalContactsArray); + + + d.WorldDestroy(world); + world = IntPtr.Zero; + //d.CloseODE(); + } + } + + public override Dictionary GetTopColliders() + { + Dictionary returncolliders = new Dictionary(); + int cnt = 0; + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.LocalID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } + } + return returncolliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + req.filter = RayFilterFlags.AllPrims; + + m_rayCastManager.QueueRequest(req); + } + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null) + { + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.AllPrims; + + m_rayCastManager.QueueRequest(req); + } + } + + + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) + { + List ourresults = new List(); + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.AllPrims; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; + } + } + + public override bool SupportsRaycastWorldFiltered() + { + return true; + } + + public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) + { + object SyncObject = new object(); + List ourresults = new List(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = filter; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; + } + } + + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) + { + if (actor == null) + return new List(); + + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + + if (geom == IntPtr.Zero) + return new List(); + + List ourResults = null; + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = actor; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeBoxCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.Normal = size; + req.Origin = position; + req.orientation = orientation; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeSphereCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = radius; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + IntPtr geom = IntPtr.Zero;; + + if (actor != null) + { + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + } + + List ourResults = null; + object SyncObject = new object(); + + ProbePlaneCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = plane.W; + req.Normal.X = plane.X; + req.Normal.Y = plane.Y; + req.Normal.Z = plane.Z; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) + { + Util.FireAndForget( delegate + { + ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); + if(sitAvatar != null) + sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); + }); + return 1; + } + + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs new file mode 100644 index 0000000..214205d --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs @@ -0,0 +1,356 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ +// Ubit Umarov 2012 +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// + public class ODESitAvatar + { + private ODEScene m_scene; + private ODERayCastRequestManager m_raymanager; + + public ODESitAvatar(ODEScene pScene, ODERayCastRequestManager raymanager) + { + m_scene = pScene; + m_raymanager = raymanager; + } + + private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); + private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; + + private void RotAroundZ(float x, float y, ref Quaternion ori) + { + double ang = Math.Atan2(y, x); + ang *= 0.5d; + float s = (float)Math.Sin(ang); + float c = (float)Math.Cos(ang); + + ori.X = 0; + ori.Y = 0; + ori.Z = s; + ori.W = c; + } + + + public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) + { + if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + + IntPtr geom = ((OdePrim)actor).prim_geom; + + Vector3 geopos = d.GeomGetPositionOMV(geom); + Quaternion geomOri = d.GeomGetQuaternionOMV(geom); + +// Vector3 geopos = actor.Position; +// Quaternion geomOri = actor.Orientation; + + Quaternion geomInvOri = Quaternion.Conjugate(geomOri); + + Quaternion ori = Quaternion.Identity; + + Vector3 rayDir = geopos + offset - avCameraPosition; + + float raylen = rayDir.Length(); + if (raylen < 0.001f) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + float t = 1 / raylen; + rayDir.X *= t; + rayDir.Y *= t; + rayDir.Z *= t; + + raylen += 30f; // focal point may be far + List rayResults; + + rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); + if (rayResults.Count == 0) + { +/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim + d.AABB aabb; + d.GeomGetAABB(geom, out aabb); + offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); + ori = geomInvOri; + offset *= geomInvOri; + PhysicsSitResponse(1, actor.LocalID, offset, ori); +*/ + PhysicsSitResponse(0, actor.LocalID, offset, ori); + return; + } + + int status = 1; + + offset = rayResults[0].Pos - geopos; + + d.GeomClassID geoclass = d.GeomGetClass(geom); + + if (geoclass == d.GeomClassID.SphereClass) + { + float r = d.GeomSphereGetRadius(geom); + + offset.Normalize(); + offset *= r; + + RotAroundZ(offset.X, offset.Y, ref ori); + + if (r < 0.4f) + { + offset = new Vector3(0, 0, r); + } + else + { + if (offset.Z < 0.4f) + { + t = offset.Z; + float rsq = r * r; + + t = 1.0f / (rsq - t * t); + offset.X *= t; + offset.Y *= t; + offset.Z = 0.4f; + t = rsq - 0.16f; + offset.X *= t; + offset.Y *= t; + } + else if (r > 0.8f && offset.Z > 0.8f * r) + { + status = 3; + avOffset.X = -avOffset.X; + avOffset.Z *= 1.6f; + } + } + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(status, actor.LocalID, offset, ori); + return; + } + + Vector3 norm = rayResults[0].Normal; + + if (norm.Z < -0.4f) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } + + + float SitNormX = -rayDir.X; + float SitNormY = -rayDir.Y; + + Vector3 pivot = geopos + offset; + + float edgeNormalX = norm.X; + float edgeNormalY = norm.Y; + float edgeDirX = -rayDir.X; + float edgeDirY = -rayDir.Y; + Vector3 edgePos = rayResults[0].Pos; + float edgeDist = float.MaxValue; + + bool foundEdge = false; + + if (norm.Z < 0.5f) + { + float rayDist = 4.0f; + + for (int i = 0; i < 6; i++) + { + pivot.X -= 0.01f * norm.X; + pivot.Y -= 0.01f * norm.Y; + pivot.Z -= 0.01f * norm.Z; + + rayDir.X = -norm.X * norm.Z; + rayDir.Y = -norm.Y * norm.Z; + rayDir.Z = 1.0f - norm.Z * norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + edgeNormalX = norm.X; + edgeNormalY = norm.Y; + edgeDirX = -rayDir.X; + edgeDirY = -rayDir.Y; + } + else + { + foundEdge = true; + edgePos = rayResults[0].Pos; + break; + } + } + + if (!foundEdge) + { + PhysicsSitResponse(0, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + else if (norm.Z > 0.866f) + { + float toCamBaseX = avCameraPosition.X - pivot.X; + float toCamBaseY = avCameraPosition.Y - pivot.Y; + float toCamX = toCamBaseX; + float toCamY = toCamBaseY; + + for (int j = 0; j < 4; j++) + { + float rayDist = 1.0f; + float curEdgeDist = 0.0f; + + for (int i = 0; i < 3; i++) + { + pivot.Z -= 0.01f; + rayDir.X = toCamX; + rayDir.Y = toCamY; + rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (rayResults[0].Normal.Z > 0.5f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgeNormalX = rayResults[0].Normal.X; + edgeNormalY = rayResults[0].Normal.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + edgePos = rayResults[0].Pos; + } + break; + } + } + if (foundEdge && edgeDist < 0.2f) + break; + + pivot = geopos + offset; + + switch (j) + { + case 0: + toCamX = -toCamBaseY; + toCamY = toCamBaseX; + break; + case 1: + toCamX = toCamBaseY; + toCamY = -toCamBaseX; + break; + case 2: + toCamX = -toCamBaseX; + toCamY = -toCamBaseY; + break; + default: + break; + } + } + + if (!foundEdge) + { + avOffset.X = -avOffset.X; + avOffset.Z *= 1.6f; + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(3, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + SitNormX = edgeNormalX; + SitNormY = edgeNormalY; + if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) + { + SitNormX = -SitNormX; + SitNormY = -SitNormY; + } + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset = edgePos + avOffset * ori; + offset -= geopos; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(1, actor.LocalID, offset, ori); + return; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..01c8470 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs @@ -0,0 +1,61 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Reflection; +using System.Runtime.InteropServices; +using Mono.Addins; + +// Information about this assembly is defined by the following +// attributes. +// +// change them to the information which is associated with the assembly +// you compile. + +[assembly : AssemblyTitle("OpenSim.Region.PhysicsModule.UbitOde")] +[assembly : AssemblyDescription("Ubit Variation")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("UbitOde")] +[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] +[assembly : AssemblyTrademark("")] +[assembly : AssemblyCulture("")] + +// This sets the default COM visibility of types in the assembly to invisible. +// If you need to expose a type to COM, use [ComVisible(true)] on that type. + +[assembly : ComVisible(false)] + +// The assembly version has following format : +// +// Major.Minor.Build.Revision +// +// You can specify all values by your own or you can build default build and revision +// numbers with the '*' character (the default): + +[assembly : AssemblyVersion("0.8.2.*")] +[assembly: Addin("OpenSim.Region.PhysicsModule.UbitOde", OpenSim.VersionInfo.VersionNumber)] +[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs new file mode 100644 index 0000000..ea37301 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs @@ -0,0 +1,340 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.Diagnostics; +using System.Globalization; +using OpenMetaverse; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenSim.Region.PhysicsModule.ubODEMeshing; + +public class Vertex : IComparable +{ + 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 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; + } + + public Vertex(Vector3 v) + { + vector = v; + } + + public Vertex Clone() + { + return new Vertex(X, Y, Z); + } + + public static Vertex FromAngle(double angle) + { + return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f); + } + + public float Length() + { + return vector.Length(); + } + + public virtual bool Equals(Vertex v, float tolerance) + { + Vertex diff = this - v; + float d = diff.Length(); + if (d < tolerance) + return true; + + return false; + } + + + public int CompareTo(Vertex other) + { + if (X < other.X) + return -1; + + if (X > other.X) + return 1; + + if (Y < other.Y) + return -1; + + if (Y > other.Y) + return 1; + + if (Z < other.Z) + return -1; + + if (Z > other.Z) + return 1; + + 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? + // Well, the raw format uses the english/US notation of numbers + // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1. + // The german notation uses these characters exactly vice versa! + // The Float.ToString() routine is a localized one, giving different results depending on the country + // settings your machine works with. Unusable for a machine readable file format :-( + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.NumberDecimalSeparator = "."; + nfi.NumberDecimalDigits = 6; + + String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi); + + return s1; + } +} + +public class Triangle +{ + public Vertex v1; + public Vertex v2; + public Vertex v3; + + public Triangle(Vertex _v1, Vertex _v2, Vertex _v3) + { + v1 = _v1; + v2 = _v2; + v3 = _v3; + } + + public Triangle(float _v1x,float _v1y,float _v1z, + float _v2x,float _v2y,float _v2z, + float _v3x,float _v3y,float _v3z) + { + v1 = new Vertex(_v1x, _v1y, _v1z); + v2 = new Vertex(_v2x, _v2y, _v2z); + v3 = new Vertex(_v3x, _v3y, _v3z); + } + + public override String ToString() + { + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.CurrencyDecimalDigits = 2; + nfi.CurrencyDecimalSeparator = "."; + + String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">"; + String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">"; + String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">"; + + 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() + { + String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw(); + return output; + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs new file mode 100644 index 0000000..da8f623 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs @@ -0,0 +1,601 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.IO; +using System.Runtime.InteropServices; +using OpenSim.Region.PhysicsModules.SharedBase; +using PrimMesher; +using OpenMetaverse; +using System.Runtime.Serialization; +using System.Runtime.Serialization.Formatters.Binary; + +namespace OpenSim.Region.PhysicsModule.ubODEMeshing +{ + public class MeshBuildingData + { + public Dictionary m_vertices; + public List m_triangles; + public float m_obbXmin; + public float m_obbXmax; + public float m_obbYmin; + public float m_obbYmax; + public float m_obbZmin; + public float m_obbZmax; + public Vector3 m_centroid; + public int m_centroidDiv; + } + + [Serializable()] + public class Mesh : IMesh + { + float[] vertices; + int[] indexes; + Vector3 m_obb; + Vector3 m_obboffset; + [NonSerialized()] + MeshBuildingData m_bdata; + [NonSerialized()] + GCHandle vhandler; + [NonSerialized()] + GCHandle ihandler; + [NonSerialized()] + IntPtr m_verticesPtr = IntPtr.Zero; + [NonSerialized()] + IntPtr m_indicesPtr = IntPtr.Zero; + [NonSerialized()] + int m_vertexCount = 0; + [NonSerialized()] + int m_indexCount = 0; + + public int RefCount { get; set; } + public AMeshKey Key { get; set; } + + 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 Mesh() + { + vertexcomp vcomp = new vertexcomp(); + + m_bdata = new MeshBuildingData(); + m_bdata.m_vertices = new Dictionary(vcomp); + m_bdata.m_triangles = new List(); + m_bdata.m_centroid = Vector3.Zero; + m_bdata.m_centroidDiv = 0; + m_bdata.m_obbXmin = float.MaxValue; + m_bdata.m_obbXmax = float.MinValue; + m_bdata.m_obbYmin = float.MaxValue; + m_bdata.m_obbYmax = float.MinValue; + m_bdata.m_obbZmin = float.MaxValue; + m_bdata.m_obbZmax = float.MinValue; + m_obb = new Vector3(0.5f, 0.5f, 0.5f); + m_obboffset = Vector3.Zero; + } + + + public Mesh Scale(Vector3 scale) + { + if (m_verticesPtr == null || m_indicesPtr == null) + return null; + + Mesh result = new Mesh(); + + float x = scale.X; + float y = scale.Y; + float z = scale.Z; + + result.m_obb.X = m_obb.X * x; + result.m_obb.Y = m_obb.Y * y; + result.m_obb.Z = m_obb.Z * z; + result.m_obboffset.X = m_obboffset.X * x; + result.m_obboffset.Y = m_obboffset.Y * y; + 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++) + { + result.vertices[j] = vertices[j] * x; + j++; + result.vertices[j] = vertices[j] * y; + j++; + result.vertices[j] = vertices[j] * z; + j++; + } + + result.indexes = new int[indexes.Length]; + indexes.CopyTo(result.indexes,0); + + result.pinMemory(); + + return result; + } + + public Mesh Clone() + { + Mesh result = new Mesh(); + + if (m_bdata != null) + { + result.m_bdata = new MeshBuildingData(); + foreach (Triangle t in m_bdata.m_triangles) + { + result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); + } + result.m_bdata.m_centroid = m_bdata.m_centroid; + result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv; + result.m_bdata.m_obbXmin = m_bdata.m_obbXmin; + result.m_bdata.m_obbXmax = m_bdata.m_obbXmax; + result.m_bdata.m_obbYmin = m_bdata.m_obbYmin; + result.m_bdata.m_obbYmax = m_bdata.m_obbYmax; + result.m_bdata.m_obbZmin = m_bdata.m_obbZmin; + result.m_bdata.m_obbZmax = m_bdata.m_obbZmax; + } + result.m_obb = m_obb; + result.m_obboffset = m_obboffset; + return result; + } + + public void addVertexLStats(Vertex v) + { + float x = v.X; + float y = v.Y; + float z = v.Z; + + m_bdata.m_centroid.X += x; + m_bdata.m_centroid.Y += y; + m_bdata.m_centroid.Z += z; + m_bdata.m_centroidDiv++; + + if (x > m_bdata.m_obbXmax) + m_bdata.m_obbXmax = x; + else if (x < m_bdata.m_obbXmin) + m_bdata.m_obbXmin = x; + + if (y > m_bdata.m_obbYmax) + m_bdata.m_obbYmax = y; + else if (y < m_bdata.m_obbYmin) + m_bdata.m_obbYmin = y; + + if (z > m_bdata.m_obbZmax) + m_bdata.m_obbZmax = z; + else if (z < m_bdata.m_obbZmin) + m_bdata.m_obbZmin = z; + + } + + public void Add(Triangle triangle) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Add to a pinned Mesh"); + + + triangle.v1.X = (float)Math.Round(triangle.v1.X, 6); + triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6); + triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6); + triangle.v2.X = (float)Math.Round(triangle.v2.X, 6); + triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6); + triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6); + triangle.v3.X = (float)Math.Round(triangle.v3.X, 6); + triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6); + triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6); + + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) + || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) + || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) + ) + { + return; + } + + if (m_bdata.m_vertices.Count == 0) + { + m_bdata.m_centroidDiv = 0; + m_bdata.m_centroid = Vector3.Zero; + } + + if (!m_bdata.m_vertices.ContainsKey(triangle.v1)) + { + m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v1); + } + if (!m_bdata.m_vertices.ContainsKey(triangle.v2)) + { + m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v2); + } + if (!m_bdata.m_vertices.ContainsKey(triangle.v3)) + { + m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v3); + } + m_bdata.m_triangles.Add(triangle); + } + + public Vector3 GetCentroid() + { + return m_obboffset; + + } + + public Vector3 GetOBB() + { + return m_obb; + float x, y, z; + if (m_bdata.m_centroidDiv > 0) + { + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; + } + else // ?? + { + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + return new Vector3(x, y, z); + } + + public List getVertexList() + { + List result = new List(); + foreach (Vertex v in m_bdata.m_vertices.Keys) + { + result.Add(new Vector3(v.X, v.Y, v.Z)); + } + return result; + } + + public float[] getVertexListAsFloat() + { + if (m_bdata.m_vertices == null) + throw new NotSupportedException(); + float[] result = new float[m_bdata.m_vertices.Count * 3]; + foreach (KeyValuePair kvp in m_bdata.m_vertices) + { + Vertex v = kvp.Key; + int i = kvp.Value; + result[3 * i + 0] = v.X; + result[3 * i + 1] = v.Y; + result[3 * i + 2] = v.Z; + } + return result; + } + + public float[] getVertexListAsFloatLocked() + { + return null; + } + + public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount) + { + // A vertex is 3 floats + vertexStride = 3 * sizeof(float); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero && m_bdata != null) + { + vertices = getVertexListAsFloat(); + // Each vertex is 3 elements (floats) + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); + } + _vertices = m_verticesPtr; + vertexCount = m_vertexCount; + } + + public int[] getIndexListAsInt() + { + if (m_bdata.m_triangles == null) + throw new NotSupportedException(); + int[] result = new int[m_bdata.m_triangles.Count * 3]; + for (int i = 0; i < m_bdata.m_triangles.Count; i++) + { + Triangle t = m_bdata.m_triangles[i]; + result[3 * i + 0] = m_bdata.m_vertices[t.v1]; + result[3 * i + 1] = m_bdata.m_vertices[t.v2]; + result[3 * i + 2] = m_bdata.m_vertices[t.v3]; + } + return result; + } + + /// + /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA + /// + /// + public int[] getIndexListAsIntLocked() + { + return null; + } + + public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount) + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero && m_bdata != null) + { + indexes = getIndexListAsInt(); + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); + } + // A triangle is 3 ints (indices) + triStride = 3 * sizeof(int); + indices = m_indicesPtr; + indexCount = m_indexCount; + } + + public void releasePinned() + { + if (m_verticesPtr != IntPtr.Zero) + { + vhandler.Free(); + vertices = null; + m_verticesPtr = IntPtr.Zero; + } + if (m_indicesPtr != IntPtr.Zero) + { + ihandler.Free(); + indexes = null; + m_indicesPtr = IntPtr.Zero; + } + } + + /// + /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions + /// + public void releaseSourceMeshData() + { + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + } + } + + public void releaseBuildingMeshData() + { + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + m_bdata = null; + } + } + + public void Append(IMesh newMesh) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Append to a pinned Mesh"); + + if (!(newMesh is Mesh)) + return; + + foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles) + Add(t); + } + + // Do a linear transformation of mesh. + public void TransformLinear(float[,] matrix, float[] offset) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); + + foreach (Vertex v in m_bdata.m_vertices.Keys) + { + if (v == null) + continue; + float x, y, z; + x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0]; + y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1]; + z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2]; + v.X = x + offset[0]; + v.Y = y + offset[1]; + v.Z = z + offset[2]; + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + if (m_bdata == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + foreach (Triangle t in m_bdata.m_triangles) + { + String s = t.ToStringRaw(); + sw.WriteLine(s); + } + sw.Close(); + } + + public void TrimExcess() + { + m_bdata.m_triangles.TrimExcess(); + } + + public void pinMemory() + { + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); + + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); + } + + public void PrepForOde() + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero) + vertices = getVertexListAsFloat(); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero) + indexes = getIndexListAsInt(); + + pinMemory(); + + float x, y, z; + + if (m_bdata.m_centroidDiv > 0) + { + m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv); + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; + } + + else + { + m_obboffset = Vector3.Zero; + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + m_obb = new Vector3(x, y, z); + + releaseBuildingMeshData(); + } + public bool ToStream(Stream st) + { + if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero) + return false; + + BinaryWriter bw = new BinaryWriter(st); + bool ok = true; + + try + { + + bw.Write(m_vertexCount); + bw.Write(m_indexCount); + + for (int i = 0; i < 3 * m_vertexCount; i++) + bw.Write(vertices[i]); + for (int i = 0; i < m_indexCount; i++) + bw.Write(indexes[i]); + bw.Write(m_obb.X); + bw.Write(m_obb.Y); + bw.Write(m_obb.Z); + bw.Write(m_obboffset.X); + bw.Write(m_obboffset.Y); + bw.Write(m_obboffset.Z); + } + catch + { + ok = false; + } + + if (bw != null) + { + bw.Flush(); + bw.Close(); + } + + return ok; + } + + public static Mesh FromStream(Stream st, AMeshKey key) + { + Mesh mesh = new Mesh(); + mesh.releaseBuildingMeshData(); + + BinaryReader br = new BinaryReader(st); + + bool ok = true; + try + { + mesh.m_vertexCount = br.ReadInt32(); + mesh.m_indexCount = br.ReadInt32(); + + int n = 3 * mesh.m_vertexCount; + mesh.vertices = new float[n]; + for (int i = 0; i < n; i++) + mesh.vertices[i] = br.ReadSingle(); + + mesh.indexes = new int[mesh.m_indexCount]; + for (int i = 0; i < mesh.m_indexCount; i++) + mesh.indexes[i] = br.ReadInt32(); + + mesh.m_obb.X = br.ReadSingle(); + mesh.m_obb.Y = br.ReadSingle(); + mesh.m_obb.Z = br.ReadSingle(); + + mesh.m_obboffset.X = br.ReadSingle(); + mesh.m_obboffset.Y = br.ReadSingle(); + mesh.m_obboffset.Z = br.ReadSingle(); + } + catch + { + ok = false; + } + + br.Close(); + + if (ok) + { + mesh.pinMemory(); + + mesh.Key = key; + mesh.RefCount = 1; + + return mesh; + } + + mesh.vertices = null; + mesh.indexes = null; + return null; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs new file mode 100644 index 0000000..be76f8f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs @@ -0,0 +1,1458 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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. + */ +//#define SPAM + +using System; +using System.Collections.Generic; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet; +using OpenMetaverse; +using OpenMetaverse.StructuredData; +using System.Drawing; +using System.Drawing.Imaging; +using System.IO.Compression; +using PrimMesher; +using log4net; +using Nini.Config; +using System.Reflection; +using System.IO; +using ComponentAce.Compression.Libs.zlib; +using System.Runtime.Serialization; +using System.Runtime.Serialization.Formatters.Binary; + +using Mono.Addins; + +namespace OpenSim.Region.PhysicsModule.ubODEMeshing +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEMeshmerizer")] + public class UbitMeshmerizer : IMesher, INonSharedRegionModule + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + // Setting baseDir to a path will enable the dumping of raw files + // raw files can be imported by blender so a visual inspection of the results can be done + + private bool m_Enabled = false; + + public object diskLock = new object(); + + public bool doMeshFileCache = true; + + public string cachePath = "MeshCache"; + public TimeSpan CacheExpire; + public bool doCacheExpire = true; + +// const string baseDir = "rawFiles"; + private const string baseDir = null; //"rawFiles"; + + private bool useMeshiesPhysicsMesh = false; + + private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh + + private Dictionary m_uniqueMeshes = new Dictionary(); + private Dictionary m_uniqueReleasedMeshes = new Dictionary(); + + #region INonSharedRegionModule + public string Name + { + get { return "ubODEMeshmerizer"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void Initialise(IConfigSource config) + { + IConfig start_config = config.Configs["Startup"]; + + string mesher = start_config.GetString("meshing", string.Empty); + if (mesher == Name) + { + float fcache = 48.0f; + // float fcache = 0.02f; + + IConfig mesh_config = config.Configs["Mesh"]; + if (mesh_config != null) + { + useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); + if (useMeshiesPhysicsMesh) + { + doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache); + cachePath = mesh_config.GetString("MeshFileCachePath", cachePath); + fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache); + doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire); + } + else + { + doMeshFileCache = false; + doCacheExpire = false; + } + + m_Enabled = true; + } + + CacheExpire = TimeSpan.FromHours(fcache); + + } + } + + public void Close() + { + } + + public void AddRegion(Scene scene) + { + if (!m_Enabled) + return; + + scene.RegisterModuleInterface(this); + } + + public void RemoveRegion(Scene scene) + { + if (!m_Enabled) + return; + + scene.UnregisterModuleInterface(this); + } + + public void RegionLoaded(Scene scene) + { + if (!m_Enabled) + return; + } + + #endregion + + /// + /// creates a simple box mesh of the specified size. This mesh is of very low vertex count and may + /// be useful as a backup proxy when level of detail is not needed or when more complex meshes fail + /// for some reason + /// + /// + /// + /// + /// + /// + /// + /// + private static Mesh CreateSimpleBoxMesh(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) + { + Mesh box = new Mesh(); + List vertices = new List(); + // bottom + + vertices.Add(new Vertex(minX, maxY, minZ)); + vertices.Add(new Vertex(maxX, maxY, minZ)); + vertices.Add(new Vertex(maxX, minY, minZ)); + vertices.Add(new Vertex(minX, minY, minZ)); + + box.Add(new Triangle(vertices[0], vertices[1], vertices[2])); + box.Add(new Triangle(vertices[0], vertices[2], vertices[3])); + + // top + + vertices.Add(new Vertex(maxX, maxY, maxZ)); + vertices.Add(new Vertex(minX, maxY, maxZ)); + vertices.Add(new Vertex(minX, minY, maxZ)); + vertices.Add(new Vertex(maxX, minY, maxZ)); + + box.Add(new Triangle(vertices[4], vertices[5], vertices[6])); + box.Add(new Triangle(vertices[4], vertices[6], vertices[7])); + + // sides + + box.Add(new Triangle(vertices[5], vertices[0], vertices[3])); + box.Add(new Triangle(vertices[5], vertices[3], vertices[6])); + + box.Add(new Triangle(vertices[1], vertices[0], vertices[5])); + box.Add(new Triangle(vertices[1], vertices[5], vertices[4])); + + box.Add(new Triangle(vertices[7], vertices[1], vertices[4])); + box.Add(new Triangle(vertices[7], vertices[2], vertices[1])); + + box.Add(new Triangle(vertices[3], vertices[2], vertices[7])); + box.Add(new Triangle(vertices[3], vertices[7], vertices[6])); + + return box; + } + + /// + /// Creates a simple bounding box mesh for a complex input mesh + /// + /// + /// + private static Mesh CreateBoundingBoxMesh(Mesh meshIn) + { + float minX = float.MaxValue; + float maxX = float.MinValue; + float minY = float.MaxValue; + float maxY = float.MinValue; + float minZ = float.MaxValue; + float maxZ = float.MinValue; + + foreach (Vector3 v in meshIn.getVertexList()) + { + if (v.X < minX) minX = v.X; + if (v.Y < minY) minY = v.Y; + if (v.Z < minZ) minZ = v.Z; + + if (v.X > maxX) maxX = v.X; + if (v.Y > maxY) maxY = v.Y; + if (v.Z > maxZ) maxZ = v.Z; + } + + return CreateSimpleBoxMesh(minX, maxX, minY, maxY, minZ, maxZ); + } + + private void ReportPrimError(string message, string primName, PrimMesh primMesh) + { + m_log.Error(message); + m_log.Error("\nPrim Name: " + primName); + m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString()); + } + + /// + /// Add a submesh to an existing list of coords and faces. + /// + /// + /// Size of entire object + /// + /// + private 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"])) + return; + + OpenMetaverse.Vector3 posMax; + OpenMetaverse.Vector3 posMin; + if (subMeshData.ContainsKey("PositionDomain")) + { + posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); + posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + } + else + { + posMax = new Vector3(0.5f, 0.5f, 0.5f); + posMin = new Vector3(-0.5f, -0.5f, -0.5f); + } + + ushort faceIndexOffset = (ushort)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); + + 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); + } + + byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); + for (int i = 0; i < triangleBytes.Length; i += 6) + { + 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); + } + } + + /// + /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type. + /// + /// + /// + /// + /// + /// + private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex) + { +// m_log.DebugFormat( +// "[MESH]: Creating physics proxy for {0}, shape {1}", +// primName, (OpenMetaverse.SculptType)primShape.SculptType); + + List coords; + List faces; + + if (primShape.SculptEntry) + { + if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh) + { + if (!useMeshiesPhysicsMesh) + return null; + + if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex)) + return null; + } + else + { + if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces)) + return null; + } + } + else + { + if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, out coords, out faces)) + return null; + } + + primShape.SculptData = Utils.EmptyBytes; + + int numCoords = coords.Count; + int numFaces = faces.Count; + + Mesh mesh = new Mesh(); + // Add the corresponding triangles to the mesh + for (int i = 0; i < numFaces; i++) + { + Face f = faces[i]; + mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z, + coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z, + coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); + } + + coords.Clear(); + faces.Clear(); + + return mesh; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim. + /// + /// + /// + /// + /// 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( + string primName, PrimitiveBaseShape primShape, out List coords, out List faces, bool convex) + { +// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); + + bool usemesh = false; + + coords = new List(); + faces = new List(); + OSD meshOsd = null; + + if (primShape.SculptData.Length <= 0) + { +// m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); + return false; + } + + long start = 0; + using (MemoryStream data = new MemoryStream(primShape.SculptData)) + { + try + { + OSD osd = OSDParser.DeserializeLLSDBinary(data); + if (osd is OSDMap) + meshOsd = (OSDMap)osd; + else + { + m_log.Warn("[Mesh}: unable to cast mesh asset to OSDMap"); + return false; + } + } + catch (Exception e) + { + m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString()); + } + + start = data.Position; + } + + if (meshOsd is OSDMap) + { + OSDMap physicsParms = null; + OSDMap map = (OSDMap)meshOsd; + + if (!convex) + { + if (map.ContainsKey("physics_shape")) + physicsParms = (OSDMap)map["physics_shape"]; // old asset format + else if (map.ContainsKey("physics_mesh")) + physicsParms = (OSDMap)map["physics_mesh"]; // new asset format + + if (physicsParms != null) + usemesh = true; + } + + if(!usemesh && (map.ContainsKey("physics_convex"))) + physicsParms = (OSDMap)map["physics_convex"]; + + + if (physicsParms == null) + { + m_log.Warn("[MESH]: unknown mesh type"); + return false; + } + + int physOffset = physicsParms["offset"].AsInteger() + (int)start; + int physSize = physicsParms["size"].AsInteger(); + + if (physOffset < 0 || physSize == 0) + return false; // no mesh data in asset + + OSD decodedMeshOsd = new OSD(); + byte[] meshBytes = new byte[physSize]; + System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize); + + try + { + using (MemoryStream inMs = new MemoryStream(meshBytes)) + { + using (MemoryStream outMs = new MemoryStream()) + { + using (ZOutputStream zOut = new ZOutputStream(outMs)) + { + byte[] readBuffer = new byte[2048]; + int readLen = 0; + while ((readLen = inMs.Read(readBuffer, 0, readBuffer.Length)) > 0) + { + zOut.Write(readBuffer, 0, readLen); + } + zOut.Flush(); + outMs.Seek(0, SeekOrigin.Begin); + + byte[] decompressedBuf = outMs.GetBuffer(); + + decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf); + } + } + } + } + catch (Exception e) + { + m_log.Error("[MESH]: exception decoding physical mesh: " + e.ToString()); + return false; + } + + if (usemesh) + { + OSDArray decodedMeshOsdArray = null; + + // physics_shape is an array of OSDMaps, one for each submesh + if (decodedMeshOsd is OSDArray) + { +// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); + + decodedMeshOsdArray = (OSDArray)decodedMeshOsd; + foreach (OSD subMeshOsd in decodedMeshOsdArray) + { + if (subMeshOsd is OSDMap) + AddSubMesh(subMeshOsd as OSDMap, coords, faces); + } + } + } + else + { + OSDMap cmap = (OSDMap)decodedMeshOsd; + if (cmap == null) + return false; + + byte[] data; + + List vs = new List(); + PHullResult hullr = new PHullResult(); + float3 f3; + Coord c; + Face f; + Vector3 range; + Vector3 min; + + const float invMaxU16 = 1.0f / 65535f; + int t1; + int t2; + int t3; + int i; + int nverts; + int nindexs; + + if (cmap.ContainsKey("Max")) + range = cmap["Max"].AsVector3(); + else + range = new Vector3(0.5f, 0.5f, 0.5f); + + if (cmap.ContainsKey("Min")) + min = cmap["Min"].AsVector3(); + else + min = new Vector3(-0.5f, -0.5f, -0.5f); + + range = range - min; + range *= invMaxU16; + + if (!convex && cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions")) + { + List hsizes = new List(); + int totalpoints = 0; + data = cmap["HullList"].AsBinary(); + for (i = 0; i < data.Length; i++) + { + t1 = data[i]; + if (t1 == 0) + t1 = 256; + totalpoints += t1; + hsizes.Add(t1); + } + + data = cmap["Positions"].AsBinary(); + int ptr = 0; + int vertsoffset = 0; + + if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point + { + foreach (int hullsize in hsizes) + { + for (i = 0; i < hullsize; i++ ) + { + 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) + { + 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 == 4) + { + // 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); + } + vertsoffset += vs.Count; + vs.Clear(); + continue; + } + + if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) + { + vs.Clear(); + continue; + } + + nverts = hullr.Vertices.Count; + nindexs = hullr.Indices.Count; + + if (nindexs % 3 != 0) + { + vs.Clear(); + continue; + } + + for (i = 0; i < nverts; i++) + { + c.X = hullr.Vertices[i].x; + c.Y = hullr.Vertices[i].y; + c.Z = hullr.Vertices[i].z; + coords.Add(c); + } + + for (i = 0; i < nindexs; i += 3) + { + t1 = hullr.Indices[i]; + if (t1 > nverts) + break; + t2 = hullr.Indices[i + 1]; + if (t2 > nverts) + break; + t3 = hullr.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) + return true; + } + + vs.Clear(); + + if (cmap.ContainsKey("BoundingVerts")) + { + 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); + } + + if (vs.Count < 3) + { + vs.Clear(); + return false; + } + + if (vs.Count < 5) + { + foreach (float3 point in vs) + { + c.X = point.x; + c.Y = point.y; + c.Z = point.z; + coords.Add(c); + } + f = new Face(0, 1, 2); + faces.Add(f); + + if (vs.Count == 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); + } + vs.Clear(); + return true; + } + + if (!HullUtils.ComputeHull(vs, ref hullr, 0, 0.0f)) + return false; + + nverts = hullr.Vertices.Count; + nindexs = hullr.Indices.Count; + + if (nindexs % 3 != 0) + return false; + + for (i = 0; i < nverts; i++) + { + c.X = hullr.Vertices[i].x; + c.Y = hullr.Vertices[i].y; + c.Z = hullr.Vertices[i].z; + coords.Add(c); + } + for (i = 0; i < nindexs; i += 3) + { + t1 = hullr.Indices[i]; + if (t1 > nverts) + break; + t2 = hullr.Indices[i + 1]; + if (t2 > nverts) + break; + t3 = hullr.Indices[i + 2]; + if (t3 > nverts) + break; + f = new Face(t1, t2, t3); + faces.Add(f); + } + + if (coords.Count > 0 && faces.Count > 0) + return true; + } + else + return false; + } + } + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim. + /// + /// + /// + /// + /// + /// 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 GenerateCoordsAndFacesFromPrimSculptData( + string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) + { + coords = new List(); + faces = new List(); + PrimMesher.SculptMesh sculptMesh; + Image idata = null; + + if (primShape.SculptData == null || primShape.SculptData.Length == 0) + return false; + + try + { + OpenMetaverse.Imaging.ManagedImage unusedData; + OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata); + + unusedData = null; + + if (idata == null) + { + // In some cases it seems that the decode can return a null bitmap without throwing + // an exception + m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName); + return false; + } + } + catch (DllNotFoundException) + { + m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed. Often times this is because of an old version of GLIBC. You must have version 2.4 or above!"); + return false; + } + catch (IndexOutOfRangeException) + { + m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed"); + return false; + } + catch (Exception ex) + { + m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message); + return false; + } + + PrimMesher.SculptMesh.SculptType sculptType; + // remove mirror and invert bits + OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f)); + switch (pbsSculptType) + { + case OpenMetaverse.SculptType.Cylinder: + sculptType = PrimMesher.SculptMesh.SculptType.cylinder; + break; + case OpenMetaverse.SculptType.Plane: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + case OpenMetaverse.SculptType.Torus: + sculptType = PrimMesher.SculptMesh.SculptType.torus; + break; + case OpenMetaverse.SculptType.Sphere: + sculptType = PrimMesher.SculptMesh.SculptType.sphere; + break; + default: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + } + + bool mirror = ((primShape.SculptType & 128) != 0); + bool invert = ((primShape.SculptType & 64) != 0); + + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert); + + idata.Dispose(); + +// sculptMesh.DumpRaw(baseDir, primName, "primMesh"); + + coords = sculptMesh.coords; + faces = sculptMesh.faces; + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim. + /// + /// + /// + /// + /// 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 GenerateCoordsAndFacesFromPrimShapeData( + string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) + { + PrimMesh primMesh; + coords = new List(); + faces = new List(); + + float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f; + float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f; + float pathBegin = (float)primShape.PathBegin * 2.0e-5f; + float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f; + float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f; + float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f; + + float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f; + float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f; + + if (profileBegin < 0.0f) + profileBegin = 0.0f; + + if (profileEnd < 0.02f) + profileEnd = 0.02f; + else if (profileEnd > 1.0f) + profileEnd = 1.0f; + + if (profileBegin >= profileEnd) + profileBegin = profileEnd - 0.02f; + + float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f; + if (profileHollow > 0.95f) + profileHollow = 0.95f; + + int sides = 4; + LevelOfDetail iLOD = (LevelOfDetail)lod; + byte profshape = (byte)(primShape.ProfileCurve & 0x07); + + if (profshape == (byte)ProfileShape.EquilateralTriangle + || profshape == (byte)ProfileShape.IsometricTriangle + || profshape == (byte)ProfileShape.RightTriangle) + sides = 3; + else if (profshape == (byte)ProfileShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + } + else if (profshape == (byte)ProfileShape.HalfCircle) + { // half circle, prim is a sphere + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + + profileBegin = 0.5f * profileBegin + 0.5f; + profileEnd = 0.5f * profileEnd + 0.5f; + } + + int hollowSides = sides; + if (primShape.HollowShape == HollowShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: hollowSides = 24; break; + case LevelOfDetail.Medium: hollowSides = 12; break; + case LevelOfDetail.Low: hollowSides = 6; break; + case LevelOfDetail.VeryLow: hollowSides = 3; break; + default: hollowSides = 24; break; + } + } + else if (primShape.HollowShape == HollowShape.Square) + hollowSides = 4; + else if (primShape.HollowShape == HollowShape.Triangle) + { + if (profshape == (byte)ProfileShape.HalfCircle) + hollowSides = 6; + else + hollowSides = 3; + } + + primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides); + + if (primMesh.errorMessage != null) + if (primMesh.errorMessage.Length > 0) + m_log.Error("[ERROR] " + primMesh.errorMessage); + + primMesh.topShearX = pathShearX; + primMesh.topShearY = pathShearY; + primMesh.pathCutBegin = pathBegin; + primMesh.pathCutEnd = pathEnd; + + if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible) + { + primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10; + primMesh.twistEnd = (primShape.PathTwist * 18) / 10; + primMesh.taperX = pathScaleX; + primMesh.taperY = pathScaleY; + +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeLinear(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + else + { + primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f; + primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f; + primMesh.radius = 0.01f * primShape.PathRadiusOffset; + primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions; + primMesh.skew = 0.01f * primShape.PathSkew; + primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10; + primMesh.twistEnd = (primShape.PathTwist * 36) / 10; + primMesh.taperX = primShape.PathTaperX * 0.01f; + primMesh.taperY = primShape.PathTaperY * 0.01f; + +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeCircular(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + +// primMesh.DumpRaw(baseDir, primName, "primMesh"); + + coords = primMesh.coords; + faces = primMesh.faces; + + return true; + } + + public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex) + { + AMeshKey key = new AMeshKey(); + Byte[] someBytes; + + key.hashB = 5181; + key.hashC = 5181; + ulong hash = 5381; + + if (primShape.SculptEntry) + { + key.uuid = primShape.SculptTexture; + key.hashC = mdjb2(key.hashC, primShape.SculptType); + key.hashC = mdjb2(key.hashC, primShape.PCode); + } + else + { + hash = mdjb2(hash, primShape.PathCurve); + hash = mdjb2(hash, (byte)primShape.HollowShape); + hash = mdjb2(hash, (byte)primShape.ProfileShape); + hash = mdjb2(hash, primShape.PathBegin); + hash = mdjb2(hash, primShape.PathEnd); + hash = mdjb2(hash, primShape.PathScaleX); + hash = mdjb2(hash, primShape.PathScaleY); + hash = mdjb2(hash, primShape.PathShearX); + key.hashA = hash; + hash = key.hashB; + hash = mdjb2(hash, primShape.PathShearY); + hash = mdjb2(hash, (byte)primShape.PathTwist); + hash = mdjb2(hash, (byte)primShape.PathTwistBegin); + hash = mdjb2(hash, (byte)primShape.PathRadiusOffset); + hash = mdjb2(hash, (byte)primShape.PathTaperX); + hash = mdjb2(hash, (byte)primShape.PathTaperY); + hash = mdjb2(hash, primShape.PathRevolutions); + hash = mdjb2(hash, (byte)primShape.PathSkew); + hash = mdjb2(hash, primShape.ProfileBegin); + hash = mdjb2(hash, primShape.ProfileEnd); + hash = mdjb2(hash, primShape.ProfileHollow); + hash = mdjb2(hash, primShape.PCode); + key.hashB = hash; + } + + hash = key.hashC; + + hash = mdjb2(hash, lod); + + if (size == m_MeshUnitSize) + { + hash = hash << 8; + hash |= 8; + } + else + { + someBytes = size.GetBytes(); + for (int i = 0; i < someBytes.Length; i++) + hash = mdjb2(hash, someBytes[i]); + hash = hash << 8; + } + + if (convex) + hash |= 4; + + if (primShape.SculptEntry) + { + hash |= 1; + if (primShape.SculptType == (byte)SculptType.Mesh) + hash |= 2; + } + + key.hashC = hash; + + return key; + } + + private ulong mdjb2(ulong hash, byte c) + { + return ((hash << 5) + hash) + (ulong)c; + } + + private ulong mdjb2(ulong hash, ushort c) + { + hash = ((hash << 5) + hash) + (ulong)((byte)c); + return ((hash << 5) + hash) + (ulong)(c >> 8); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) + { + return CreateMesh(primName, primShape, size, lod, false,false,false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) + { + return CreateMesh(primName, primShape, size, lod, false,false,false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, false, false, false); + } + + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + Mesh mesh = null; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex); + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + mesh.RefCount = 1; + return mesh; + } + } + } + return null; + } + + private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f); + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) + { +#if SPAM + m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); +#endif + + Mesh mesh = null; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + // try to find a identical mesh on meshs in use + + AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex); + + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + mesh.RefCount = 1; + return mesh; + } + } + } + + Mesh UnitMesh = null; + AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex); + + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh); + if (UnitMesh != null) + { + UnitMesh.RefCount = 1; + } + } + + if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache) + UnitMesh = GetFromFileCache(unitKey); + + if (UnitMesh == null) + { + UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex); + + if (UnitMesh == null) + return null; + + UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z"); + + if (forOde) + { + // force pinned mem allocation + UnitMesh.PrepForOde(); + } + else + UnitMesh.TrimExcess(); + + UnitMesh.Key = unitKey; + UnitMesh.RefCount = 1; + + if (doMeshFileCache && primShape.SculptEntry) + StoreToFileCache(unitKey, UnitMesh); + + lock (m_uniqueReleasedMeshes) + { + try + { + m_uniqueReleasedMeshes.Add(unitKey, UnitMesh); + } + catch { } + } + } + + mesh = UnitMesh.Scale(size); + mesh.Key = key; + mesh.RefCount = 1; + lock (m_uniqueMeshes) + { + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + } + + return mesh; + } + + public void ReleaseMesh(IMesh imesh) + { + if (imesh == null) + return; + + Mesh mesh = (Mesh)imesh; + + lock (m_uniqueMeshes) + { + int curRefCount = mesh.RefCount; + curRefCount--; + + if (curRefCount > 0) + { + mesh.RefCount = curRefCount; + return; + } + + mesh.RefCount = 0; + m_uniqueMeshes.Remove(mesh.Key); + lock (m_uniqueReleasedMeshes) + { + try + { + m_uniqueReleasedMeshes.Add(mesh.Key, mesh); + } + catch { } + } + } + } + + public void ExpireReleaseMeshs() + { + if (m_uniqueReleasedMeshes.Count == 0) + return; + + List meshstodelete = new List(); + int refcntr; + + lock (m_uniqueReleasedMeshes) + { + foreach (Mesh m in m_uniqueReleasedMeshes.Values) + { + refcntr = m.RefCount; + refcntr--; + if (refcntr > -6) + m.RefCount = refcntr; + else + meshstodelete.Add(m); + } + + foreach (Mesh m in meshstodelete) + { + m_uniqueReleasedMeshes.Remove(m.Key); + m.releaseBuildingMeshData(); + m.releasePinned(); + } + } + } + + public void FileNames(AMeshKey key, out string dir,out string fullFileName) + { + string id = key.ToString(); + string init = id.Substring(0, 1); + dir = System.IO.Path.Combine(cachePath, init); + fullFileName = System.IO.Path.Combine(dir, id); + } + + public string FullFileName(AMeshKey key) + { + string id = key.ToString(); + string init = id.Substring(0,1); + id = System.IO.Path.Combine(init, id); + id = System.IO.Path.Combine(cachePath, id); + return id; + } + + private Mesh GetFromFileCache(AMeshKey key) + { + Mesh mesh = null; + string filename = FullFileName(key); + bool ok = true; + + lock (diskLock) + { + if (File.Exists(filename)) + { + FileStream stream = null; + try + { + stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read); + BinaryFormatter bformatter = new BinaryFormatter(); + + mesh = Mesh.FromStream(stream, key); + + } + catch (Exception e) + { + ok = false; + m_log.ErrorFormat( + "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}", + filename, e.Message, e.StackTrace); + } + + if (stream != null) + stream.Close(); + + if (mesh == null || !ok) + File.Delete(filename); + else + File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); + } + } + + return mesh; + } + + private void StoreToFileCache(AMeshKey key, Mesh mesh) + { + Stream stream = null; + bool ok = false; + + // Make sure the target cache directory exists + string dir = String.Empty; + string filename = String.Empty; + + FileNames(key, out dir, out filename); + + lock (diskLock) + { + try + { + if (!Directory.Exists(dir)) + { + Directory.CreateDirectory(dir); + } + + stream = File.Open(filename, FileMode.Create); + ok = mesh.ToStream(stream); + } + catch (IOException e) + { + m_log.ErrorFormat( + "[MESH CACHE]: Failed to write file {0}. Exception {1} {2}.", + filename, e.Message, e.StackTrace); + ok = false; + } + + if (stream != null) + stream.Close(); + + if (File.Exists(filename)) + { + if (ok) + File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); + else + File.Delete(filename); + } + } + } + + public void ExpireFileCache() + { + if (!doCacheExpire) + return; + + string controlfile = System.IO.Path.Combine(cachePath, "cntr"); + + lock (diskLock) + { + try + { + if (File.Exists(controlfile)) + { + int ndeleted = 0; + int totalfiles = 0; + int ndirs = 0; + DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire; + File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow); + + foreach (string dir in Directory.GetDirectories(cachePath)) + { + try + { + foreach (string file in Directory.GetFiles(dir)) + { + try + { + if (File.GetLastAccessTimeUtc(file) < OlderTime) + { + File.Delete(file); + ndeleted++; + } + } + catch { } + totalfiles++; + } + } + catch { } + ndirs++; + } + + if (ndeleted == 0) + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires", + totalfiles,ndirs); + else + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}", + totalfiles,ndirs, ndeleted, OlderTime.ToString()); + } + else + { + m_log.Info("[MESH CACHE]: Expire delayed to next startup"); + FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough); + fs.Close(); + } + } + catch { } + } + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs new file mode 100644 index 0000000..8eb136b --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs @@ -0,0 +1,1708 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.Text; +using System.IO; + +namespace PrimMesher +{ + public struct Quat + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + /// W value + public float W; + + public Quat(float x, float y, float z, float w) + { + X = x; + Y = y; + Z = z; + W = w; + } + + public Quat(Coord axis, float angle) + { + axis = axis.Normalize(); + + angle *= 0.5f; + float c = (float)Math.Cos(angle); + float s = (float)Math.Sin(angle); + + X = axis.X * s; + Y = axis.Y * s; + Z = axis.Z * s; + W = c; + + Normalize(); + } + + public float Length() + { + return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); + } + + public Quat Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1f / mag; + X *= oomag; + Y *= oomag; + Z *= oomag; + W *= oomag; + } + else + { + X = 0f; + Y = 0f; + Z = 0f; + W = 1f; + } + + return this; + } + + public static Quat operator *(Quat q1, Quat q2) + { + float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; + float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X; + float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W; + float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; + return new Quat(x, y, z, w); + } + + public override string ToString() + { + return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">"; + } + } + + public struct Coord + { + public float X; + public float Y; + public float Z; + + public Coord(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + public float Length() + { + return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z); + } + + public Coord Invert() + { + this.X = -this.X; + this.Y = -this.Y; + this.Z = -this.Z; + + return this; + } + + public Coord Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1.0f / mag; + this.X *= oomag; + this.Y *= oomag; + this.Z *= oomag; + } + else + { + this.X = 0.0f; + this.Y = 0.0f; + this.Z = 0.0f; + } + + return this; + } + + public override string ToString() + { + return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString(); + } + + public static Coord Cross(Coord c1, Coord c2) + { + return new Coord( + c1.Y * c2.Z - c2.Y * c1.Z, + c1.Z * c2.X - c2.Z * c1.X, + c1.X * c2.Y - c2.X * c1.Y + ); + } + + public static Coord operator +(Coord v, Coord a) + { + return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z); + } + + public static Coord operator *(Coord v, Coord m) + { + return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z); + } + + public static Coord operator *(Coord v, Quat q) + { + // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + Coord c2 = new Coord(0.0f, 0.0f, 0.0f); + + c2.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; + + c2.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; + + c2.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 c2; + } + } + + public struct Face + { + public int primFace; + + // vertices + public int v1; + public int v2; + public int v3; + + public Face(int v1, int v2, int v3) + { + primFace = 0; + + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + + } + + public Coord SurfaceNormal(List coordList) + { + Coord c1 = coordList[this.v1]; + Coord c2 = coordList[this.v2]; + Coord c3 = coordList[this.v3]; + + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + return Coord.Cross(edge1, edge2).Normalize(); + } + } + + internal struct Angle + { + internal float angle; + internal float X; + internal float Y; + + internal Angle(float angle, float x, float y) + { + this.angle = angle; + this.X = x; + this.Y = y; + } + } + + internal class AngleList + { + private float iX, iY; // intersection point + + private static Angle[] angles3 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles4 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles6 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles12 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles24 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private Angle interpolatePoints(float newPoint, Angle p1, Angle p2) + { + float m = (newPoint - p1.angle) / (p2.angle - p1.angle); + return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y)); + } + + private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) + { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ + double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); + double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3); + + if (denom != 0.0) + { + double ua = uaNumerator / denom; + iX = (float)(x1 + ua * (x2 - x1)); + iY = (float)(y1 + ua * (y2 - y1)); + } + } + + internal List angles; + + internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut) + { + angles = new List(); + + const double twoPi = System.Math.PI * 2.0; + const float twoPiInv = (float)(1.0d / twoPi); + + if (sides < 1) + throw new Exception("number of sides not greater than zero"); + if (stopAngle <= startAngle) + throw new Exception("stopAngle not greater than startAngle"); + + if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24)) + { + startAngle *= twoPiInv; + stopAngle *= twoPiInv; + + Angle[] sourceAngles; + switch (sides) + { + case 3: + sourceAngles = angles3; + break; + case 4: + sourceAngles = angles4; + break; + case 6: + sourceAngles = angles6; + break; + case 12: + sourceAngles = angles12; + break; + default: + sourceAngles = angles24; + break; + } + + int startAngleIndex = (int)(startAngle * sides); + int endAngleIndex = sourceAngles.Length - 1; + + if (hasCut) + { + if (stopAngle < 1.0f) + endAngleIndex = (int)(stopAngle * sides) + 1; + if (endAngleIndex == startAngleIndex) + endAngleIndex++; + + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) + { + angles.Add(sourceAngles[angleIndex]); + } + + if (startAngle > 0.0f) + angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); + + if (stopAngle < 1.0f) + { + int lastAngleIndex = angles.Count - 1; + angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); + } + } + else + { + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++) + angles.Add(sourceAngles[angleIndex]); + } + } + else + { + double stepSize = twoPi / sides; + + int startStep = (int)(startAngle / stepSize); + double angle = stepSize * startStep; + int step = startStep; + double stopAngleTest = stopAngle; + if (stopAngle < twoPi) + { + stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1); + if (stopAngleTest < stopAngle) + stopAngleTest += stepSize; + if (stopAngleTest > twoPi) + stopAngleTest = twoPi; + } + + while (angle <= stopAngleTest) + { + Angle newAngle; + newAngle.angle = (float)angle; + newAngle.X = (float)System.Math.Cos(angle); + newAngle.Y = (float)System.Math.Sin(angle); + angles.Add(newAngle); + step += 1; + angle = stepSize * step; + } + + if (startAngle > angles[0].angle) + { + Angle newAngle; + intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle)); + newAngle.angle = startAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[0] = newAngle; + } + + int index = angles.Count - 1; + if (stopAngle < angles[index].angle) + { + Angle newAngle; + intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle)); + newAngle.angle = stopAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[index] = newAngle; + } + } + } + } + + /// + /// generates a profile for extrusion + /// + public class Profile + { + private const float twoPi = 2.0f * (float)Math.PI; + + public string errorMessage = null; + + public List coords; + public List faces; + + // use these for making individual meshes for each prim face + public List outerCoordIndices = null; + public List hollowCoordIndices = null; + + public int numOuterVerts = 0; + public int numHollowVerts = 0; + + public int outerFaceNumber = -1; + public int hollowFaceNumber = -1; + + public int bottomFaceNumber = 0; + public int numPrimFaces = 0; + + public Profile() + { + this.coords = new List(); + this.faces = new List(); + } + + public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces) + { + const float halfSqr2 = 0.7071067811866f; + + this.coords = new List(); + this.faces = new List(); + + List hollowCoords = new List(); + + bool hasHollow = (hollow > 0.0f); + + AngleList angles = new AngleList(); + AngleList hollowAngles = new AngleList(); + + float xScale = 0.5f; + float yScale = 0.5f; + if (sides == 4) // corners of a square are sqrt(2) from center + { + xScale = halfSqr2; + yScale = halfSqr2; + } + + float startAngle = profileStart * twoPi; + float stopAngle = profileEnd * twoPi; + + try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); } + catch (Exception ex) + { + + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + + this.numOuterVerts = angles.angles.Count; + + Angle angle; + Coord newVert = new Coord(); + + // flag to create as few triangles as possible for 3 or 4 side profile + bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut); + + if (hasHollow) + { + if (sides == hollowSides) + hollowAngles = angles; + else + { + try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); } + catch (Exception ex) + { + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + + int numHollowAngles = hollowAngles.angles.Count; + for (int i = 0; i < numHollowAngles; i++) + { + angle = hollowAngles.angles[i]; + newVert.X = hollow * xScale * angle.X; + newVert.Y = hollow * yScale * angle.Y; + newVert.Z = 0.0f; + + hollowCoords.Add(newVert); + } + } + this.numHollowVerts = hollowAngles.angles.Count; + } + else if (!simpleFace) + { + Coord center = new Coord(0.0f, 0.0f, 0.0f); + this.coords.Add(center); + } + + int numAngles = angles.angles.Count; + bool hollowsame = (hasHollow && hollowSides == sides); + + for (int i = 0; i < numAngles; i++) + { + angle = angles.angles[i]; + newVert.X = angle.X * xScale; + newVert.Y = angle.Y * yScale; + newVert.Z = 0.0f; + this.coords.Add(newVert); + if (hollowsame) + { + newVert.X *= hollow; + newVert.Y *= hollow; + hollowCoords.Add(newVert); + } + } + + if (hasHollow) + { + hollowCoords.Reverse(); + this.coords.AddRange(hollowCoords); + + if (createFaces) + { + int numTotalVerts = this.numOuterVerts + this.numHollowVerts; + + if (this.numOuterVerts == this.numHollowVerts) + { + Face newFace = new Face(); + + for (int coordIndex = 0; coordIndex < this.numOuterVerts - 1; coordIndex++) + { + newFace.v1 = coordIndex; + newFace.v2 = coordIndex + 1; + newFace.v3 = numTotalVerts - coordIndex - 1; + this.faces.Add(newFace); + + newFace.v1 = coordIndex + 1; + newFace.v2 = numTotalVerts - coordIndex - 2; + newFace.v3 = numTotalVerts - coordIndex - 1; + this.faces.Add(newFace); + } + if (!hasProfileCut) + { + newFace.v1 = this.numOuterVerts - 1; + newFace.v2 = 0; + newFace.v3 = this.numOuterVerts; + this.faces.Add(newFace); + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - 1; + newFace.v3 = this.numOuterVerts; + this.faces.Add(newFace); + } + } + else if (this.numOuterVerts < this.numHollowVerts) + { + Face newFace = new Face(); + int j = 0; // j is the index for outer vertices + int i; + int maxJ = this.numOuterVerts - 1; + float curHollowAngle = 0; + for (i = 0; i < this.numHollowVerts; i++) // i is the index for inner vertices + { + curHollowAngle = hollowAngles.angles[i].angle; + if (j < maxJ) + { + if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = j; + newFace.v3 = j + 1; + this.faces.Add(newFace); + j++; + } + } + else + { + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + break; + } + + newFace.v1 = j; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; + + this.faces.Add(newFace); + } + + if (!hasProfileCut) + { + if (i == this.numHollowVerts) + { + newFace.v1 = numTotalVerts - this.numHollowVerts; + newFace.v2 = maxJ; + newFace.v3 = 0; + + this.faces.Add(newFace); + } + else + { + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = maxJ; + newFace.v3 = 0; + + this.faces.Add(newFace); + } + + for (; i < this.numHollowVerts - 1; i++) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; + + this.faces.Add(newFace); + } + } + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - this.numHollowVerts; + newFace.v3 = numTotalVerts - 1; + this.faces.Add(newFace); + } + } + else // numHollowVerts < numOuterVerts + { + Face newFace = new Face(); + int j = 0; // j is the index for inner vertices + int maxJ = this.numHollowVerts - 1; + for (int i = 0; i < this.numOuterVerts; i++) + { + if (j < maxJ) + if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) + { + newFace.v1 = i; + newFace.v2 = numTotalVerts - j - 2; + newFace.v3 = numTotalVerts - j - 1; + + this.faces.Add(newFace); + j += 1; + } + + newFace.v1 = numTotalVerts - j - 1; + newFace.v2 = i; + newFace.v3 = i + 1; + + this.faces.Add(newFace); + } + + if (!hasProfileCut) + { + int i = this.numOuterVerts - 1; + + if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - maxJ - 1; + newFace.v3 = numTotalVerts - 1; + + this.faces.Add(newFace); + } + + newFace.v1 = numTotalVerts - maxJ - 1; + newFace.v2 = i; + newFace.v3 = 0; + + this.faces.Add(newFace); + } + } + } + + } + + else if (createFaces) + { + if (simpleFace) + { + if (sides == 3) + this.faces.Add(new Face(0, 1, 2)); + else if (sides == 4) + { + this.faces.Add(new Face(0, 1, 2)); + this.faces.Add(new Face(0, 2, 3)); + } + } + else + { + for (int i = 1; i < numAngles ; i++) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = i; + newFace.v3 = i + 1; + this.faces.Add(newFace); + } + if (!hasProfileCut) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = numAngles; + newFace.v3 = 1; + this.faces.Add(newFace); + } + } + } + + + hollowCoords = null; + } + + + public Profile Copy() + { + return this.Copy(true); + } + + public Profile Copy(bool needFaces) + { + Profile copy = new Profile(); + + copy.coords.AddRange(this.coords); + + if (needFaces) + copy.faces.AddRange(this.faces); + + copy.numOuterVerts = this.numOuterVerts; + copy.numHollowVerts = this.numHollowVerts; + + return copy; + } + + public void AddPos(Coord v) + { + this.AddPos(v.X, v.Y, v.Z); + } + + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + } + + public void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + } + + public void Scale(float x, float y) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X *= x; + vert.Y *= y; + this.coords[i] = vert; + } + } + + /// + /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices + /// + public void FlipNormals() + { + int i; + int numFaces = this.faces.Count; + Face tmpFace; + int tmp; + + for (i = 0; i < numFaces; i++) + { + tmpFace = this.faces[i]; + tmp = tmpFace.v3; + tmpFace.v3 = tmpFace.v1; + tmpFace.v1 = tmp; + this.faces[i] = tmpFace; + } + } + + public void AddValue2FaceVertexIndices(int num) + { + int numFaces = this.faces.Count; + Face tmpFace; + for (int i = 0; i < numFaces; i++) + { + tmpFace = this.faces[i]; + tmpFace.v1 += num; + tmpFace.v2 += num; + tmpFace.v3 += num; + + this.faces[i] = tmpFace; + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } + + public struct PathNode + { + public Coord position; + public Quat rotation; + public float xScale; + public float yScale; + public float percentOfPath; + } + + public enum PathType { Linear = 0, Circular = 1, Flexible = 2 } + + public class Path + { + public List pathNodes = new List(); + + public float twistBegin = 0.0f; + public float twistEnd = 0.0f; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private const float twoPi = 2.0f * (float)Math.PI; + + public void Create(PathType pathType, int steps) + { + if (this.taperX > 0.999f) + this.taperX = 0.999f; + if (this.taperX < -0.999f) + this.taperX = -0.999f; + if (this.taperY > 0.999f) + this.taperY = 0.999f; + if (this.taperY < -0.999f) + this.taperY = -0.999f; + + if (pathType == PathType.Linear || pathType == PathType.Flexible) + { + int step = 0; + + float length = this.pathCutEnd - this.pathCutBegin; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float start = -0.5f; + float stepSize = length / (float)steps; + float percentOfPathMultiplier = stepSize * 0.999999f; + float xOffset = this.topShearX * this.pathCutBegin; + float yOffset = this.topShearY * this.pathCutBegin; + float zOffset = start; + float xOffsetStepIncrement = this.topShearX * length / steps; + float yOffsetStepIncrement = this.topShearY * length / steps; + + float percentOfPath = this.pathCutBegin; + zOffset += percentOfPath; + + // sanity checks + + bool done = false; + + while (!done) + { + PathNode newNode = new PathNode(); + + newNode.xScale = 1.0f; + if (this.taperX == 0.0f) + newNode.xScale = 1.0f; + else if (this.taperX > 0.0f) + newNode.xScale = 1.0f - percentOfPath * this.taperX; + else newNode.xScale = 1.0f + (1.0f - percentOfPath) * this.taperX; + + newNode.yScale = 1.0f; + if (this.taperY == 0.0f) + newNode.yScale = 1.0f; + else if (this.taperY > 0.0f) + newNode.yScale = 1.0f - percentOfPath * this.taperY; + else newNode.yScale = 1.0f + (1.0f - percentOfPath) * this.taperY; + + float twist = twistBegin + twistTotal * percentOfPath; + + newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + newNode.position = new Coord(xOffset, yOffset, zOffset); + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + if (step < steps) + { + step += 1; + percentOfPath += percentOfPathMultiplier; + xOffset += xOffsetStepIncrement; + yOffset += yOffsetStepIncrement; + zOffset += stepSize; + if (percentOfPath > this.pathCutEnd) + done = true; + } + else done = true; + } + } // end of linear path code + + else // pathType == Circular + { + float twistTotal = twistEnd - twistBegin; + + // if the profile has a lot of twist, add more layers otherwise the layers may overlap + // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't + // accurately match the viewer + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + { + if (twistTotalAbs > Math.PI * 1.5f) + steps *= 2; + if (twistTotalAbs > Math.PI * 3.0f) + steps *= 2; + } + + float yPathScale = this.holeSizeY * 0.5f; + float pathLength = this.pathCutEnd - this.pathCutBegin; + float totalSkew = this.skew * 2.0f * pathLength; + float skewStart = this.pathCutBegin * 2.0f * this.skew - this.skew; + float xOffsetTopShearXFactor = this.topShearX * (0.25f + 0.5f * (0.5f - this.holeSizeY)); + float yShearCompensation = 1.0f + Math.Abs(this.topShearY) * 0.25f; + + // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end + // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used + // to calculate the sine for generating the path radius appears to approximate it's effects there + // too, but there are some subtle differences in the radius which are noticeable as the prim size + // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on + // the meshes generated with this technique appear nearly identical in shape to the same prims when + // displayed by the viewer. + + float startAngle = (twoPi * this.pathCutBegin * this.revolutions) - this.topShearY * 0.9f; + float endAngle = (twoPi * this.pathCutEnd * this.revolutions) - this.topShearY * 0.9f; + float stepSize = twoPi / this.stepsPerRevolution; + + int step = (int)(startAngle / stepSize); + float angle = startAngle; + + bool done = false; + while (!done) // loop through the length of the path and add the layers + { + PathNode newNode = new PathNode(); + + float xProfileScale = (1.0f - Math.Abs(this.skew)) * this.holeSizeX; + float yProfileScale = this.holeSizeY; + + float percentOfPath = angle / (twoPi * this.revolutions); + float percentOfAngles = (angle - startAngle) / (endAngle - startAngle); + + if (this.taperX > 0.01f) + xProfileScale *= 1.0f - percentOfPath * this.taperX; + else if (this.taperX < -0.01f) + xProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperX; + + if (this.taperY > 0.01f) + yProfileScale *= 1.0f - percentOfPath * this.taperY; + else if (this.taperY < -0.01f) + yProfileScale *= 1.0f + (1.0f - percentOfPath) * this.taperY; + + newNode.xScale = xProfileScale; + newNode.yScale = yProfileScale; + + float radiusScale = 1.0f; + if (this.radius > 0.001f) + radiusScale = 1.0f - this.radius * percentOfPath; + else if (this.radius < 0.001f) + radiusScale = 1.0f + this.radius * (1.0f - percentOfPath); + + float twist = twistBegin + twistTotal * percentOfPath; + + float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles); + xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor; + + float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale; + + float zOffset = (float)Math.Sin(angle + this.topShearY) * (0.5f - yPathScale) * radiusScale; + + newNode.position = new Coord(xOffset, yOffset, zOffset); + + // now orient the rotation of the profile layer relative to it's position on the path + // adding taperY to the angle used to generate the quat appears to approximate the viewer + + newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + this.topShearY); + + // next apply twist rotation to the profile layer + if (twistTotal != 0.0f || twistBegin != 0.0f) + newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + // calculate terms for next iteration + // calculate the angle for the next iteration of the loop + + if (angle >= endAngle - 0.01) + done = true; + else + { + step += 1; + angle = stepSize * step; + if (angle > endAngle) + angle = endAngle; + } + } + } + } + } + + public class PrimMesh + { + public string errorMessage = ""; + private const float twoPi = 2.0f * (float)Math.PI; + + public List coords; +// public List normals; + public List faces; + + private int sides = 4; + private int hollowSides = 4; + private float profileStart = 0.0f; + private float profileEnd = 1.0f; + private float hollow = 0.0f; + public int twistBegin = 0; + public int twistEnd = 0; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private bool hasProfileCut = false; + private bool hasHollow = false; + + public int numPrimFaces = 0; + + /// + /// Human readable string representation of the parameters used to create a mesh. + /// + /// + public string ParamsToDisplayString() + { + string s = ""; + s += "sides..................: " + this.sides.ToString(); + s += "\nhollowSides..........: " + this.hollowSides.ToString(); + s += "\nprofileStart.........: " + this.profileStart.ToString(); + s += "\nprofileEnd...........: " + this.profileEnd.ToString(); + s += "\nhollow...............: " + this.hollow.ToString(); + s += "\ntwistBegin...........: " + this.twistBegin.ToString(); + s += "\ntwistEnd.............: " + this.twistEnd.ToString(); + s += "\ntopShearX............: " + this.topShearX.ToString(); + s += "\ntopShearY............: " + this.topShearY.ToString(); + s += "\npathCutBegin.........: " + this.pathCutBegin.ToString(); + s += "\npathCutEnd...........: " + this.pathCutEnd.ToString(); + s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString(); + s += "\ndimpleEnd............: " + this.dimpleEnd.ToString(); + s += "\nskew.................: " + this.skew.ToString(); + s += "\nholeSizeX............: " + this.holeSizeX.ToString(); + s += "\nholeSizeY............: " + this.holeSizeY.ToString(); + s += "\ntaperX...............: " + this.taperX.ToString(); + s += "\ntaperY...............: " + this.taperY.ToString(); + s += "\nradius...............: " + this.radius.ToString(); + s += "\nrevolutions..........: " + this.revolutions.ToString(); + s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString(); + s += "\nhasProfileCut........: " + this.hasProfileCut.ToString(); + s += "\nhasHollow............: " + this.hasHollow.ToString(); + + return s; + } + + public bool HasProfileCut + { + get { return hasProfileCut; } + set { hasProfileCut = value; } + } + + public bool HasHollow + { + get { return hasHollow; } + } + + + /// + /// Constructs a PrimMesh object and creates the profile for extrusion. + /// + /// + /// + /// + /// + /// + /// + public PrimMesh(int sides, float profileStart, float profileEnd, float hollow, int hollowSides) + { + this.coords = new List(); + this.faces = new List(); + + this.sides = sides; + this.profileStart = profileStart; + this.profileEnd = profileEnd; + this.hollow = hollow; + this.hollowSides = hollowSides; + + if (sides < 3) + this.sides = 3; + if (hollowSides < 3) + this.hollowSides = 3; + if (profileStart < 0.0f) + this.profileStart = 0.0f; + if (profileEnd > 1.0f) + this.profileEnd = 1.0f; + if (profileEnd < 0.02f) + this.profileEnd = 0.02f; + if (profileStart >= profileEnd) + this.profileStart = profileEnd - 0.02f; + if (hollow > 0.99f) + this.hollow = 0.99f; + if (hollow < 0.0f) + this.hollow = 0.0f; + } + + /// + /// Extrudes a profile along a path. + /// + public void Extrude(PathType pathType) + { + bool needEndFaces = false; + + this.coords = new List(); + this.faces = new List(); + + int steps = 1; + + float length = this.pathCutEnd - this.pathCutBegin; + + this.hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; + + this.hasHollow = (this.hollow > 0.001f); + + float twistBegin = this.twistBegin / 360.0f * twoPi; + float twistEnd = this.twistEnd / 360.0f * twoPi; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float hollow = this.hollow; + + if (pathType == PathType.Circular) + { + needEndFaces = false; + if (this.pathCutBegin != 0.0f || this.pathCutEnd != 1.0f) + needEndFaces = true; + else if (this.taperX != 0.0f || this.taperY != 0.0f) + needEndFaces = true; + else if (this.skew != 0.0f) + needEndFaces = true; + else if (twistTotal != 0.0f) + needEndFaces = true; + else if (this.radius != 0.0f) + needEndFaces = true; + } + else needEndFaces = true; + + // sanity checks + float initialProfileRot = 0.0f; + if (pathType == PathType.Circular) + { + if (this.sides == 3) + { + initialProfileRot = (float)Math.PI; + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (this.sides == 4) + { + initialProfileRot = 0.25f * (float)Math.PI; + if (this.hollowSides != 4) + hollow *= 0.707f; + } + else if (this.sides > 4) + { + initialProfileRot = (float)Math.PI; + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow /= 0.7f; + } + } + } + else + { + if (this.sides == 3) + { + if (this.hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (this.sides == 4) + { + initialProfileRot = 1.25f * (float)Math.PI; + if (this.hollowSides != 4) + hollow *= 0.707f; + } + else if (this.sides == 24 && this.hollowSides == 4) + hollow *= 1.414f; + } + + Profile profile = new Profile(this.sides, this.profileStart, this.profileEnd, hollow, this.hollowSides, this.hasProfileCut,true); + this.errorMessage = profile.errorMessage; + + this.numPrimFaces = profile.numPrimFaces; + + if (initialProfileRot != 0.0f) + { + profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot)); + } + + float thisV = 0.0f; + float lastV = 0.0f; + + Path path = new Path(); + path.twistBegin = twistBegin; + path.twistEnd = twistEnd; + path.topShearX = topShearX; + path.topShearY = topShearY; + path.pathCutBegin = pathCutBegin; + path.pathCutEnd = pathCutEnd; + path.dimpleBegin = dimpleBegin; + path.dimpleEnd = dimpleEnd; + path.skew = skew; + path.holeSizeX = holeSizeX; + path.holeSizeY = holeSizeY; + path.taperX = taperX; + path.taperY = taperY; + path.radius = radius; + path.revolutions = revolutions; + path.stepsPerRevolution = stepsPerRevolution; + + path.Create(pathType, steps); + + int lastNode = path.pathNodes.Count -1; + + for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + { + PathNode node = path.pathNodes[nodeIndex]; + Profile newLayer = profile.Copy(); + + newLayer.Scale(node.xScale, node.yScale); + newLayer.AddRot(node.rotation); + newLayer.AddPos(node.position); + + if (needEndFaces && nodeIndex == 0) + { + newLayer.FlipNormals(); + } // if (nodeIndex == 0) + + // append this layer + + int coordsLen = this.coords.Count; + newLayer.AddValue2FaceVertexIndices(coordsLen); + + this.coords.AddRange(newLayer.coords); + + if (needEndFaces) + { + if (nodeIndex == 0) + this.faces.AddRange(newLayer.faces); + else if (nodeIndex == lastNode) + { + if (node.xScale > 1e-6 && node.yScale > 1e-6) + this.faces.AddRange(newLayer.faces); + } + } + + // fill faces between layers + + int numVerts = newLayer.coords.Count; + Face newFace1 = new Face(); + Face newFace2 = new Face(); + + thisV = 1.0f - node.percentOfPath; + + if (nodeIndex > 0) + { + int startVert = coordsLen; + int endVert = this.coords.Count; + if (!this.hasProfileCut) + { + int i = startVert; + for (int l = 0; l < profile.numOuterVerts - 1; l++) + { + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + this.faces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + this.faces.Add(newFace2); + i++; + } + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = startVert; + this.faces.Add(newFace1); + + newFace2.v1 = startVert; + newFace2.v2 = i - numVerts; + newFace2.v3 = startVert - numVerts; + this.faces.Add(newFace2); + + if (this.hasHollow) + { + startVert = ++i; + for (int l = 0; l < profile.numHollowVerts - 1; l++) + { + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + this.faces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + this.faces.Add(newFace2); + i++; + } + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = startVert; + this.faces.Add(newFace1); + + newFace2.v1 = startVert; + newFace2.v2 = i - numVerts; + newFace2.v3 = startVert - numVerts; + this.faces.Add(newFace2); + } + + + } + else + { + for (int i = startVert; i < endVert; i++) + { + int iNext = i + 1; + if (i == endVert - 1) + iNext = startVert; + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = iNext; + this.faces.Add(newFace1); + + newFace2.v1 = iNext; + newFace2.v2 = i - numVerts; + newFace2.v3 = iNext - numVerts; + this.faces.Add(newFace2); + + } + } + } + + lastV = thisV; + + } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + + } + + + /// + /// DEPRICATED - use Extrude(PathType.Linear) instead + /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism. + /// + /// + public void ExtrudeLinear() + { + this.Extrude(PathType.Linear); + } + + + /// + /// DEPRICATED - use Extrude(PathType.Circular) instead + /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring. + /// + /// + public void ExtrudeCircular() + { + this.Extrude(PathType.Circular); + } + + + private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3) + { + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + Coord normal = Coord.Cross(edge1, edge2); + + normal.Normalize(); + + return normal; + } + + private Coord SurfaceNormal(Face face) + { + return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]); + } + + /// + /// Calculate the surface normal for a face in the list of faces + /// + /// + /// + public Coord SurfaceNormal(int faceIndex) + { + int numFaces = this.faces.Count; + if (faceIndex < 0 || faceIndex >= numFaces) + throw new Exception("faceIndex out of range"); + + return SurfaceNormal(this.faces[faceIndex]); + } + + /// + /// Duplicates a PrimMesh object. All object properties are copied by value, including lists. + /// + /// + public PrimMesh Copy() + { + PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides); + copy.twistBegin = this.twistBegin; + copy.twistEnd = this.twistEnd; + copy.topShearX = this.topShearX; + copy.topShearY = this.topShearY; + copy.pathCutBegin = this.pathCutBegin; + copy.pathCutEnd = this.pathCutEnd; + copy.dimpleBegin = this.dimpleBegin; + copy.dimpleEnd = this.dimpleEnd; + copy.skew = this.skew; + copy.holeSizeX = this.holeSizeX; + copy.holeSizeY = this.holeSizeY; + copy.taperX = this.taperX; + copy.taperY = this.taperY; + copy.radius = this.radius; + copy.revolutions = this.revolutions; + copy.stepsPerRevolution = this.stepsPerRevolution; + + copy.numPrimFaces = this.numPrimFaces; + copy.errorMessage = this.errorMessage; + + copy.coords = new List(this.coords); + copy.faces = new List(this.faces); + + return copy; + } + + /// + /// Adds a value to each XYZ vertex coordinate in the mesh + /// + /// + /// + /// + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + } + + /// + /// Rotates the mesh + /// + /// + public void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + } + +#if VERTEX_INDEXER + public VertexIndexer GetVertexIndexer() + { + return null; + } +#endif + + /// + /// Scales the mesh + /// + /// + /// + /// + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + //Coord vert; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + } + + /// + /// Dumps the mesh to a Blender compatible "Raw" format file + /// + /// + /// + /// + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..6881e26 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using Mono.Addins; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.UbitMeshing")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("http://opensimulator.org")] +[assembly: AssemblyProduct("OpenSim")] +[assembly: AssemblyCopyright("OpenSimulator developers")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("4b7e35c2-a9dd-4b10-b778-eb417f4f6884")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +[assembly: AssemblyVersion("0.8.2.*")] + +[assembly: Addin("OpenSim.Region.PhysicsModule.UbitMeshing", OpenSim.VersionInfo.VersionNumber)] +[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs new file mode 100644 index 0000000..1c75db6 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs @@ -0,0 +1,244 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.Text; + +using System.Drawing; +using System.Drawing.Imaging; + +namespace PrimMesher +{ + public class SculptMap + { + public int width; + public int height; + public byte[] redBytes; + public byte[] greenBytes; + public byte[] blueBytes; + + public SculptMap() + { + } + + public SculptMap(Bitmap bm, int lod) + { + int bmW = bm.Width; + int bmH = bm.Height; + + if (bmW == 0 || bmH == 0) + throw new Exception("SculptMap: bitmap has no data"); + + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + + bool needsScaling = false; + bool smallMap = false; + + width = bmW; + height = bmH; + + while (width * height > numLodPixels * 4) + { + width >>= 1; + height >>= 1; + needsScaling = true; + } + + try + { + if (needsScaling) + bm = ScaleImage(bm, width, height); + } + + catch (Exception e) + { + throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); + } + + if (width * height > numLodPixels) + { + smallMap = false; + width >>= 1; + height >>= 1; + } + else + smallMap = true; + + int numBytes = (width + 1) * (height + 1); + redBytes = new byte[numBytes]; + greenBytes = new byte[numBytes]; + blueBytes = new byte[numBytes]; + + int byteNdx = 0; + Color c; + + try + { + for (int y = 0; y <= height; y++) + { + for (int x = 0; x < width; x++) + { + if (smallMap) + c = bm.GetPixel(x, y < height ? y : y - 1); + else + c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; + } + + if (smallMap) + c = bm.GetPixel(width - 1, y < height ? y : y - 1); + else + c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; + } + } + catch (Exception e) + { + throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString()); + } + + width++; + height++; + } + + public List> ToRows(bool mirror) + { + int numRows = height; + int numCols = width; + + List> rows = new List>(numRows); + + float pixScale = 1.0f / 255; + + int rowNdx, colNdx; + int smNdx = 0; + + for (rowNdx = 0; rowNdx < numRows; rowNdx++) + { + List row = new List(numCols); + for (colNdx = 0; colNdx < numCols; colNdx++) + { + + if (mirror) + row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); + else + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); + + ++smNdx; + } + rows.Add(row); + } + return rows; + } + + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) + { + + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + + + // will let last step to be eventually diferent, as seems to be in sl + + float xscale = (float)srcImage.Width / (float)destWidth; + float yscale = (float)srcImage.Height / (float)destHeight; + + int lastsx = srcImage.Width - 1; + int lastsy = srcImage.Height - 1; + int lastdx = destWidth - 1; + int lastdy = destHeight - 1; + + float sy = 0.5f; + float sx; + + for (int y = 0; y < lastdy; y++) + { + sx = 0.5f; + for (int x = 0; x < lastdx; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + sx += xscale; + } + try + { + c = srcImage.GetPixel(lastsx, (int)(sy)); + scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sy += yscale; + } + + sx = 0.5f; + for (int x = 0; x < lastdx; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), lastsy); + scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sx += xscale; + } + try + { + c = srcImage.GetPixel(lastsx, lastsy); + scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + srcImage.Dispose(); + return scaledImage; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs new file mode 100644 index 0000000..bc1375b --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs @@ -0,0 +1,220 @@ +/* + * Copyright (c) Contributors + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * 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. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project 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 DEVELOPERS ``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 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.Collections.Generic; +using System.Text; +using System.IO; + +using System.Drawing; +using System.Drawing.Imaging; + +namespace PrimMesher +{ + + public class SculptMesh + { + public List coords; + public List faces; + + public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 }; + + + public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert) + { + if (mirror) + invert = !invert; + + SculptMap smap = new SculptMap(sculptBitmap, lod); + + List> rows = smap.ToRows(mirror); + + _SculptMesh(rows, sculptType, invert); + } + + private void _SculptMesh(List> rows, SculptType sculptType, bool invert) + { + coords = new List(); + faces = new List(); + + sculptType = (SculptType)(((int)sculptType) & 0x07); + + int width = rows[0].Count; + + int p1, p2, p3, p4; + + int imageX, imageY; + + if (sculptType != SculptType.plane) + { + if (rows.Count % 2 == 0) + { + for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++) + rows[rowNdx].Add(rows[rowNdx][0]); + } + else + { + int lastIndex = rows[0].Count - 1; + + for (int i = 0; i < rows.Count; i++) + rows[i][0] = rows[i][lastIndex]; + } + } + + Coord topPole = rows[0][width / 2]; + Coord bottomPole = rows[rows.Count - 1][width / 2]; + + if (sculptType == SculptType.sphere) + { + if (rows.Count % 2 == 0) + { + int count = rows[0].Count; + List topPoleRow = new List(count); + List bottomPoleRow = new List(count); + + for (int i = 0; i < count; i++) + { + topPoleRow.Add(topPole); + bottomPoleRow.Add(bottomPole); + } + rows.Insert(0, topPoleRow); + rows.Add(bottomPoleRow); + } + else + { + int count = rows[0].Count; + + List topPoleRow = rows[0]; + List bottomPoleRow = rows[rows.Count - 1]; + + for (int i = 0; i < count; i++) + { + topPoleRow[i] = topPole; + bottomPoleRow[i] = bottomPole; + } + } + } + + if (sculptType == SculptType.torus) + rows.Add(rows[0]); + + int coordsDown = rows.Count; + int coordsAcross = rows[0].Count; + + float widthUnit = 1.0f / (coordsAcross - 1); + float heightUnit = 1.0f / (coordsDown - 1); + + for (imageY = 0; imageY < coordsDown; imageY++) + { + int rowOffset = imageY * coordsAcross; + + for (imageX = 0; imageX < coordsAcross; imageX++) + { + /* + * p1-----p2 + * | \ f2 | + * | \ | + * | f1 \| + * p3-----p4 + */ + + p4 = rowOffset + imageX; + p3 = p4 - 1; + + p2 = p4 - coordsAcross; + p1 = p3 - coordsAcross; + + this.coords.Add(rows[imageY][imageX]); + + if (imageY > 0 && imageX > 0) + { + Face f1, f2; + + if (invert) + { + f1 = new Face(p1, p4, p3); + f2 = new Face(p1, p2, p4); + } + else + { + f1 = new Face(p1, p3, p4); + f2 = new Face(p1, p4, p2); + } + + this.faces.Add(f1); + this.faces.Add(f2); + } + } + } + } + + /// + /// Duplicates a SculptMesh object. All object properties are copied by value, including lists. + /// + /// + public SculptMesh Copy() + { + return new SculptMesh(this); + } + + public SculptMesh(SculptMesh sm) + { + coords = new List(sm.coords); + faces = new List(sm.faces); + } + + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} -- cgit v1.1