aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics')
-rw-r--r--OpenSim/Region/Physics/Manager/IMesher.cs1
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsActor.cs19
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsScene.cs10
-rw-r--r--OpenSim/Region/Physics/Meshing/Mesh.cs72
-rw-r--r--OpenSim/Region/Physics/Meshing/Meshmerizer.cs21
-rw-r--r--OpenSim/Region/Physics/Meshing/SculptMap.cs62
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs58
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs1451
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs849
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs3317
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs443
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs1961
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs86
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2540
14 files changed, 10860 insertions, 30 deletions
diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs
index 3a9ca1b..cc92484 100644
--- a/OpenSim/Region/Physics/Manager/IMesher.cs
+++ b/OpenSim/Region/Physics/Manager/IMesher.cs
@@ -65,5 +65,6 @@ namespace OpenSim.Region.Physics.Manager
65 void releasePinned(); 65 void releasePinned();
66 void Append(IMesh newMesh); 66 void Append(IMesh newMesh);
67 void TransformLinear(float[,] matrix, float[] offset); 67 void TransformLinear(float[,] matrix, float[] offset);
68 Vector3 GetCentroid();
68 } 69 }
69} 70}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 0587054..e1a68be 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -68,6 +68,17 @@ namespace OpenSim.Region.Physics.Manager
68 } 68 }
69 } 69 }
70 70
71 public struct ContactData
72 {
73 public float mu;
74 public float bounce;
75
76 public ContactData(float _mu, float _bounce)
77 {
78 mu = _mu;
79 bounce = _bounce;
80 }
81 }
71 /// <summary> 82 /// <summary>
72 /// Used to pass collision information to OnCollisionUpdate listeners. 83 /// Used to pass collision information to OnCollisionUpdate listeners.
73 /// </summary> 84 /// </summary>
@@ -143,6 +154,14 @@ namespace OpenSim.Region.Physics.Manager
143 get { return new NullPhysicsActor(); } 154 get { return new NullPhysicsActor(); }
144 } 155 }
145 156
157
158 public virtual bool Building { get; set; }
159
160 public virtual ContactData ContactData
161 {
162 get { return new ContactData(0, 0); }
163 }
164
146 public abstract bool Stopped { get; } 165 public abstract bool Stopped { get; }
147 166
148 public abstract Vector3 Size { get; set; } 167 public abstract Vector3 Size { get; set; }
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 514d9ad..3db71e5 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -225,7 +225,7 @@ namespace OpenSim.Region.Physics.Manager
225 } 225 }
226 226
227 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} 227 public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {}
228 228 public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {}
229 public virtual void UnCombine(PhysicsScene pScene) {} 229 public virtual void UnCombine(PhysicsScene pScene) {}
230 230
231 /// <summary> 231 /// <summary>
@@ -263,5 +263,13 @@ namespace OpenSim.Region.Physics.Manager
263 { 263 {
264 return new List<ContactResult>(); 264 return new List<ContactResult>();
265 } 265 }
266
267 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){}
268 public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { }
269 public virtual List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
270 {
271 return new List<ContactResult>();
272 }
273
266 } 274 }
267} 275}
diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs
index f781ff9..c715642 100644
--- a/OpenSim/Region/Physics/Meshing/Mesh.cs
+++ b/OpenSim/Region/Physics/Meshing/Mesh.cs
@@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing
46 IntPtr m_indicesPtr = IntPtr.Zero; 46 IntPtr m_indicesPtr = IntPtr.Zero;
47 int m_indexCount = 0; 47 int m_indexCount = 0;
48 public float[] m_normals; 48 public float[] m_normals;
49 Vector3 _centroid;
50 int _centroidDiv;
51
52 private class vertexcomp : IEqualityComparer<Vertex>
53 {
54 public bool Equals(Vertex v1, Vertex v2)
55 {
56 if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z)
57 return true;
58 else
59 return false;
60 }
61 public int GetHashCode(Vertex v)
62 {
63 int a = v.X.GetHashCode();
64 int b = v.Y.GetHashCode();
65 int c = v.Z.GetHashCode();
66 return (a << 16) ^ (b << 8) ^ c;
67 }
68
69 }
49 70
50 public Mesh() 71 public Mesh()
51 { 72 {
52 m_vertices = new Dictionary<Vertex, int>(); 73 vertexcomp vcomp = new vertexcomp();
74
75 m_vertices = new Dictionary<Vertex, int>(vcomp);
53 m_triangles = new List<Triangle>(); 76 m_triangles = new List<Triangle>();
77 _centroid = Vector3.Zero;
78 _centroidDiv = 0;
54 } 79 }
55 80
56 public Mesh Clone() 81 public Mesh Clone()
@@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing
61 { 86 {
62 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); 87 result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone()));
63 } 88 }
64 89 result._centroid = _centroid;
90 result._centroidDiv = _centroidDiv;
65 return result; 91 return result;
66 } 92 }
67 93
@@ -71,15 +97,57 @@ namespace OpenSim.Region.Physics.Meshing
71 throw new NotSupportedException("Attempt to Add to a pinned Mesh"); 97 throw new NotSupportedException("Attempt to Add to a pinned Mesh");
72 // If a vertex of the triangle is not yet in the vertices list, 98 // If a vertex of the triangle is not yet in the vertices list,
73 // add it and set its index to the current index count 99 // add it and set its index to the current index count
100 // vertex == seems broken
101 // skip colapsed triangles
102 if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z)
103 || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z)
104 || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z)
105 )
106 {
107 return;
108 }
109
110 if (m_vertices.Count == 0)
111 {
112 _centroidDiv = 0;
113 _centroid = Vector3.Zero;
114 }
115
74 if (!m_vertices.ContainsKey(triangle.v1)) 116 if (!m_vertices.ContainsKey(triangle.v1))
117 {
75 m_vertices[triangle.v1] = m_vertices.Count; 118 m_vertices[triangle.v1] = m_vertices.Count;
119 _centroid.X += triangle.v1.X;
120 _centroid.Y += triangle.v1.Y;
121 _centroid.Z += triangle.v1.Z;
122 _centroidDiv++;
123 }
76 if (!m_vertices.ContainsKey(triangle.v2)) 124 if (!m_vertices.ContainsKey(triangle.v2))
125 {
77 m_vertices[triangle.v2] = m_vertices.Count; 126 m_vertices[triangle.v2] = m_vertices.Count;
127 _centroid.X += triangle.v2.X;
128 _centroid.Y += triangle.v2.Y;
129 _centroid.Z += triangle.v2.Z;
130 _centroidDiv++;
131 }
78 if (!m_vertices.ContainsKey(triangle.v3)) 132 if (!m_vertices.ContainsKey(triangle.v3))
133 {
79 m_vertices[triangle.v3] = m_vertices.Count; 134 m_vertices[triangle.v3] = m_vertices.Count;
135 _centroid.X += triangle.v3.X;
136 _centroid.Y += triangle.v3.Y;
137 _centroid.Z += triangle.v3.Z;
138 _centroidDiv++;
139 }
80 m_triangles.Add(triangle); 140 m_triangles.Add(triangle);
81 } 141 }
82 142
143 public Vector3 GetCentroid()
144 {
145 if (_centroidDiv > 0)
146 return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv);
147 else
148 return Vector3.Zero;
149 }
150
83 public void CalcNormals() 151 public void CalcNormals()
84 { 152 {
85 int iTriangles = m_triangles.Count; 153 int iTriangles = m_triangles.Count;
diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
index f15e81b..ba461f7 100644
--- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
+++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs
@@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing
74#endif 74#endif
75 75
76 private bool cacheSculptMaps = true; 76 private bool cacheSculptMaps = true;
77 private bool cacheSculptAlphaMaps = true;
78
77 private string decodedSculptMapPath = null; 79 private string decodedSculptMapPath = null;
78 private bool useMeshiesPhysicsMesh = false; 80 private bool useMeshiesPhysicsMesh = false;
79 81
@@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing
87 IConfig mesh_config = config.Configs["Mesh"]; 89 IConfig mesh_config = config.Configs["Mesh"];
88 90
89 decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); 91 decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache");
92
90 cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); 93 cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps);
94
95 if (Environment.OSVersion.Platform == PlatformID.Unix)
96 {
97 cacheSculptAlphaMaps = false;
98 }
99 else
100 cacheSculptAlphaMaps = cacheSculptMaps;
101
91 if(mesh_config != null) 102 if(mesh_config != null)
92 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); 103 useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh);
93 104
@@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing
268 { 279 {
269 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) 280 if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces))
270 return null; 281 return null;
282 // Remove the reference to any JPEG2000 sculpt data so it can be GCed
283 // don't loose it
284 // primShape.SculptData = Utils.EmptyBytes;
271 } 285 }
286// primShape.SculptDataLoaded = true;
272 } 287 }
273 else 288 else
274 { 289 {
275 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) 290 if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces))
276 return null; 291 return null;
277 } 292 }
278 293 // keep compatible
279 // Remove the reference to any JPEG2000 sculpt data so it can be GCed
280 primShape.SculptData = Utils.EmptyBytes; 294 primShape.SculptData = Utils.EmptyBytes;
281 295
282 int numCoords = coords.Count; 296 int numCoords = coords.Count;
@@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing
482 496
483 //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); 497 //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData);
484 498
485 if (cacheSculptMaps) 499 if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0)))
500 // don't cache images with alpha channel in linux since mono can't load them correctly)
486 { 501 {
487 try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } 502 try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); }
488 catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } 503 catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); }
diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs
index 740424e..b3d9cb6 100644
--- a/OpenSim/Region/Physics/Meshing/SculptMap.cs
+++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs
@@ -58,28 +58,24 @@ namespace PrimMesher
58 if (bmW == 0 || bmH == 0) 58 if (bmW == 0 || bmH == 0)
59 throw new Exception("SculptMap: bitmap has no data"); 59 throw new Exception("SculptMap: bitmap has no data");
60 60
61 int numLodPixels = lod * 2 * lod * 2; // (32 * 2)^2 = 64^2 pixels for default sculpt map image 61 int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image
62 62
63 bool smallMap = bmW * bmH <= numLodPixels;
63 bool needsScaling = false; 64 bool needsScaling = false;
64 65
65 bool smallMap = bmW * bmH <= lod * lod;
66
67 width = bmW; 66 width = bmW;
68 height = bmH; 67 height = bmH;
69 while (width * height > numLodPixels) 68 while (width * height > numLodPixels * 4)
70 { 69 {
71 width >>= 1; 70 width >>= 1;
72 height >>= 1; 71 height >>= 1;
73 needsScaling = true; 72 needsScaling = true;
74 } 73 }
75 74
76
77
78 try 75 try
79 { 76 {
80 if (needsScaling) 77 if (needsScaling)
81 bm = ScaleImage(bm, width, height, 78 bm = ScaleImage(bm, width, height);
82 System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor);
83 } 79 }
84 80
85 catch (Exception e) 81 catch (Exception e)
@@ -87,7 +83,7 @@ namespace PrimMesher
87 throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); 83 throw new Exception("Exception in ScaleImage(): e: " + e.ToString());
88 } 84 }
89 85
90 if (width * height > lod * lod) 86 if (width * height > numLodPixels)
91 { 87 {
92 width >>= 1; 88 width >>= 1;
93 height >>= 1; 89 height >>= 1;
@@ -144,15 +140,17 @@ namespace PrimMesher
144 int rowNdx, colNdx; 140 int rowNdx, colNdx;
145 int smNdx = 0; 141 int smNdx = 0;
146 142
143
147 for (rowNdx = 0; rowNdx < numRows; rowNdx++) 144 for (rowNdx = 0; rowNdx < numRows; rowNdx++)
148 { 145 {
149 List<Coord> row = new List<Coord>(numCols); 146 List<Coord> row = new List<Coord>(numCols);
150 for (colNdx = 0; colNdx < numCols; colNdx++) 147 for (colNdx = 0; colNdx < numCols; colNdx++)
151 { 148 {
149
152 if (mirror) 150 if (mirror)
153 row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f)); 151 row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f));
154 else 152 else
155 row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f)); 153 row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f));
156 154
157 ++smNdx; 155 ++smNdx;
158 } 156 }
@@ -161,23 +159,39 @@ namespace PrimMesher
161 return rows; 159 return rows;
162 } 160 }
163 161
164 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight, 162 private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight)
165 System.Drawing.Drawing2D.InterpolationMode interpMode)
166 { 163 {
167 Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight);
168 scaledImage.SetResolution(96.0f, 96.0f);
169
170 Graphics grPhoto = Graphics.FromImage(scaledImage);
171 grPhoto.InterpolationMode = interpMode;
172 164
173 grPhoto.DrawImage(srcImage, 165 Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb);
174 new Rectangle(0, 0, destWidth, destHeight), 166
175 new Rectangle(0, 0, srcImage.Width, srcImage.Height), 167 Color c;
176 GraphicsUnit.Pixel); 168 float xscale = srcImage.Width / destWidth;
169 float yscale = srcImage.Height / destHeight;
170
171 float sy = 0.5f;
172 for (int y = 0; y < destHeight; y++)
173 {
174 float sx = 0.5f;
175 for (int x = 0; x < destWidth; x++)
176 {
177 try
178 {
179 c = srcImage.GetPixel((int)(sx), (int)(sy));
180 scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B));
181 }
182 catch (IndexOutOfRangeException)
183 {
184 }
177 185
178 grPhoto.Dispose(); 186 sx += xscale;
187 }
188 sy += yscale;
189 }
190 srcImage.Dispose();
179 return scaledImage; 191 return scaledImage;
180 } 192 }
193
194 }
195
181 } 196 }
182}
183#endif 197#endif
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
new file mode 100644
index 0000000..d46341b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs
@@ -0,0 +1,58 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System.Reflection;
29using System.Runtime.InteropServices;
30
31// Information about this assembly is defined by the following
32// attributes.
33//
34// change them to the information which is associated with the assembly
35// you compile.
36
37[assembly : AssemblyTitle("OdePlugin")]
38[assembly : AssemblyDescription("Ubit Variation")]
39[assembly : AssemblyConfiguration("")]
40[assembly : AssemblyCompany("http://opensimulator.org")]
41[assembly : AssemblyProduct("OdePlugin")]
42[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")]
43[assembly : AssemblyTrademark("")]
44[assembly : AssemblyCulture("")]
45
46// This sets the default COM visibility of types in the assembly to invisible.
47// If you need to expose a type to COM, use [ComVisible(true)] on that type.
48
49[assembly : ComVisible(false)]
50
51// The assembly version has following format :
52//
53// Major.Minor.Build.Revision
54//
55// You can specify all values by your own or you can build default build and revision
56// numbers with the '*' character (the default):
57
58[assembly : AssemblyVersion("0.6.5.*")]
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..793e281
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -0,0 +1,1451 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28
29// Revision by Ubit 2011/12
30
31using System;
32using System.Collections.Generic;
33using System.Reflection;
34using OpenMetaverse;
35using OdeAPI;
36using OpenSim.Framework;
37using OpenSim.Region.Physics.Manager;
38using log4net;
39
40namespace OpenSim.Region.Physics.OdePlugin
41{
42 /// <summary>
43 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
44 /// </summary>
45
46 public enum dParam : int
47 {
48 LowStop = 0,
49 HiStop = 1,
50 Vel = 2,
51 FMax = 3,
52 FudgeFactor = 4,
53 Bounce = 5,
54 CFM = 6,
55 StopERP = 7,
56 StopCFM = 8,
57 LoStop2 = 256,
58 HiStop2 = 257,
59 Vel2 = 258,
60 FMax2 = 259,
61 StopERP2 = 7 + 256,
62 StopCFM2 = 8 + 256,
63 LoStop3 = 512,
64 HiStop3 = 513,
65 Vel3 = 514,
66 FMax3 = 515,
67 StopERP3 = 7 + 512,
68 StopCFM3 = 8 + 512
69 }
70
71 public class OdeCharacter : PhysicsActor
72 {
73 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
74
75 private Vector3 _position;
76 private Vector3 _zeroPosition;
77 private bool _zeroFlag = false;
78 private Vector3 _velocity;
79 private Vector3 _target_velocity;
80 private Vector3 _acceleration;
81 private Vector3 m_rotationalVelocity;
82 private float m_mass = 80f;
83 public float m_density = 60f;
84 private bool m_pidControllerActive = true;
85 public float PID_D = 800.0f;
86 public float PID_P = 900.0f;
87 //private static float POSTURE_SERVO = 10000.0f;
88 public float CAPSULE_RADIUS = 0.37f;
89 public float CAPSULE_LENGTH = 2.140599f;
90 public float walkDivisor = 1.3f;
91 public float runDivisor = 0.8f;
92 private bool flying = false;
93 private bool m_iscolliding = false;
94 private bool m_iscollidingGround = false;
95 private bool m_iscollidingObj = false;
96 private bool m_alwaysRun = false;
97 private int m_requestedUpdateFrequency = 0;
98 public uint m_localID = 0;
99 public bool m_returnCollisions = false;
100 // taints and their non-tainted counterparts
101 public bool m_isPhysical = false; // the current physical status
102 public float MinimumGroundFlightOffset = 3f;
103
104 private float m_buoyancy = 0f;
105
106 // private CollisionLocker ode;
107
108 private string m_name = String.Empty;
109 // other filter control
110 int m_colliderfilter = 0;
111 // int m_colliderGroundfilter = 0;
112 int m_colliderObjectfilter = 0;
113
114 // Default we're a Character
115 private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
116
117 // Default, Collide with Other Geometries, spaces, bodies and characters.
118 private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
119 | CollisionCategories.Space
120 | CollisionCategories.Body
121 | CollisionCategories.Character
122 );
123 // we do land collisions not ode | CollisionCategories.Land);
124 public IntPtr Body = IntPtr.Zero;
125 private OdeScene _parent_scene;
126 public IntPtr Shell = IntPtr.Zero;
127 public IntPtr Amotor = IntPtr.Zero;
128 public d.Mass ShellMass;
129// public bool collidelock = false;
130
131 private bool m_haseventsubscription = false;
132 public int m_eventsubscription = 0;
133 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
134
135 // unique UUID of this character object
136 public UUID m_uuid;
137 public bool bad = false;
138
139 public ContactData AvatarContactData = new ContactData(10f, 0.3f);
140
141 public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor)
142 {
143 m_uuid = UUID.Random();
144
145 if (pos.IsFinite())
146 {
147 if (pos.Z > 99999f)
148 {
149 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
150 }
151 if (pos.Z < -100f) // shouldn't this be 0 ?
152 {
153 pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
154 }
155 _position = pos;
156 }
157 else
158 {
159 _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
160 m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
161 }
162
163 _parent_scene = parent_scene;
164
165 PID_D = pid_d;
166 PID_P = pid_p;
167 CAPSULE_RADIUS = capsule_radius;
168 m_density = density;
169 m_mass = 80f; // sure we have a default
170
171 AvatarContactData.mu = parent_scene.AvatarFriction;
172 AvatarContactData.bounce = parent_scene.AvatarBounce;
173
174 walkDivisor = walk_divisor;
175 runDivisor = rundivisor;
176
177 CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f;
178 //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
179
180 m_isPhysical = false; // current status: no ODE information exists
181
182 m_name = avName;
183
184 AddChange(changes.Add, null);
185 }
186
187 public override int PhysicsActorType
188 {
189 get { return (int)ActorTypes.Agent; }
190 set { return; }
191 }
192
193 public override ContactData ContactData
194 {
195 get { return AvatarContactData; }
196 }
197
198 public override bool Building { get; set; }
199
200 /// <summary>
201 /// If this is set, the avatar will move faster
202 /// </summary>
203 public override bool SetAlwaysRun
204 {
205 get { return m_alwaysRun; }
206 set { m_alwaysRun = value; }
207 }
208
209 public override uint LocalID
210 {
211 set { m_localID = value; }
212 }
213
214 public override bool Grabbed
215 {
216 set { return; }
217 }
218
219 public override bool Selected
220 {
221 set { return; }
222 }
223
224 public override float Buoyancy
225 {
226 get { return m_buoyancy; }
227 set { m_buoyancy = value; }
228 }
229
230 public override bool FloatOnWater
231 {
232 set { return; }
233 }
234
235 public override bool IsPhysical
236 {
237 get { return false; }
238 set { return; }
239 }
240
241 public override bool ThrottleUpdates
242 {
243 get { return false; }
244 set { return; }
245 }
246
247 public override bool Flying
248 {
249 get { return flying; }
250 set
251 {
252 flying = value;
253 // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
254 }
255 }
256
257 /// <summary>
258 /// Returns if the avatar is colliding in general.
259 /// This includes the ground and objects and avatar.
260 /// </summary>
261 public override bool IsColliding
262 {
263 get { return (m_iscolliding || m_iscollidingGround); }
264 set
265 {
266 if (value)
267 {
268 m_colliderfilter += 2;
269 if (m_colliderfilter > 2)
270 m_colliderfilter = 2;
271 }
272 else
273 {
274 m_colliderfilter--;
275 if (m_colliderfilter < 0)
276 m_colliderfilter = 0;
277 }
278
279 if (m_colliderfilter == 0)
280 m_iscolliding = false;
281 else
282 {
283// SetPidStatus(false);
284 m_pidControllerActive = true;
285 m_iscolliding = true;
286 }
287 }
288 }
289
290 /// <summary>
291 /// Returns if an avatar is colliding with the ground
292 /// </summary>
293 public override bool CollidingGround
294 {
295 get { return m_iscollidingGround; }
296 set
297 {
298 /* we now control this
299 if (value)
300 {
301 m_colliderGroundfilter += 2;
302 if (m_colliderGroundfilter > 2)
303 m_colliderGroundfilter = 2;
304 }
305 else
306 {
307 m_colliderGroundfilter--;
308 if (m_colliderGroundfilter < 0)
309 m_colliderGroundfilter = 0;
310 }
311
312 if (m_colliderGroundfilter == 0)
313 m_iscollidingGround = false;
314 else
315 m_iscollidingGround = true;
316 */
317 }
318
319 }
320
321 /// <summary>
322 /// Returns if the avatar is colliding with an object
323 /// </summary>
324 public override bool CollidingObj
325 {
326 get { return m_iscollidingObj; }
327 set
328 {
329 // Ubit filter this also
330 if (value)
331 {
332 m_colliderObjectfilter += 2;
333 if (m_colliderObjectfilter > 2)
334 m_colliderObjectfilter = 2;
335 }
336 else
337 {
338 m_colliderObjectfilter--;
339 if (m_colliderObjectfilter < 0)
340 m_colliderObjectfilter = 0;
341 }
342
343 if (m_colliderObjectfilter == 0)
344 m_iscollidingObj = false;
345 else
346 m_iscollidingObj = true;
347
348 // m_iscollidingObj = value;
349/*
350 if (m_iscollidingObj)
351 m_pidControllerActive = false;
352 else
353 m_pidControllerActive = true;
354 */
355 }
356 }
357
358 /// <summary>
359 /// turn the PID controller on or off.
360 /// The PID Controller will turn on all by itself in many situations
361 /// </summary>
362 /// <param name="status"></param>
363 public void SetPidStatus(bool status)
364 {
365 m_pidControllerActive = status;
366 }
367
368 public override bool Stopped
369 {
370 get { return _zeroFlag; }
371 }
372
373 /// <summary>
374 /// This 'puts' an avatar somewhere in the physics space.
375 /// Not really a good choice unless you 'know' it's a good
376 /// spot otherwise you're likely to orbit the avatar.
377 /// </summary>
378 public override Vector3 Position
379 {
380 get { return _position; }
381 set
382 {
383 if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
384 {
385 if (value.IsFinite())
386 {
387 if (value.Z > 9999999f)
388 {
389 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
390 }
391 if (value.Z < -100f)
392 {
393 value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
394 }
395 AddChange(changes.Position, value);
396 }
397 else
398 {
399 m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
400 }
401 }
402 }
403 }
404
405 public override Vector3 RotationalVelocity
406 {
407 get { return m_rotationalVelocity; }
408 set { m_rotationalVelocity = value; }
409 }
410
411 /// <summary>
412 /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
413 /// and use it to offset landings properly
414 /// </summary>
415 public override Vector3 Size
416 {
417 get {
418 float d = CAPSULE_RADIUS * 2;
419 return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); }
420 set
421 {
422 if (value.IsFinite())
423 {
424 AddChange(changes.Size, value);
425 }
426 else
427 {
428 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
429 }
430 }
431 }
432
433 /// <summary>
434 /// This creates the Avatar's physical Surrogate at the position supplied
435 /// </summary>
436 /// <param name="npositionX"></param>
437 /// <param name="npositionY"></param>
438 /// <param name="npositionZ"></param>
439
440 //
441 /// <summary>
442 /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
443 /// This may be used in calculations in the scene/scenepresence
444 /// </summary>
445 public override float Mass
446 {
447 get
448 {
449 float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH));
450 return m_density * AVvolume;
451 }
452 }
453 public override void link(PhysicsActor obj)
454 {
455
456 }
457
458 public override void delink()
459 {
460
461 }
462
463 public override void LockAngularMotion(Vector3 axis)
464 {
465
466 }
467
468
469 public override Vector3 Force
470 {
471 get { return _target_velocity; }
472 set { return; }
473 }
474
475 public override int VehicleType
476 {
477 get { return 0; }
478 set { return; }
479 }
480
481 public override void VehicleFloatParam(int param, float value)
482 {
483
484 }
485
486 public override void VehicleVectorParam(int param, Vector3 value)
487 {
488
489 }
490
491 public override void VehicleRotationParam(int param, Quaternion rotation)
492 {
493
494 }
495
496 public override void VehicleFlags(int param, bool remove)
497 {
498
499 }
500
501 public override void SetVolumeDetect(int param)
502 {
503
504 }
505
506 public override Vector3 CenterOfMass
507 {
508 get
509 {
510 Vector3 pos = _position;
511 return pos;
512 }
513 }
514
515 public override Vector3 GeometricCenter
516 {
517 get
518 {
519 Vector3 pos = _position;
520 return pos;
521 }
522 }
523
524 //UBit mess
525 /* for later use
526 public override Vector3 PrimOOBsize
527 {
528 get
529 {
530 Vector3 s=Size;
531 s.X *=0.5f;
532 s.Y *=0.5f;
533 s.Z *=0.5f;
534 return s;
535 }
536 }
537
538 public override Vector3 PrimOOBoffset
539 {
540 get
541 {
542 return Vector3.Zero;
543 }
544 }
545 */
546
547 public override PrimitiveBaseShape Shape
548 {
549 set { return; }
550 }
551
552 public override Vector3 Velocity
553 {
554 get
555 {
556 return _velocity;
557 }
558 set
559 {
560 if (value.IsFinite())
561 {
562 AddChange(changes.Velocity, value);
563 }
564 else
565 {
566 m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
567 }
568 }
569 }
570
571 public override Vector3 Torque
572 {
573 get { return Vector3.Zero; }
574 set { return; }
575 }
576
577 public override float CollisionScore
578 {
579 get { return 0f; }
580 set { }
581 }
582
583 public override bool Kinematic
584 {
585 get { return false; }
586 set { }
587 }
588
589 public override Quaternion Orientation
590 {
591 get { return Quaternion.Identity; }
592 set
593 {
594 }
595 }
596
597 public override Vector3 Acceleration
598 {
599 get { return _acceleration; }
600 set { }
601 }
602
603 public void SetAcceleration(Vector3 accel)
604 {
605 m_pidControllerActive = true;
606 _acceleration = accel;
607 }
608
609 /// <summary>
610 /// Adds the force supplied to the Target Velocity
611 /// The PID controller takes this target velocity and tries to make it a reality
612 /// </summary>
613 /// <param name="force"></param>
614 public override void AddForce(Vector3 force, bool pushforce)
615 {
616 if (force.IsFinite())
617 {
618 if (pushforce)
619 {
620 AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f);
621 }
622 else
623 {
624 AddChange(changes.Velocity, force);
625 }
626 }
627 else
628 {
629 m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
630 }
631 //m_lastUpdateSent = false;
632 }
633
634 public override void AddAngularForce(Vector3 force, bool pushforce)
635 {
636
637 }
638
639 public override void SetMomentum(Vector3 momentum)
640 {
641 }
642
643
644 // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
645 // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
646 // place that is safe to call this routine AvatarGeomAndBodyCreation.
647 private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ)
648 {
649 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
650 if (CAPSULE_LENGTH <= 0)
651 {
652 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
653 CAPSULE_LENGTH = 0.01f;
654
655 }
656
657 if (CAPSULE_RADIUS <= 0)
658 {
659 m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
660 CAPSULE_RADIUS = 0.01f;
661
662 }
663 Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH);
664
665 d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
666 d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
667
668 d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
669
670 m_mass = ShellMass.mass; // update mass
671
672 // rescale PID parameters
673 PID_D = _parent_scene.avPIDD;
674 PID_P = _parent_scene.avPIDP;
675
676 // rescale PID parameters so that this aren't affected by mass
677 // and so don't get unstable for some masses
678 // also scale by ode time step so you don't need to refix them
679
680 PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps
681 PID_D *= m_mass / _parent_scene.ODE_STEPSIZE;
682 PID_P /= 50 * 80;
683 PID_P *= m_mass / _parent_scene.ODE_STEPSIZE;
684
685 Body = d.BodyCreate(_parent_scene.world);
686
687 d.BodySetAutoDisableFlag(Body, false);
688 d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
689
690 _position.X = npositionX;
691 _position.Y = npositionY;
692 _position.Z = npositionZ;
693
694 d.BodySetMass(Body, ref ShellMass);
695 d.GeomSetBody(Shell, Body);
696
697 // The purpose of the AMotor here is to keep the avatar's physical
698 // surrogate from rotating while moving
699 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
700 d.JointAttach(Amotor, Body, IntPtr.Zero);
701
702 d.JointSetAMotorMode(Amotor, 0);
703 d.JointSetAMotorNumAxes(Amotor, 3);
704 d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
705 d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
706 d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
707
708 d.JointSetAMotorAngle(Amotor, 0, 0);
709 d.JointSetAMotorAngle(Amotor, 1, 0);
710 d.JointSetAMotorAngle(Amotor, 2, 0);
711
712 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD
713 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
714 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
715 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f);
716 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f);
717 d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f);
718
719 // These lowstops and high stops are effectively (no wiggle room)
720 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f);
721 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f);
722 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f);
723 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f);
724 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f);
725 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f);
726
727 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
728 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0);
729 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0);
730
731 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f);
732 d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f);
733 d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f);
734 }
735
736 /// <summary>
737 /// Destroys the avatar body and geom
738
739 private void AvatarGeomAndBodyDestroy()
740 {
741 // Kill the Amotor
742 if (Amotor != IntPtr.Zero)
743 {
744 d.JointDestroy(Amotor);
745 Amotor = IntPtr.Zero;
746 }
747
748 if (Body != IntPtr.Zero)
749 {
750 //kill the body
751 d.BodyDestroy(Body);
752 Body = IntPtr.Zero;
753 }
754
755 //kill the Geometry
756 if (Shell != IntPtr.Zero)
757 {
758 _parent_scene.geom_name_map.Remove(Shell);
759 _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace);
760 d.GeomDestroy(Shell);
761 _parent_scene.geom_name_map.Remove(Shell);
762 Shell = IntPtr.Zero;
763 }
764 }
765
766 /// <summary>
767 /// Called from Simulate
768 /// This is the avatar's movement control + PID Controller
769 /// </summary>
770 /// <param name="timeStep"></param>
771 public void Move(float timeStep, List<OdeCharacter> defects)
772 {
773 // no lock; for now it's only called from within Simulate()
774
775 // If the PID Controller isn't active then we set our force
776 // calculating base velocity to the current position
777
778 if (Body == IntPtr.Zero)
779 return;
780
781 d.Vector3 dtmp;
782 d.BodyCopyPosition(Body, out dtmp);
783 Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
784
785 // the Amotor still lets avatar rotation to drift during colisions
786 // so force it back to identity
787
788 d.Quaternion qtmp;
789 qtmp.W = 1;
790 qtmp.X = 0;
791 qtmp.Y = 0;
792 qtmp.Z = 0;
793 d.BodySetQuaternion(Body, ref qtmp);
794
795 if (m_pidControllerActive == false)
796 {
797 _zeroPosition = localpos;
798 }
799 //PidStatus = true;
800
801
802 if (!localpos.IsFinite())
803 {
804
805 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
806 defects.Add(this);
807 // _parent_scene.RemoveCharacter(this);
808
809 // destroy avatar capsule and related ODE data
810 AvatarGeomAndBodyDestroy();
811
812 return;
813 }
814
815 Vector3 vec = Vector3.Zero;
816 dtmp = d.BodyGetLinearVel(Body);
817 Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
818
819 float movementdivisor = 1f;
820 //Ubit change divisions into multiplications below
821 if (!m_alwaysRun)
822 {
823 movementdivisor = 1 / walkDivisor;
824 }
825 else
826 {
827 movementdivisor = 1 / runDivisor;
828 }
829
830 // colide with land
831
832 d.AABB aabb;
833 d.GeomGetAABB(Shell, out aabb);
834 float chrminZ = aabb.MinZ;
835
836 Vector3 posch = localpos;
837
838 float ftmp;
839
840 if (flying)
841 {
842 ftmp = timeStep;
843 posch.X += vel.X * ftmp;
844 posch.Y += vel.Y * ftmp;
845 }
846
847 float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
848 if (chrminZ < terrainheight)
849 {
850 float depth = terrainheight - chrminZ;
851 if (!flying)
852 {
853 vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50;
854 }
855 else
856 vec.Z = depth * PID_P * 50;
857
858 /*
859 Vector3 vtmp;
860 vtmp.X = _target_velocity.X * timeStep;
861 vtmp.Y = _target_velocity.Y * timeStep;
862 // fake and avoid squares
863 float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y));
864 if (k > 0)
865 {
866 posch.X += vtmp.X;
867 posch.Y += vtmp.Y;
868 terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
869 k = 1 + Math.Abs(terrainheight) / k;
870 movementdivisor /= k;
871
872 if (k < 1)
873 k = 1;
874 }
875 */
876
877
878 if (depth < 0.1f)
879 {
880 m_iscolliding = true;
881 m_colliderfilter = 2;
882 m_iscollidingGround = true;
883
884 ContactPoint contact = new ContactPoint();
885 contact.PenetrationDepth = depth;
886 contact.Position.X = localpos.X;
887 contact.Position.Y = localpos.Y;
888 contact.Position.Z = chrminZ;
889 contact.SurfaceNormal.X = 0f;
890 contact.SurfaceNormal.Y = 0f;
891 contact.SurfaceNormal.Z = -1f;
892 AddCollisionEvent(0, contact);
893
894 vec.Z *= 0.5f;
895 }
896
897 else
898 m_iscollidingGround = false;
899 }
900 else
901 m_iscollidingGround = false;
902
903
904 // if velocity is zero, use position control; otherwise, velocity control
905 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f
906 && m_iscolliding)
907 {
908 // keep track of where we stopped. No more slippin' & slidin'
909 if (!_zeroFlag)
910 {
911 _zeroFlag = true;
912 _zeroPosition = localpos;
913 }
914 if (m_pidControllerActive)
915 {
916 // We only want to deactivate the PID Controller if we think we want to have our surrogate
917 // react to the physics scene by moving it's position.
918 // Avatar to Avatar collisions
919 // Prim to avatar collisions
920
921 vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2);
922 vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2);
923 if (flying)
924 {
925 vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P;
926 }
927 }
928 //PidStatus = true;
929 }
930 else
931 {
932 m_pidControllerActive = true;
933 _zeroFlag = false;
934
935 if (m_iscolliding)
936 {
937 if (!flying)
938 {
939 if (_target_velocity.Z > 0.0f)
940 {
941 // We're colliding with something and we're not flying but we're moving
942 // This means we're walking or running. JUMPING
943 vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P;
944 }
945 // We're standing on something
946 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D);
947 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D);
948 }
949 else
950 {
951 // We're flying and colliding with something
952 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f);
953 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f);
954 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
955 }
956 }
957 else // ie not colliding
958 {
959 if (flying) //(!m_iscolliding && flying)
960 {
961 // we're in mid air suspended
962 vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f);
963 vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f);
964 vec.Z += (_target_velocity.Z - vel.Z) * (PID_D);
965 }
966
967 else
968 {
969 // we're not colliding and we're not flying so that means we're falling!
970 // m_iscolliding includes collisions with the ground.
971
972 // d.Vector3 pos = d.BodyGetPosition(Body);
973 vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f;
974 vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f;
975 }
976 }
977 }
978
979 if (flying)
980 {
981 vec.Z -= _parent_scene.gravityz * m_mass;
982
983 //Added for auto fly height. Kitto Flora
984 float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset;
985
986 if (localpos.Z < target_altitude)
987 {
988 vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f;
989 }
990 // end add Kitto Flora
991 }
992
993 if (vec.IsFinite())
994 {
995 if (vec.X != 0 || vec.Y !=0 || vec.Z !=0)
996 d.BodyAddForce(Body, vec.X, vec.Y, vec.Z);
997 }
998 else
999 {
1000 m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
1001 m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
1002 defects.Add(this);
1003 // _parent_scene.RemoveCharacter(this);
1004 // destroy avatar capsule and related ODE data
1005 AvatarGeomAndBodyDestroy();
1006 }
1007 }
1008
1009 /// <summary>
1010 /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
1011 /// </summary>
1012 public void UpdatePositionAndVelocity()
1013 {
1014 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
1015 if (Body == IntPtr.Zero)
1016 return;
1017
1018 d.Vector3 vec;
1019 try
1020 {
1021 d.BodyCopyPosition(Body, out vec);
1022 }
1023 catch (NullReferenceException)
1024 {
1025 bad = true;
1026 _parent_scene.BadCharacter(this);
1027 vec = new d.Vector3(_position.X, _position.Y, _position.Z);
1028 base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
1029 m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
1030 }
1031
1032 _position.X = vec.X;
1033 _position.Y = vec.Y;
1034 _position.Z = vec.Z;
1035
1036 bool fixbody = false;
1037
1038 if (_position.X < 0.0f)
1039 {
1040 fixbody = true;
1041 _position.X = 0.1f;
1042 }
1043 else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f)
1044 {
1045 fixbody = true;
1046 _position.X = (int)_parent_scene.WorldExtents.X - 0.1f;
1047 }
1048
1049 if (_position.Y < 0.0f)
1050 {
1051 fixbody = true;
1052 _position.Y = 0.1f;
1053 }
1054 else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1)
1055 {
1056 fixbody = true;
1057 _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f;
1058 }
1059
1060 if (fixbody)
1061 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1062
1063 // Did we move last? = zeroflag
1064 // This helps keep us from sliding all over
1065/*
1066 if (_zeroFlag)
1067 {
1068 _velocity.X = 0.0f;
1069 _velocity.Y = 0.0f;
1070 _velocity.Z = 0.0f;
1071
1072 // Did we send out the 'stopped' message?
1073 if (!m_lastUpdateSent)
1074 {
1075 m_lastUpdateSent = true;
1076 base.RequestPhysicsterseUpdate();
1077 }
1078 }
1079 else
1080 {
1081 m_lastUpdateSent = false;
1082 */
1083 try
1084 {
1085 vec = d.BodyGetLinearVel(Body);
1086 }
1087 catch (NullReferenceException)
1088 {
1089 vec.X = _velocity.X;
1090 vec.Y = _velocity.Y;
1091 vec.Z = _velocity.Z;
1092 }
1093 _velocity.X = (vec.X);
1094 _velocity.Y = (vec.Y);
1095 _velocity.Z = (vec.Z);
1096 // }
1097 }
1098
1099 /// <summary>
1100 /// Cleanup the things we use in the scene.
1101 /// </summary>
1102 public void Destroy()
1103 {
1104 AddChange(changes.Remove, null);
1105 }
1106
1107 public override void CrossingFailure()
1108 {
1109 }
1110
1111 public override Vector3 PIDTarget { set { return; } }
1112 public override bool PIDActive { set { return; } }
1113 public override float PIDTau { set { return; } }
1114
1115 public override float PIDHoverHeight { set { return; } }
1116 public override bool PIDHoverActive { set { return; } }
1117 public override PIDHoverType PIDHoverType { set { return; } }
1118 public override float PIDHoverTau { set { return; } }
1119
1120 public override Quaternion APIDTarget { set { return; } }
1121
1122 public override bool APIDActive { set { return; } }
1123
1124 public override float APIDStrength { set { return; } }
1125
1126 public override float APIDDamping { set { return; } }
1127
1128
1129 public override void SubscribeEvents(int ms)
1130 {
1131 m_requestedUpdateFrequency = ms;
1132 m_eventsubscription = ms;
1133 _parent_scene.AddCollisionEventReporting(this);
1134 m_haseventsubscription = true;
1135 }
1136
1137 public override void UnSubscribeEvents()
1138 {
1139 m_haseventsubscription = false;
1140 _parent_scene.RemoveCollisionEventReporting(this);
1141 m_requestedUpdateFrequency = 0;
1142 m_eventsubscription = 0;
1143 }
1144
1145 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
1146 {
1147 if (m_haseventsubscription)
1148 {
1149 // m_log.DebugFormat(
1150 // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
1151
1152 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
1153 }
1154 }
1155
1156 public void SendCollisions()
1157 {
1158 if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency)
1159 {
1160 if (CollisionEventsThisFrame != null)
1161 {
1162 base.SendCollisionUpdate(CollisionEventsThisFrame);
1163 }
1164 CollisionEventsThisFrame = new CollisionEventUpdate();
1165 m_eventsubscription = 0;
1166 }
1167 }
1168
1169 public override bool SubscribedEvents()
1170 {
1171 return m_haseventsubscription;
1172 }
1173
1174 private void changePhysicsStatus(bool NewStatus)
1175 {
1176 if (NewStatus != m_isPhysical)
1177 {
1178 if (NewStatus)
1179 {
1180 // Create avatar capsule and related ODE data
1181 if ((Shell != IntPtr.Zero))
1182 {
1183 // a lost shell ?
1184 m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
1185 + (Shell != IntPtr.Zero ? "Shell " : "")
1186 + (Body != IntPtr.Zero ? "Body " : "")
1187 + (Amotor != IntPtr.Zero ? "Amotor " : ""));
1188 AvatarGeomAndBodyDestroy();
1189 }
1190
1191 AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);
1192 _parent_scene.geom_name_map[Shell] = m_name;
1193 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1194 _parent_scene.AddCharacter(this);
1195 }
1196 else
1197 {
1198 _parent_scene.RemoveCharacter(this);
1199 // destroy avatar capsule and related ODE data
1200 AvatarGeomAndBodyDestroy();
1201 }
1202
1203 m_isPhysical = NewStatus;
1204 }
1205 }
1206
1207 private void changeAdd()
1208 {
1209 changePhysicsStatus(true);
1210 }
1211
1212 private void changeRemove()
1213 {
1214 changePhysicsStatus(false);
1215 }
1216
1217 private void changeShape(PrimitiveBaseShape arg)
1218 {
1219 }
1220
1221 private void changeSize(Vector3 Size)
1222 {
1223 if (Size.IsFinite())
1224 {
1225 float caplen = Size.Z;
1226
1227 caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f;
1228
1229 if (caplen != CAPSULE_LENGTH)
1230 {
1231 if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
1232 {
1233 AvatarGeomAndBodyDestroy();
1234
1235 float prevCapsule = CAPSULE_LENGTH;
1236 CAPSULE_LENGTH = caplen;
1237
1238 AvatarGeomAndBodyCreation(_position.X, _position.Y,
1239 _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2));
1240
1241 Velocity = Vector3.Zero;
1242
1243 _parent_scene.geom_name_map[Shell] = m_name;
1244 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
1245 }
1246 else
1247 {
1248 m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
1249 + (Shell == IntPtr.Zero ? "Shell " : "")
1250 + (Body == IntPtr.Zero ? "Body " : "")
1251 + (Amotor == IntPtr.Zero ? "Amotor " : ""));
1252 }
1253 }
1254
1255 m_pidControllerActive = true;
1256 }
1257 else
1258 {
1259 m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
1260 }
1261 }
1262
1263 private void changePosition( Vector3 newPos)
1264 {
1265 if (Body != IntPtr.Zero)
1266 d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
1267 _position = newPos;
1268 }
1269
1270 private void changeOrientation(Quaternion newOri)
1271 {
1272 }
1273
1274 private void changeVelocity(Vector3 newVel)
1275 {
1276 m_pidControllerActive = true;
1277 _target_velocity = newVel;
1278 }
1279
1280 private void changeSetTorque(Vector3 newTorque)
1281 {
1282 }
1283
1284 private void changeAddForce(Vector3 newForce)
1285 {
1286 }
1287
1288 private void changeAddAngularForce(Vector3 arg)
1289 {
1290 }
1291
1292 private void changeAngularLock(Vector3 arg)
1293 {
1294 }
1295
1296 private void changeFloatOnWater(bool arg)
1297 {
1298 }
1299
1300 private void changeVolumedetetion(bool arg)
1301 {
1302 }
1303
1304 private void changeSelectedStatus(bool arg)
1305 {
1306 }
1307
1308 private void changeDisable(bool arg)
1309 {
1310 }
1311
1312 private void changeBuilding(bool arg)
1313 {
1314 }
1315
1316 private void changeForce(Vector3 newForce)
1317 {
1318 m_pidControllerActive = false;
1319 if (Body != IntPtr.Zero)
1320 {
1321 if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0)
1322 d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z);
1323 }
1324 }
1325
1326 private void donullchange()
1327 {
1328 }
1329
1330 public bool DoAChange(changes what, object arg)
1331 {
1332 if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove)
1333 {
1334 return false;
1335 }
1336
1337 // nasty switch
1338 switch (what)
1339 {
1340 case changes.Add:
1341 changeAdd();
1342 break;
1343 case changes.Remove:
1344 changeRemove();
1345 break;
1346
1347 case changes.Position:
1348 changePosition((Vector3)arg);
1349 break;
1350
1351 case changes.Orientation:
1352 changeOrientation((Quaternion)arg);
1353 break;
1354
1355 case changes.PosOffset:
1356 donullchange();
1357 break;
1358
1359 case changes.OriOffset:
1360 donullchange();
1361 break;
1362
1363 case changes.Velocity:
1364 changeVelocity((Vector3)arg);
1365 break;
1366
1367 // case changes.Acceleration:
1368 // changeacceleration((Vector3)arg);
1369 // break;
1370 // case changes.AngVelocity:
1371 // changeangvelocity((Vector3)arg);
1372 // break;
1373
1374 case changes.Force:
1375 changeForce((Vector3)arg);
1376 break;
1377
1378 case changes.Torque:
1379 changeSetTorque((Vector3)arg);
1380 break;
1381
1382 case changes.AddForce:
1383 changeAddForce((Vector3)arg);
1384 break;
1385
1386 case changes.AddAngForce:
1387 changeAddAngularForce((Vector3)arg);
1388 break;
1389
1390 case changes.AngLock:
1391 changeAngularLock((Vector3)arg);
1392 break;
1393
1394 case changes.Size:
1395 changeSize((Vector3)arg);
1396 break;
1397/* not in use for now
1398 case changes.Shape:
1399 changeShape((PrimitiveBaseShape)arg);
1400 break;
1401
1402 case changes.CollidesWater:
1403 changeFloatOnWater((bool)arg);
1404 break;
1405
1406 case changes.VolumeDtc:
1407 changeVolumedetetion((bool)arg);
1408 break;
1409
1410 case changes.Physical:
1411 changePhysicsStatus((bool)arg);
1412 break;
1413
1414 case changes.Selected:
1415 changeSelectedStatus((bool)arg);
1416 break;
1417
1418 case changes.disabled:
1419 changeDisable((bool)arg);
1420 break;
1421
1422 case changes.building:
1423 changeBuilding((bool)arg);
1424 break;
1425*/
1426 case changes.Null:
1427 donullchange();
1428 break;
1429
1430 default:
1431 donullchange();
1432 break;
1433 }
1434 return false;
1435 }
1436
1437 public void AddChange(changes what, object arg)
1438 {
1439 _parent_scene.AddChange((PhysicsActor)this, what, arg);
1440 }
1441
1442
1443 internal void AddCollisionFrameTime(int p)
1444 {
1445 // protect it from overflow crashing
1446 if (m_eventsubscription + p >= int.MaxValue)
1447 m_eventsubscription = 0;
1448 m_eventsubscription += p;
1449 }
1450 }
1451}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
new file mode 100644
index 0000000..80218e7
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs
@@ -0,0 +1,849 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
29 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
30 * ODEPrim.cs contains methods dealing with Prim editing, Prim
31 * characteristics and Kinetic motion.
32 * ODEDynamics.cs contains methods dealing with Prim Physical motion
33 * (dynamics) and the associated settings. Old Linear and angular
34 * motors for dynamic motion have been replace with MoveLinear()
35 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
36 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
37 * switch between 'VEHICLE' parameter use and general dynamics
38 * settings use.
39 */
40
41// Ubit 2012
42
43using System;
44using System.Collections.Generic;
45using System.Reflection;
46using System.Runtime.InteropServices;
47using log4net;
48using OpenMetaverse;
49using OdeAPI;
50using OpenSim.Framework;
51using OpenSim.Region.Physics.Manager;
52
53namespace OpenSim.Region.Physics.OdePlugin
54{
55 public class ODEDynamics
56 {
57 public Vehicle Type
58 {
59 get { return m_type; }
60 }
61
62 private OdePrim rootPrim;
63 private OdeScene _pParentScene;
64
65 // Vehicle properties
66 private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
67 private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ?
68
69 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
70
71 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
72 // HOVER_TERRAIN_ONLY
73 // HOVER_GLOBAL_HEIGHT
74 // NO_DEFLECTION_UP
75 // HOVER_WATER_ONLY
76 // HOVER_UP_ONLY
77 // LIMIT_MOTOR_UP
78 // LIMIT_ROLL_ONLY
79 private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl
80
81 // Linear properties
82 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
83 private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
84 private float m_linearMotorDecayTimescale = 120;
85 private float m_linearMotorTimescale = 1000;
86 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
87 private Vector3 m_linearMotorOffset = Vector3.Zero;
88
89 //Angular properties
90 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
91 private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate
92 private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate
93 private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate
94 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
95
96 //Deflection properties
97 private float m_angularDeflectionEfficiency = 0;
98 private float m_angularDeflectionTimescale = 1000;
99 private float m_linearDeflectionEfficiency = 0;
100 private float m_linearDeflectionTimescale = 1000;
101
102 //Banking properties
103 private float m_bankingEfficiency = 0;
104 private float m_bankingMix = 0;
105 private float m_bankingTimescale = 0;
106
107 //Hover and Buoyancy properties
108 private float m_VhoverHeight = 0f;
109 private float m_VhoverEfficiency = 0f;
110 private float m_VhoverTimescale = 1000f;
111 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
112 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
113 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
114 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
115
116 //Attractor properties
117 private float m_verticalAttractionEfficiency = 1.0f; // damped
118 private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor.
119
120 // auxiliar
121 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
122
123 private float m_lmEfect = 0; // current linear motor eficiency
124 private float m_amEfect = 0; // current angular motor eficiency
125
126
127 public ODEDynamics(OdePrim rootp)
128 {
129 rootPrim = rootp;
130 _pParentScene = rootPrim._parent_scene;
131 }
132
133 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
134 {
135 float len;
136 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
137 float timestep = _pParentScene.ODE_STEPSIZE;
138
139 switch (pParam)
140 {
141 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
142 if (pValue < 0f) pValue = 0f;
143 if (pValue > 1f) pValue = 1f;
144 m_angularDeflectionEfficiency = pValue;
145 break;
146 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
147 if (pValue < timestep) pValue = timestep;
148 m_angularDeflectionTimescale = pValue;
149 break;
150 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
151 // if (pValue < timestep) pValue = timestep;
152 // try to make impulses to work a bit better
153 if (pValue < 0.5f) pValue = 0.5f;
154 else if (pValue > 120) pValue = 120;
155 m_angularMotorDecayTimescale = pValue * invtimestep;
156 break;
157 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
158 if (pValue < timestep) pValue = timestep;
159 m_angularMotorTimescale = pValue;
160 break;
161 case Vehicle.BANKING_EFFICIENCY:
162 if (pValue < -1f) pValue = -1f;
163 if (pValue > 1f) pValue = 1f;
164 m_bankingEfficiency = pValue;
165 break;
166 case Vehicle.BANKING_MIX:
167 if (pValue < 0f) pValue = 0f;
168 if (pValue > 1f) pValue = 1f;
169 m_bankingMix = pValue;
170 break;
171 case Vehicle.BANKING_TIMESCALE:
172 if (pValue < timestep) pValue = timestep;
173 m_bankingTimescale = pValue;
174 break;
175 case Vehicle.BUOYANCY:
176 if (pValue < -1f) pValue = -1f;
177 if (pValue > 1f) pValue = 1f;
178 m_VehicleBuoyancy = pValue;
179 break;
180 case Vehicle.HOVER_EFFICIENCY:
181 if (pValue < 0f) pValue = 0f;
182 if (pValue > 1f) pValue = 1f;
183 m_VhoverEfficiency = pValue;
184 break;
185 case Vehicle.HOVER_HEIGHT:
186 m_VhoverHeight = pValue;
187 break;
188 case Vehicle.HOVER_TIMESCALE:
189 if (pValue < timestep) pValue = timestep;
190 m_VhoverTimescale = pValue;
191 break;
192 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
193 if (pValue < 0f) pValue = 0f;
194 if (pValue > 1f) pValue = 1f;
195 m_linearDeflectionEfficiency = pValue;
196 break;
197 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
198 if (pValue < timestep) pValue = timestep;
199 m_linearDeflectionTimescale = pValue;
200 break;
201 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
202 // if (pValue < timestep) pValue = timestep;
203 // try to make impulses to work a bit better
204 if (pValue < 0.5f) pValue = 0.5f;
205 else if (pValue > 120) pValue = 120;
206 m_linearMotorDecayTimescale = pValue * invtimestep;
207 break;
208 case Vehicle.LINEAR_MOTOR_TIMESCALE:
209 if (pValue < timestep) pValue = timestep;
210 m_linearMotorTimescale = pValue;
211 break;
212 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
213 if (pValue < 0f) pValue = 0f;
214 if (pValue > 1f) pValue = 1f;
215 m_verticalAttractionEfficiency = pValue;
216 break;
217 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
218 if (pValue < timestep) pValue = timestep;
219 m_verticalAttractionTimescale = pValue;
220 break;
221
222 // These are vector properties but the engine lets you use a single float value to
223 // set all of the components to the same value
224 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
225 if (pValue < timestep) pValue = timestep;
226 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
227 break;
228 case Vehicle.ANGULAR_MOTOR_DIRECTION:
229 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
230 len = m_angularMotorDirection.Length();
231 if (len > 12.566f)
232 m_angularMotorDirection *= (12.566f / len);
233 m_amEfect = 1.0f; // turn it on
234 break;
235 case Vehicle.LINEAR_FRICTION_TIMESCALE:
236 if (pValue < timestep) pValue = timestep;
237 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
238 break;
239 case Vehicle.LINEAR_MOTOR_DIRECTION:
240 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
241 len = m_linearMotorDirection.Length();
242 if (len > 30.0f)
243 m_linearMotorDirection *= (30.0f / len);
244 m_lmEfect = 1.0f; // turn it on
245 break;
246 case Vehicle.LINEAR_MOTOR_OFFSET:
247 m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
248 len = m_linearMotorOffset.Length();
249 if (len > 100.0f)
250 m_linearMotorOffset *= (100.0f / len);
251 break;
252 }
253 }//end ProcessFloatVehicleParam
254
255 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
256 {
257 float len;
258 float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE;
259 float timestep = _pParentScene.ODE_STEPSIZE;
260 switch (pParam)
261 {
262 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
263 if (pValue.X < timestep) pValue.X = timestep;
264 if (pValue.Y < timestep) pValue.Y = timestep;
265 if (pValue.Z < timestep) pValue.Z = timestep;
266
267 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
268 break;
269 case Vehicle.ANGULAR_MOTOR_DIRECTION:
270 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
271 // Limit requested angular speed to 2 rps= 4 pi rads/sec
272 len = m_angularMotorDirection.Length();
273 if (len > 12.566f)
274 m_angularMotorDirection *= (12.566f / len);
275 m_amEfect = 1.0f; // turn it on
276 break;
277 case Vehicle.LINEAR_FRICTION_TIMESCALE:
278 if (pValue.X < timestep) pValue.X = timestep;
279 if (pValue.Y < timestep) pValue.Y = timestep;
280 if (pValue.Z < timestep) pValue.Z = timestep;
281 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
282 break;
283 case Vehicle.LINEAR_MOTOR_DIRECTION:
284 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
285 len = m_linearMotorDirection.Length();
286 if (len > 30.0f)
287 m_linearMotorDirection *= (30.0f / len);
288 m_lmEfect = 1.0f; // turn it on
289 break;
290 case Vehicle.LINEAR_MOTOR_OFFSET:
291 m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
292 len = m_linearMotorOffset.Length();
293 if (len > 100.0f)
294 m_linearMotorOffset *= (100.0f / len);
295 break;
296 case Vehicle.BLOCK_EXIT:
297 m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
298 break;
299 }
300 }//end ProcessVectorVehicleParam
301
302 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
303 {
304 switch (pParam)
305 {
306 case Vehicle.REFERENCE_FRAME:
307 m_referenceFrame = Quaternion.Inverse(pValue);
308 break;
309 case Vehicle.ROLL_FRAME:
310 m_RollreferenceFrame = pValue;
311 break;
312 }
313 }//end ProcessRotationVehicleParam
314
315 internal void ProcessVehicleFlags(int pParam, bool remove)
316 {
317 if (remove)
318 {
319 m_flags &= ~((VehicleFlag)pParam);
320 }
321 else
322 {
323 m_flags |= (VehicleFlag)pParam;
324 }
325 }//end ProcessVehicleFlags
326
327 internal void ProcessTypeChange(Vehicle pType)
328 {
329 float invtimestep = _pParentScene.ODE_STEPSIZE;
330 m_lmEfect = 0;
331 m_amEfect = 0;
332
333 m_linearMotorDirection = Vector3.Zero;
334 m_angularMotorDirection = Vector3.Zero;
335
336 m_BlockingEndPoint = Vector3.Zero;
337 m_RollreferenceFrame = Quaternion.Identity;
338 m_linearMotorOffset = Vector3.Zero;
339
340 m_referenceFrame = Quaternion.Identity;
341
342 // Set Defaults For Type
343 m_type = pType;
344 switch (pType)
345 {
346 case Vehicle.TYPE_NONE:
347 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000);
348 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
349 m_linearMotorTimescale = 1000;
350 m_linearMotorDecayTimescale = 120 * invtimestep;
351 m_angularMotorTimescale = 1000;
352 m_angularMotorDecayTimescale = 1000 * invtimestep;
353 m_VhoverHeight = 0;
354 m_VhoverTimescale = 1000;
355 m_VehicleBuoyancy = 0;
356 m_flags = (VehicleFlag)0;
357 break;
358
359 case Vehicle.TYPE_SLED:
360 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
361 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
362 m_linearMotorTimescale = 1000;
363 m_linearMotorDecayTimescale = 120 * invtimestep;
364 m_angularMotorTimescale = 1000;
365 m_angularMotorDecayTimescale = 120 * invtimestep;
366 m_VhoverHeight = 0;
367 m_VhoverEfficiency = 1;
368 m_VhoverTimescale = 10;
369 m_VehicleBuoyancy = 0;
370 m_linearDeflectionEfficiency = 1;
371 m_linearDeflectionTimescale = 1;
372 m_angularDeflectionEfficiency = 0;
373 m_angularDeflectionTimescale = 1000;
374 m_bankingEfficiency = 0;
375 m_bankingMix = 1;
376 m_bankingTimescale = 10;
377 m_flags &=
378 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
379 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
380 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
381 break;
382 case Vehicle.TYPE_CAR:
383 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
384 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
385 m_linearMotorTimescale = 1;
386 m_linearMotorDecayTimescale = 60 * invtimestep;
387 m_angularMotorTimescale = 1;
388 m_angularMotorDecayTimescale = 0.8f * invtimestep;
389 m_VhoverHeight = 0;
390 m_VhoverEfficiency = 0;
391 m_VhoverTimescale = 1000;
392 m_VehicleBuoyancy = 0;
393 m_linearDeflectionEfficiency = 1;
394 m_linearDeflectionTimescale = 2;
395 m_angularDeflectionEfficiency = 0;
396 m_angularDeflectionTimescale = 10;
397 m_verticalAttractionEfficiency = 1f;
398 m_verticalAttractionTimescale = 10f;
399 m_bankingEfficiency = -0.2f;
400 m_bankingMix = 1;
401 m_bankingTimescale = 1;
402 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
403 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
404 VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY);
405 break;
406 case Vehicle.TYPE_BOAT:
407 m_linearFrictionTimescale = new Vector3(10, 3, 2);
408 m_angularFrictionTimescale = new Vector3(10, 10, 10);
409 m_linearMotorTimescale = 5;
410 m_linearMotorDecayTimescale = 60 * invtimestep;
411 m_angularMotorTimescale = 4;
412 m_angularMotorDecayTimescale = 4 * invtimestep;
413 m_VhoverHeight = 0;
414 m_VhoverEfficiency = 0.5f;
415 m_VhoverTimescale = 2;
416 m_VehicleBuoyancy = 1;
417 m_linearDeflectionEfficiency = 0.5f;
418 m_linearDeflectionTimescale = 3;
419 m_angularDeflectionEfficiency = 0.5f;
420 m_angularDeflectionTimescale = 5;
421 m_verticalAttractionEfficiency = 0.5f;
422 m_verticalAttractionTimescale = 5f;
423 m_bankingEfficiency = -0.3f;
424 m_bankingMix = 0.8f;
425 m_bankingTimescale = 1;
426 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
427 VehicleFlag.HOVER_GLOBAL_HEIGHT |
428 VehicleFlag.HOVER_UP_ONLY |
429 VehicleFlag.LIMIT_ROLL_ONLY);
430 m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
431 VehicleFlag.LIMIT_MOTOR_UP |
432 VehicleFlag.HOVER_WATER_ONLY);
433 break;
434 case Vehicle.TYPE_AIRPLANE:
435 m_linearFrictionTimescale = new Vector3(200, 10, 5);
436 m_angularFrictionTimescale = new Vector3(20, 20, 20);
437 m_linearMotorTimescale = 2;
438 m_linearMotorDecayTimescale = 60 * invtimestep;
439 m_angularMotorTimescale = 4;
440 m_angularMotorDecayTimescale = 8 * invtimestep;
441 m_VhoverHeight = 0;
442 m_VhoverEfficiency = 0.5f;
443 m_VhoverTimescale = 1000;
444 m_VehicleBuoyancy = 0;
445 m_linearDeflectionEfficiency = 0.5f;
446 m_linearDeflectionTimescale = 0.5f;
447 m_angularDeflectionEfficiency = 1;
448 m_angularDeflectionTimescale = 2;
449 m_verticalAttractionEfficiency = 0.9f;
450 m_verticalAttractionTimescale = 2f;
451 m_bankingEfficiency = 1;
452 m_bankingMix = 0.7f;
453 m_bankingTimescale = 2;
454 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
455 VehicleFlag.HOVER_TERRAIN_ONLY |
456 VehicleFlag.HOVER_GLOBAL_HEIGHT |
457 VehicleFlag.HOVER_UP_ONLY |
458 VehicleFlag.NO_DEFLECTION_UP |
459 VehicleFlag.LIMIT_MOTOR_UP);
460 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
461 break;
462 case Vehicle.TYPE_BALLOON:
463 m_linearFrictionTimescale = new Vector3(5, 5, 5);
464 m_angularFrictionTimescale = new Vector3(10, 10, 10);
465 m_linearMotorTimescale = 5;
466 m_linearMotorDecayTimescale = 60 * invtimestep;
467 m_angularMotorTimescale = 6;
468 m_angularMotorDecayTimescale = 10 * invtimestep;
469 m_VhoverHeight = 5;
470 m_VhoverEfficiency = 0.8f;
471 m_VhoverTimescale = 10;
472 m_VehicleBuoyancy = 1;
473 m_linearDeflectionEfficiency = 0;
474 m_linearDeflectionTimescale = 5 * invtimestep;
475 m_angularDeflectionEfficiency = 0;
476 m_angularDeflectionTimescale = 5;
477 m_verticalAttractionEfficiency = 0f;
478 m_verticalAttractionTimescale = 1000f;
479 m_bankingEfficiency = 0;
480 m_bankingMix = 0.7f;
481 m_bankingTimescale = 5;
482 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY |
483 VehicleFlag.HOVER_TERRAIN_ONLY |
484 VehicleFlag.HOVER_UP_ONLY |
485 VehicleFlag.NO_DEFLECTION_UP |
486 VehicleFlag.LIMIT_MOTOR_UP);
487 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY |
488 VehicleFlag.HOVER_GLOBAL_HEIGHT);
489 break;
490 }
491
492 }//end SetDefaultsForType
493
494 internal void Stop()
495 {
496 m_lmEfect = 0;
497 m_amEfect = 0;
498 }
499
500 public static Vector3 Xrot(Quaternion rot)
501 {
502 Vector3 vec;
503 rot.Normalize(); // just in case
504 vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1;
505 vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W);
506 vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W);
507 return vec;
508 }
509
510 public static Vector3 Zrot(Quaternion rot)
511 {
512 Vector3 vec;
513 rot.Normalize(); // just in case
514 vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W);
515 vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W);
516 vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1;
517
518 return vec;
519 }
520
521 private const float halfpi = 0.5f * (float)Math.PI;
522
523 public static Vector3 ubitRot2Euler(Quaternion rot)
524 {
525 // returns roll in X
526 // pitch in Y
527 // yaw in Z
528 Vector3 vec;
529
530 // assuming rot is normalised
531 // rot.Normalize();
532
533 float zX = rot.X * rot.Z + rot.Y * rot.W;
534
535 if (zX < -0.49999f)
536 {
537 vec.X = 0;
538 vec.Y = -halfpi;
539 vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W));
540 }
541 else if (zX > 0.49999f)
542 {
543 vec.X = 0;
544 vec.Y = halfpi;
545 vec.Z = (float)(2d * Math.Atan(rot.X / rot.W));
546 }
547 else
548 {
549 vec.Y = (float)Math.Asin(2 * zX);
550
551 float sqw = rot.W * rot.W;
552
553 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
554 float zZ = rot.Z * rot.Z + sqw - 0.5f;
555
556 vec.X = (float)Math.Atan2(minuszY, zZ);
557
558 float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?)
559 float yY = rot.X * rot.X + sqw - 0.5f;
560 vec.Z = (float)Math.Atan2(yX, yY);
561 }
562 return vec;
563 }
564
565 public static void GetRollPitch(Quaternion rot, out float roll, out float pitch)
566 {
567 // assuming rot is normalised
568 // rot.Normalize();
569
570 float zX = rot.X * rot.Z + rot.Y * rot.W;
571
572 if (zX < -0.49999f)
573 {
574 roll = 0;
575 pitch = -halfpi;
576 }
577 else if (zX > 0.49999f)
578 {
579 roll = 0;
580 pitch = halfpi;
581 }
582 else
583 {
584 pitch = (float)Math.Asin(2 * zX);
585
586 float minuszY = rot.X * rot.W - rot.Y * rot.Z;
587 float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f;
588
589 roll = (float)Math.Atan2(minuszY, zZ);
590 }
591 return ;
592 }
593
594 internal void Step()//float pTimestep)
595 {
596 IntPtr Body = rootPrim.Body;
597
598 d.Quaternion rot = d.BodyGetQuaternion(Body);
599 Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
600 Quaternion rotq = objrotq; // rotq = rotation of object
601 rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame
602 Quaternion irotq = Quaternion.Inverse(rotq);
603
604 d.Vector3 dvtmp;
605 Vector3 tmpV;
606 Vector3 curVel; // velocity in world
607 Vector3 curAngVel; // angular velocity in world
608 Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
609 Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame
610 d.Vector3 dtorque = new d.Vector3();
611
612 dvtmp = d.BodyGetLinearVel(Body);
613 curVel.X = dvtmp.X;
614 curVel.Y = dvtmp.Y;
615 curVel.Z = dvtmp.Z;
616 Vector3 curLocalVel = curVel * irotq; // current velocity in local
617
618 dvtmp = d.BodyGetAngularVel(Body);
619 curAngVel.X = dvtmp.X;
620 curAngVel.Y = dvtmp.Y;
621 curAngVel.Z = dvtmp.Z;
622 Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local
623
624 // linear motor
625 if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
626 {
627 tmpV = m_linearMotorDirection - curLocalVel; // velocity error
628 tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep
629 tmpV *= rotq; // to world
630
631 if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
632 tmpV.Z = 0;
633
634 if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
635 {
636 // have offset, do it now
637 tmpV *= rootPrim.Mass;
638 d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
639 }
640 else
641 {
642 force.X += tmpV.X;
643 force.Y += tmpV.Y;
644 force.Z += tmpV.Z;
645 }
646 m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale);
647 }
648 else
649 m_lmEfect = 0;
650
651 // friction
652 if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
653 {
654 tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
655 tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
656 tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
657 tmpV *= rotq; // to world
658 force.X += tmpV.X;
659 force.Y += tmpV.Y;
660 force.Z += tmpV.Z;
661 }
662
663 // hover
664 if (m_VhoverTimescale < 300)
665 {
666 d.Vector3 pos = d.BodyGetPosition(Body);
667
668 // default to global
669 float perr = m_VhoverHeight - pos.Z;;
670
671 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
672 {
673 perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
674 }
675 else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
676 {
677 perr += _pParentScene.GetWaterLevel();
678 }
679 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
680 {
681 float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
682 float w = _pParentScene.GetWaterLevel();
683 if (t > w)
684 perr += t;
685 else
686 perr += w;
687 }
688
689 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0)
690 {
691 force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE;
692 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
693 }
694 else // no buoyancy
695 force.Z += _pParentScene.gravityz;
696 }
697 else
698 {
699 // default gravity and buoancy
700 force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy);
701 }
702
703 // linear deflection
704 if (m_linearDeflectionEfficiency > 0)
705 {
706 float len = curVel.Length();
707 Vector3 atAxis;
708 atAxis = Xrot(rotq); // where are we pointing to
709 atAxis *= len; // make it same size as world velocity vector
710 tmpV = -atAxis; // oposite direction
711 atAxis -= curVel; // error to one direction
712 len = atAxis.LengthSquared();
713 tmpV -= curVel; // error to oposite
714 float lens = tmpV.LengthSquared();
715 if (len > 0.01 || lens > 0.01) // do nothing if close enougth
716 {
717 if (len < lens)
718 tmpV = atAxis;
719
720 tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
721 force.X += tmpV.X;
722 force.Y += tmpV.Y;
723 if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
724 force.Z += tmpV.Z;
725 }
726 }
727
728 // angular motor
729 if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
730 {
731 tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error
732 tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep
733 torque.X += tmpV.X;
734 torque.Y += tmpV.Y;
735 torque.Z += tmpV.Z;
736 m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale);
737 }
738 else
739 m_amEfect = 0;
740
741 // angular friction
742 if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
743 {
744 torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X;
745 torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y;
746 torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z;
747 }
748
749 // angular deflection
750 if (m_angularDeflectionEfficiency > 0)
751 {
752 Vector3 dirv;
753
754 if (curLocalVel.X > 0.01f)
755 dirv = curLocalVel;
756 else if (curLocalVel.X < -0.01f)
757 // use oposite
758 dirv = -curLocalVel;
759 else
760 {
761 // make it fall into small positive x case
762 dirv.X = 0.01f;
763 dirv.Y = curLocalVel.Y;
764 dirv.Z = curLocalVel.Z;
765 }
766
767 float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;
768
769 if (Math.Abs(dirv.Z) > 0.01)
770 {
771 torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
772 }
773
774 if (Math.Abs(dirv.Y) > 0.01)
775 {
776 torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
777 }
778 }
779
780 // vertical atractor
781 if (m_verticalAttractionTimescale < 300)
782 {
783 float roll;
784 float pitch;
785
786 GetRollPitch(irotq, out roll, out pitch);
787
788 float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE;
789 float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE;
790
791 if (Math.Abs(roll) > 0.01) // roll
792 {
793 torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2;
794 }
795
796 if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch
797 {
798 torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2;
799 }
800
801 if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01)
802 {
803 float broll = roll * m_bankingEfficiency; ;
804 if (m_bankingMix != 0)
805 {
806 float vfact = Math.Abs(curLocalVel.X) / 10.0f;
807 if (vfact > 1.0f) vfact = 1.0f;
808 if (curLocalVel.X >= 0)
809 broll *= ((1 - m_bankingMix) + vfact);
810 else
811 broll *= -((1 - m_bankingMix) + vfact);
812 }
813 broll = (broll - curLocalAngVel.Z) / m_bankingTimescale;
814 // torque.Z += broll;
815
816 // make z rot be in world Z not local as seems to be in sl
817 tmpV.X = 0;
818 tmpV.Y = 0;
819 tmpV.Z = broll;
820 tmpV *= irotq;
821
822 torque.X += tmpV.X;
823 torque.Y += tmpV.Y;
824 torque.Z += tmpV.Z;
825 }
826 }
827
828 d.Mass dmass;
829 d.BodyGetMass(Body,out dmass);
830
831 if (force.X != 0 || force.Y != 0 || force.Z != 0)
832 {
833 force *= dmass.mass;
834 d.BodySetForce(Body, force.X, force.Y, force.Z);
835 }
836
837 if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
838 {
839 torque *= m_referenceFrame; // to object frame
840 dtorque.X = torque.X;
841 dtorque.Y = torque.Y;
842 dtorque.Z = torque.Z;
843
844 d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
845 d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
846 }
847 }
848 }
849}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
new file mode 100644
index 0000000..3b7f562
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -0,0 +1,3317 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
33/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
36 * ODEPrim.cs contains methods dealing with Prim editing, Prim
37 * characteristics and Kinetic motion.
38 * ODEDynamics.cs contains methods dealing with Prim Physical motion
39 * (dynamics) and the associated settings. Old Linear and angular
40 * motors for dynamic motion have been replace with MoveLinear()
41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
43 * switch between 'VEHICLE' parameter use and general dynamics
44 * settings use.
45 */
46
47//#define SPAM
48
49using System;
50using System.Collections.Generic;
51using System.Reflection;
52using System.Runtime.InteropServices;
53using System.Threading;
54using log4net;
55using OpenMetaverse;
56using OdeAPI;
57using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager;
59
60
61namespace OpenSim.Region.Physics.OdePlugin
62{
63 public class OdePrim : PhysicsActor
64 {
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66
67 private bool m_isphysical;
68 private bool m_fakeisphysical;
69
70 protected bool m_building;
71 private Quaternion m_lastorientation = new Quaternion();
72 private Quaternion _orientation;
73
74 private Vector3 _position;
75 private Vector3 _velocity;
76 private Vector3 _torque;
77 private Vector3 m_lastVelocity;
78 private Vector3 m_lastposition;
79 private Vector3 m_rotationalVelocity;
80 private Vector3 _size;
81 private Vector3 _acceleration;
82 private Vector3 m_angularlock = Vector3.One;
83 private IntPtr Amotor = IntPtr.Zero;
84
85 private Vector3 m_force;
86 private Vector3 m_forceacc;
87 private Vector3 m_angularForceacc;
88
89 private Vector3 m_PIDTarget;
90 private float m_PIDTau;
91 private float PID_D = 35f;
92 private float PID_G = 25f;
93 private bool m_usePID;
94
95 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
96 // and are for non-VEHICLES only.
97
98 private float m_PIDHoverHeight;
99 private float m_PIDHoverTau;
100 private bool m_useHoverPID;
101 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
102 private float m_targetHoverHeight;
103 private float m_groundHeight;
104 private float m_waterHeight;
105 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
106
107 private int body_autodisable_frames = 20;
108
109 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
110 | CollisionCategories.Space
111 | CollisionCategories.Body
112 | CollisionCategories.Character
113 );
114 private bool m_collidesLand = true;
115 private bool m_collidesWater;
116 public bool m_returnCollisions;
117
118 // Default we're a Geometry
119 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
120
121 // Default, Collide with Other Geometries, spaces and Bodies
122 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
123
124 public bool m_disabled;
125 public bool m_taintselected;
126
127 public uint m_localID;
128
129 private PrimitiveBaseShape _pbs;
130 public OdeScene _parent_scene;
131
132 /// <summary>
133 /// The physics space which contains prim geometry
134 /// </summary>
135 public IntPtr m_targetSpace = IntPtr.Zero;
136
137 public IntPtr prim_geom;
138 public IntPtr _triMeshData;
139
140 private PhysicsActor _parent;
141
142 private List<OdePrim> childrenPrim = new List<OdePrim>();
143
144 private bool m_iscolliding;
145 private bool m_wascolliding;
146 private bool m_isSelected;
147
148 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
149
150 private bool m_throttleUpdates;
151 private int throttleCounter;
152 public int m_interpenetrationcount;
153 public float m_collisionscore;
154 int m_colliderfilter = 0;
155 public int m_roundsUnderMotionThreshold;
156 private int m_crossingfailures;
157
158 public bool outofBounds;
159 private float m_density = 10.000006836f; // Aluminum g/cm3;
160
161 public bool _zeroFlag;
162 private bool m_lastUpdateSent;
163
164 public IntPtr Body = IntPtr.Zero;
165 public String Name { get; private set; }
166 private Vector3 _target_velocity;
167
168 public Vector3 primOOBsize; // prim real dimensions from mesh
169 public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb
170 public float primOOBradiusSQ;
171 public d.Mass primdMass; // prim inertia information on it's own referencial
172 float primMass; // prim own mass
173 float _mass; // object mass acording to case
174 public d.Mass objectpMass; // object last computed inertia
175 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
176
177 public int givefakepos = 0;
178 private Vector3 fakepos;
179 public int givefakeori = 0;
180 private Quaternion fakeori;
181
182 public int m_eventsubscription;
183 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
184
185 private IntPtr m_linkJoint = IntPtr.Zero;
186 private IntPtr _linkJointGroup = IntPtr.Zero;
187
188 public volatile bool childPrim;
189
190 public ODEDynamics m_vehicle;
191
192 internal int m_material = (int)Material.Wood;
193 protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f };
194
195 /// <summary>
196 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
197 /// </summary>
198 public override bool IsPhysical // this is not reliable for internal use
199 {
200 get { return m_fakeisphysical; }
201 set
202 {
203 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
204 // and also to stop imediatly some updates
205 // but real change will only happen in taintprocessing
206
207 if (!value) // Zero the remembered last velocity
208 m_lastVelocity = Vector3.Zero;
209 AddChange(changes.Physical, value);
210 }
211 }
212
213 public override bool Building // this is not reliable for internal use
214 {
215 get { return m_building; }
216 set
217 {
218 if (value)
219 m_building = true;
220 AddChange(changes.building, value);
221 }
222 }
223
224 public override ContactData ContactData
225 {
226 get
227 {
228 return primContactData;
229 }
230 }
231
232 public override int PhysicsActorType
233 {
234 get { return (int)ActorTypes.Prim; }
235 set { return; }
236 }
237
238 public override bool SetAlwaysRun
239 {
240 get { return false; }
241 set { return; }
242 }
243
244 public override uint LocalID
245 {
246 get
247 {
248 return m_localID;
249 }
250 set
251 {
252 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
253 m_localID = value;
254 }
255 }
256
257 public override bool Grabbed
258 {
259 set { return; }
260 }
261
262 public override bool Selected
263 {
264 set
265 {
266 if (value)
267 m_isSelected = value;
268 AddChange(changes.Selected, value);
269 }
270 }
271
272 public override bool Flying
273 {
274 // no flying prims for you
275 get { return false; }
276 set { }
277 }
278
279 public override bool IsColliding
280 {
281 get { return m_iscolliding; }
282 set
283 {
284 if (value)
285 {
286 m_colliderfilter += 2;
287 if (m_colliderfilter > 2)
288 m_colliderfilter = 2;
289 }
290 else
291 {
292 m_colliderfilter--;
293 if (m_colliderfilter < 0)
294 m_colliderfilter = 0;
295 }
296
297 if (m_colliderfilter == 0)
298 m_iscolliding = false;
299 else
300 m_iscolliding = true;
301
302 if (m_wascolliding != m_iscolliding)
303 {
304 if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero)
305 d.BodyEnable(Body);
306 m_wascolliding = m_iscolliding;
307 }
308 }
309 }
310
311 public override bool CollidingGround
312 {
313 get { return false; }
314 set { return; }
315 }
316
317 public override bool CollidingObj
318 {
319 get { return false; }
320 set { return; }
321 }
322
323 public override bool ThrottleUpdates
324 {
325 get { return m_throttleUpdates; }
326 set { m_throttleUpdates = value; }
327 }
328
329 public override bool Stopped
330 {
331 get { return _zeroFlag; }
332 }
333
334 public override Vector3 Position
335 {
336 get
337 {
338 if (givefakepos > 0)
339 return fakepos;
340 else
341 return _position;
342 }
343
344 set
345 {
346 fakepos = value;
347 givefakepos++;
348 AddChange(changes.Position, value);
349 }
350 }
351
352 public override Vector3 Size
353 {
354 get { return _size; }
355 set
356 {
357 if (value.IsFinite())
358 {
359 AddChange(changes.Size, value);
360 }
361 else
362 {
363 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
364 }
365 }
366 }
367
368 public override float Mass
369 {
370 get { return _mass; }
371 }
372
373 public override Vector3 Force
374 {
375 //get { return Vector3.Zero; }
376 get { return m_force; }
377 set
378 {
379 if (value.IsFinite())
380 {
381 AddChange(changes.Force, value);
382 }
383 else
384 {
385 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
386 }
387 }
388 }
389
390 public override void SetVolumeDetect(int param)
391 {
392 AddChange(changes.VolumeDtc, (param != 0));
393 }
394
395 public override Vector3 GeometricCenter
396 {
397 get
398 {
399 return Vector3.Zero;
400 }
401 }
402
403 public override Vector3 CenterOfMass
404 {
405 get
406 {
407 d.Vector3 dtmp;
408 if (IsPhysical && !childPrim && Body != IntPtr.Zero)
409 {
410 dtmp = d.BodyGetPosition(Body);
411 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
412 }
413 else if (prim_geom != IntPtr.Zero)
414 {
415 d.Quaternion dq;
416 d.GeomCopyQuaternion(prim_geom, out dq);
417 Quaternion q;
418 q.X = dq.X;
419 q.Y = dq.Y;
420 q.Z = dq.Z;
421 q.W = dq.W;
422
423 Vector3 vtmp = primOOBoffset * q;
424 dtmp = d.GeomGetPosition(prim_geom);
425 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
426 }
427 else
428 return Vector3.Zero;
429 }
430 }
431 /*
432 public override Vector3 PrimOOBsize
433 {
434 get
435 {
436 return primOOBsize;
437 }
438 }
439
440 public override Vector3 PrimOOBoffset
441 {
442 get
443 {
444 return primOOBoffset;
445 }
446 }
447
448 public override float PrimOOBRadiusSQ
449 {
450 get
451 {
452 return primOOBradiusSQ;
453 }
454 }
455 */
456 public override PrimitiveBaseShape Shape
457 {
458 set
459 {
460 AddChange(changes.Shape, value);
461 }
462 }
463
464 public override Vector3 Velocity
465 {
466 get
467 {
468 // Averate previous velocity with the new one so
469 // client object interpolation works a 'little' better
470 if (_zeroFlag)
471 return Vector3.Zero;
472 return _velocity;
473 }
474 set
475 {
476 if (value.IsFinite())
477 {
478 AddChange(changes.Velocity, value);
479 // _velocity = value;
480
481 }
482 else
483 {
484 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
485 }
486
487 }
488 }
489
490 public override Vector3 Torque
491 {
492 get
493 {
494 if (!IsPhysical || Body == IntPtr.Zero)
495 return Vector3.Zero;
496
497 return _torque;
498 }
499
500 set
501 {
502 if (value.IsFinite())
503 {
504 AddChange(changes.Torque, value);
505 }
506 else
507 {
508 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
509 }
510 }
511 }
512
513 public override float CollisionScore
514 {
515 get { return m_collisionscore; }
516 set { m_collisionscore = value; }
517 }
518
519 public override bool Kinematic
520 {
521 get { return false; }
522 set { }
523 }
524
525 public override Quaternion Orientation
526 {
527 get
528 {
529 if (givefakeori > 0)
530 return fakeori;
531 else
532
533 return _orientation;
534 }
535 set
536 {
537 if (QuaternionIsFinite(value))
538 {
539 fakeori = value;
540 givefakeori++;
541 AddChange(changes.Orientation, value);
542 }
543 else
544 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
545
546 }
547 }
548
549 public override Vector3 Acceleration
550 {
551 get { return _acceleration; }
552 set { }
553 }
554
555 public override Vector3 RotationalVelocity
556 {
557 get
558 {
559 Vector3 pv = Vector3.Zero;
560 if (_zeroFlag)
561 return pv;
562 m_lastUpdateSent = false;
563
564 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
565 return pv;
566
567 return m_rotationalVelocity;
568 }
569 set
570 {
571 if (value.IsFinite())
572 {
573 m_rotationalVelocity = value;
574 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
575 d.BodyEnable(Body);
576 }
577 else
578 {
579 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
580 }
581 }
582 }
583
584
585 public override float Buoyancy
586 {
587 get { return m_buoyancy; }
588 set
589 {
590 m_buoyancy = value;
591 }
592 }
593
594 public override bool FloatOnWater
595 {
596 set
597 {
598 AddChange(changes.CollidesWater, value);
599 }
600 }
601
602 public override Vector3 PIDTarget
603 {
604 set
605 {
606 if (value.IsFinite())
607 {
608 m_PIDTarget = value;
609 }
610 else
611 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
612 }
613 }
614
615 public override bool PIDActive { set { m_usePID = value; } }
616 public override float PIDTau { set { m_PIDTau = value; } }
617
618 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
619 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
620 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
621 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
622
623 public override Quaternion APIDTarget { set { return; } }
624
625 public override bool APIDActive { set { return; } }
626
627 public override float APIDStrength { set { return; } }
628
629 public override float APIDDamping { set { return; } }
630
631 public override int VehicleType
632 {
633 // we may need to put a fake on this
634 get
635 {
636 if (m_vehicle == null)
637 return (int)Vehicle.TYPE_NONE;
638 else
639 return (int)m_vehicle.Type;
640 }
641 set
642 {
643 AddChange(changes.VehicleType, value);
644 }
645 }
646
647 public override void VehicleFloatParam(int param, float value)
648 {
649 strVehicleFloatParam fp = new strVehicleFloatParam();
650 fp.param = param;
651 fp.value = value;
652 AddChange(changes.VehicleFloatParam, fp);
653 }
654
655 public override void VehicleVectorParam(int param, Vector3 value)
656 {
657 strVehicleVectorParam fp = new strVehicleVectorParam();
658 fp.param = param;
659 fp.value = value;
660 AddChange(changes.VehicleVectorParam, fp);
661 }
662
663 public override void VehicleRotationParam(int param, Quaternion value)
664 {
665 strVehicleQuatParam fp = new strVehicleQuatParam();
666 fp.param = param;
667 fp.value = value;
668 AddChange(changes.VehicleVectorParam, fp);
669 }
670
671 public override void VehicleFlags(int param, bool value)
672 {
673 if (m_vehicle == null)
674 return;
675 strVehicleBoolParam bp = new strVehicleBoolParam();
676 bp.param = param;
677 bp.value = value;
678 AddChange(changes.VehicleFlags, bp);
679 }
680
681 public void SetAcceleration(Vector3 accel)
682 {
683 _acceleration = accel;
684 }
685
686 public override void AddForce(Vector3 force, bool pushforce)
687 {
688 if (force.IsFinite())
689 {
690 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE);
691 }
692 else
693 {
694 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
695 }
696 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
697 }
698
699 public override void AddAngularForce(Vector3 force, bool pushforce)
700 {
701 if (force.IsFinite())
702 {
703 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE);
704 }
705 else
706 {
707 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
708 }
709 }
710
711 public override void CrossingFailure()
712 {
713 m_crossingfailures++;
714 changeDisable(false);
715 }
716
717 public override void SetMomentum(Vector3 momentum)
718 {
719 }
720
721 public override void SetMaterial(int pMaterial)
722 {
723 m_material = pMaterial;
724 primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu;
725 primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
726 }
727
728 public void setPrimForRemoval()
729 {
730 AddChange(changes.Remove, null);
731 }
732
733 public override void link(PhysicsActor obj)
734 {
735 AddChange(changes.Link, obj);
736 }
737
738 public override void delink()
739 {
740 AddChange(changes.DeLink, null);
741 }
742
743 public override void LockAngularMotion(Vector3 axis)
744 {
745 // reverse the zero/non zero values for ODE.
746 if (axis.IsFinite())
747 {
748 axis.X = (axis.X > 0) ? 1f : 0f;
749 axis.Y = (axis.Y > 0) ? 1f : 0f;
750 axis.Z = (axis.Z > 0) ? 1f : 0f;
751 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
752 AddChange(changes.AngLock, axis);
753 }
754 else
755 {
756 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
757 }
758 }
759
760 public override void SubscribeEvents(int ms)
761 {
762 m_eventsubscription = ms;
763 _parent_scene.AddCollisionEventReporting(this);
764 }
765
766 public override void UnSubscribeEvents()
767 {
768 _parent_scene.RemoveCollisionEventReporting(this);
769 m_eventsubscription = 0;
770 }
771
772 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
773 {
774 if (CollisionEventsThisFrame == null)
775 CollisionEventsThisFrame = new CollisionEventUpdate();
776
777 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
778 }
779
780 public void SendCollisions()
781 {
782 if (CollisionEventsThisFrame == null)
783 return;
784
785 base.SendCollisionUpdate(CollisionEventsThisFrame);
786
787 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
788 CollisionEventsThisFrame = null;
789 else
790 CollisionEventsThisFrame = new CollisionEventUpdate();
791 }
792
793 public override bool SubscribedEvents()
794 {
795 if (m_eventsubscription > 0)
796 return true;
797 return false;
798 }
799
800
801 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
802 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
803 {
804 Name = primName;
805
806 m_vehicle = null;
807
808 if (!pos.IsFinite())
809 {
810 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
811 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
812 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
813 }
814 _position = pos;
815 givefakepos = 0;
816
817 PID_D = parent_scene.bodyPIDD;
818 PID_G = parent_scene.bodyPIDG;
819 m_density = parent_scene.geomDefaultDensity;
820 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
821 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
822
823 prim_geom = IntPtr.Zero;
824 Body = IntPtr.Zero;
825
826 if (!size.IsFinite())
827 {
828 size = new Vector3(0.5f, 0.5f, 0.5f);
829 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
830 }
831
832 if (size.X <= 0) size.X = 0.01f;
833 if (size.Y <= 0) size.Y = 0.01f;
834 if (size.Z <= 0) size.Z = 0.01f;
835
836 _size = size;
837
838
839 if (!QuaternionIsFinite(rotation))
840 {
841 rotation = Quaternion.Identity;
842 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
843 }
844
845 _orientation = rotation;
846 givefakeori = 0;
847
848 _pbs = pbs;
849
850 _parent_scene = parent_scene;
851 m_targetSpace = IntPtr.Zero;
852
853 if (pos.Z < 0)
854 {
855 m_isphysical = false;
856 }
857 else
858 {
859 m_isphysical = pisPhysical;
860 }
861 m_fakeisphysical = m_isphysical;
862
863 m_isVolumeDetect = false;
864
865 m_force = Vector3.Zero;
866
867 m_iscolliding = false;
868 m_wascolliding = false;
869 m_colliderfilter = 0;
870
871 hasOOBoffsetFromMesh = false;
872 _triMeshData = IntPtr.Zero;
873
874
875 primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
876 primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
877
878 CalcPrimBodyData();
879
880 m_building = true; // control must set this to false when done
881
882 AddChange(changes.Add, null);
883 }
884
885 private void resetCollisionAccounting()
886 {
887 m_collisionscore = 0;
888 m_interpenetrationcount = 0;
889 m_disabled = false;
890 }
891
892 private void createAMotor(Vector3 axis)
893 {
894 if (Body == IntPtr.Zero)
895 return;
896
897 if (Amotor != IntPtr.Zero)
898 {
899 d.JointDestroy(Amotor);
900 Amotor = IntPtr.Zero;
901 }
902
903 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
904
905 if (axisnum <= 0)
906 return;
907
908 // stop it
909 d.BodySetTorque(Body, 0, 0, 0);
910 d.BodySetAngularVel(Body, 0, 0, 0);
911
912 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
913 d.JointAttach(Amotor, Body, IntPtr.Zero);
914
915 d.JointSetAMotorMode(Amotor, 0);
916
917 d.JointSetAMotorNumAxes(Amotor, axisnum);
918
919 // get current orientation to lock
920
921 d.Quaternion dcur = d.BodyGetQuaternion(Body);
922 Quaternion curr; // crap convertion between identical things
923 curr.X = dcur.X;
924 curr.Y = dcur.Y;
925 curr.Z = dcur.Z;
926 curr.W = dcur.W;
927 Vector3 ax;
928
929 const int StopERP = 7;
930 const int StopCFM = 8;
931
932 int i = 0;
933 int j = 0;
934 if (axis.X == 0)
935 {
936 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
937 // ODE should do this with axis relative to body 1 but seems to fail
938 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
939 d.JointSetAMotorAngle(Amotor, 0, 0);
940 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
941 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
942 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
943 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
944 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
945 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
946 d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f);
947 d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f);
948 i++;
949 j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set
950 }
951
952 if (axis.Y == 0)
953 {
954 ax = (new Vector3(0, 1, 0)) * curr;
955 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
956 d.JointSetAMotorAngle(Amotor, i, 0);
957 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
958 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
959 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
960 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
961 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
962 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
963 d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
964 d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
965 i++;
966 j += 256;
967 }
968
969 if (axis.Z == 0)
970 {
971 ax = (new Vector3(0, 0, 1)) * curr;
972 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
973 d.JointSetAMotorAngle(Amotor, i, 0);
974 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
975 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
976 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
977 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
978 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
979 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
980 d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f);
981 d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f);
982 }
983 }
984
985 private bool setMesh(OdeScene parent_scene)
986 {
987 if (Body != IntPtr.Zero)
988 {
989 if (childPrim)
990 {
991 if (_parent != null)
992 {
993 OdePrim parent = (OdePrim)_parent;
994 parent.ChildDelink(this, false);
995 }
996 }
997 else
998 {
999 DestroyBody();
1000 }
1001 }
1002
1003 IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true);
1004 if (mesh == null)
1005 {
1006 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1007 return false;
1008 }
1009
1010 IntPtr vertices, indices;
1011 int vertexCount, indexCount;
1012 int vertexStride, triStride;
1013
1014 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1015 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1016
1017 if (vertexCount == 0 || indexCount == 0)
1018 {
1019 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z);
1020 _size.X = 0.01f;
1021 _size.Y = 0.01f;
1022 _size.Z = 0.01f;
1023 return false;
1024 }
1025
1026 primOOBoffset = mesh.GetCentroid();
1027 hasOOBoffsetFromMesh = true;
1028
1029 _triMeshData = d.GeomTriMeshDataCreate();
1030
1031 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1032 d.GeomTriMeshDataPreprocess(_triMeshData);
1033
1034 mesh.releaseSourceMeshData();
1035
1036 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1037 try
1038 {
1039 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
1040 }
1041
1042 catch (Exception e)
1043 {
1044 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1045 return false;
1046 }
1047 return true;
1048 }
1049
1050 private void SetGeom(IntPtr geom)
1051 {
1052 prim_geom = geom;
1053 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1054 if (prim_geom != IntPtr.Zero)
1055 {
1056 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1057 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1058
1059 CalcPrimBodyData();
1060
1061 _parent_scene.geom_name_map[prim_geom] = Name;
1062 _parent_scene.actor_name_map[prim_geom] = this;
1063
1064 /*
1065 if (childPrim)
1066 {
1067 if (_parent != null && _parent is OdePrim)
1068 {
1069 OdePrim parent = (OdePrim)_parent;
1070 //Console.WriteLine("SetGeom calls ChildSetGeom");
1071 parent.ChildSetGeom(this);
1072 }
1073 }
1074 */
1075 }
1076 else
1077 m_log.Warn("Setting bad Geom");
1078 }
1079
1080
1081 /// <summary>
1082 /// Create a geometry for the given mesh in the given target space.
1083 /// </summary>
1084 /// <param name="m_targetSpace"></param>
1085 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1086 private void CreateGeom()
1087 {
1088 if (_triMeshData != IntPtr.Zero)
1089 {
1090 d.GeomTriMeshDataDestroy(_triMeshData);
1091 _triMeshData = IntPtr.Zero;
1092 }
1093
1094 bool haveMesh = false;
1095 hasOOBoffsetFromMesh = false;
1096
1097 if (_parent_scene.needsMeshing(_pbs))
1098 {
1099 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1100 }
1101
1102 if (!haveMesh)
1103 {
1104 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1105 && _size.X == _size.Y && _size.Y == _size.Z)
1106 { // it's a sphere
1107 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1108 try
1109 {
1110 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1111 }
1112 catch (Exception e)
1113 {
1114 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1115 return;
1116 }
1117 }
1118 else
1119 {// do it as a box
1120 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1121 try
1122 {
1123 //Console.WriteLine(" CreateGeom 4");
1124 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1125 }
1126 catch (Exception e)
1127 {
1128 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1129 return;
1130 }
1131 }
1132 }
1133 }
1134
1135 /// <summary>
1136 /// Set a new geometry for this prim.
1137 /// </summary>
1138 /// <param name="geom"></param>
1139 private void RemoveGeom()
1140 {
1141 if (prim_geom != IntPtr.Zero)
1142 {
1143 _parent_scene.geom_name_map.Remove(prim_geom);
1144 _parent_scene.actor_name_map.Remove(prim_geom);
1145 try
1146 {
1147 d.GeomDestroy(prim_geom);
1148 if (_triMeshData != IntPtr.Zero)
1149 {
1150 d.GeomTriMeshDataDestroy(_triMeshData);
1151 _triMeshData = IntPtr.Zero;
1152 }
1153 }
1154 // catch (System.AccessViolationException)
1155 catch (Exception e)
1156 {
1157 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1158 }
1159
1160 prim_geom = IntPtr.Zero;
1161 }
1162 else
1163 {
1164 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1165 }
1166 Body = IntPtr.Zero;
1167 hasOOBoffsetFromMesh = false;
1168 CalcPrimBodyData();
1169 }
1170
1171 private void ChildSetGeom(OdePrim odePrim)
1172 {
1173 // well..
1174 DestroyBody();
1175 MakeBody();
1176 }
1177
1178 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1179 // should only be called for non physical prims unless they are becoming non physical
1180 private void SetInStaticSpace(OdePrim prim)
1181 {
1182 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1183 prim.m_targetSpace = targetSpace;
1184 d.GeomEnable(prim_geom);
1185 }
1186
1187 public void enableBodySoft()
1188 {
1189 if (!childPrim)
1190 {
1191 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
1192 {
1193 if (m_targetSpace != _parent_scene.ActiveSpace)
1194 {
1195 m_targetSpace = _parent_scene.ActiveSpace;
1196
1197 foreach (OdePrim prm in childrenPrim)
1198 {
1199 if (prm.prim_geom != IntPtr.Zero)
1200 {
1201 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1202 prm.m_targetSpace = m_targetSpace;
1203 }
1204 }
1205 d.SpaceAdd(m_targetSpace, prim_geom);
1206 }
1207 d.GeomEnable(prim_geom);
1208 foreach (OdePrim prm in childrenPrim)
1209 d.GeomEnable(prm.prim_geom);
1210
1211 d.BodyEnable(Body);
1212 }
1213 }
1214 resetCollisionAccounting(); // this sets m_disable to false
1215 }
1216
1217 private void disableBodySoft()
1218 {
1219 m_disabled = true;
1220 if (!childPrim)
1221 {
1222 if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
1223 {
1224 if (m_targetSpace == _parent_scene.ActiveSpace)
1225 {
1226 foreach (OdePrim prm in childrenPrim)
1227 {
1228 if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero)
1229 {
1230 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1231 prm.m_targetSpace = IntPtr.Zero;
1232 }
1233 }
1234 d.SpaceRemove(m_targetSpace, prim_geom);
1235 m_targetSpace = IntPtr.Zero;
1236 }
1237 d.GeomDisable(prim_geom);
1238 foreach (OdePrim prm in childrenPrim)
1239 d.GeomDisable(prm.prim_geom);
1240 d.BodyDisable(Body);
1241 }
1242 }
1243 }
1244
1245 private void MakeBody()
1246 {
1247 if (!m_isphysical) // only physical get bodies
1248 return;
1249
1250 if (childPrim) // child prims don't get bodies;
1251 return;
1252
1253 if (m_building)
1254 return;
1255
1256 if (prim_geom == IntPtr.Zero)
1257 {
1258 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1259 return;
1260 }
1261
1262 if (Body != IntPtr.Zero)
1263 {
1264 d.BodyDestroy(Body);
1265 Body = IntPtr.Zero;
1266 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1267 }
1268
1269
1270 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1271 {
1272 d.GeomSetBody(prim_geom, IntPtr.Zero);
1273 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1274 }
1275
1276 d.Matrix3 mymat = new d.Matrix3();
1277 d.Quaternion myrot = new d.Quaternion();
1278 d.Mass objdmass = new d.Mass { };
1279
1280 Body = d.BodyCreate(_parent_scene.world);
1281
1282 DMassDup(ref primdMass, out objdmass);
1283
1284 // rotate inertia
1285 myrot.X = _orientation.X;
1286 myrot.Y = _orientation.Y;
1287 myrot.Z = _orientation.Z;
1288 myrot.W = _orientation.W;
1289
1290 d.RfromQ(out mymat, ref myrot);
1291 d.MassRotate(ref objdmass, ref mymat);
1292
1293 // set the body rotation and position
1294 d.BodySetRotation(Body, ref mymat);
1295
1296 // recompute full object inertia if needed
1297 if (childrenPrim.Count > 0)
1298 {
1299 d.Matrix3 mat = new d.Matrix3();
1300 d.Quaternion quat = new d.Quaternion();
1301 d.Mass tmpdmass = new d.Mass { };
1302 Vector3 rcm;
1303
1304 rcm.X = _position.X + objdmass.c.X;
1305 rcm.Y = _position.Y + objdmass.c.Y;
1306 rcm.Z = _position.Z + objdmass.c.Z;
1307
1308 lock (childrenPrim)
1309 {
1310 foreach (OdePrim prm in childrenPrim)
1311 {
1312 if (prm.prim_geom == IntPtr.Zero)
1313 {
1314 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1315 continue;
1316 }
1317
1318
1319
1320 DMassCopy(ref prm.primdMass, ref tmpdmass);
1321
1322 // apply prim current rotation to inertia
1323 quat.X = prm._orientation.X;
1324 quat.Y = prm._orientation.Y;
1325 quat.Z = prm._orientation.Z;
1326 quat.W = prm._orientation.W;
1327 d.RfromQ(out mat, ref quat);
1328 d.MassRotate(ref tmpdmass, ref mat);
1329
1330 Vector3 ppos = prm._position;
1331 ppos.X += tmpdmass.c.X - rcm.X;
1332 ppos.Y += tmpdmass.c.Y - rcm.Y;
1333 ppos.Z += tmpdmass.c.Z - rcm.Z;
1334
1335 // refer inertia to root prim center of mass position
1336 d.MassTranslate(ref tmpdmass,
1337 ppos.X,
1338 ppos.Y,
1339 ppos.Z);
1340
1341 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1342 // fix prim colision cats
1343
1344 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1345 {
1346 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1347 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1348 }
1349
1350 d.GeomClearOffset(prm.prim_geom);
1351 d.GeomSetBody(prm.prim_geom, Body);
1352 prm.Body = Body;
1353 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1354 }
1355 }
1356 }
1357
1358 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1359 // associate root geom with body
1360 d.GeomSetBody(prim_geom, Body);
1361
1362 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1363 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1364
1365 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1366 myrot.W = -myrot.W;
1367 d.RfromQ(out mymat, ref myrot);
1368 d.MassRotate(ref objdmass, ref mymat);
1369 d.BodySetMass(Body, ref objdmass);
1370 _mass = objdmass.mass;
1371
1372 m_collisionCategories |= CollisionCategories.Body;
1373 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1374
1375 // disconnect from world gravity so we can apply buoyancy
1376 d.BodySetGravityMode(Body, false);
1377
1378 d.BodySetAutoDisableFlag(Body, true);
1379 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1380 // d.BodySetLinearDampingThreshold(Body, 0.01f);
1381 // d.BodySetAngularDampingThreshold(Body, 0.001f);
1382 d.BodySetDamping(Body, .001f, .0002f);
1383
1384 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1385 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1386
1387 m_interpenetrationcount = 0;
1388 m_collisionscore = 0;
1389
1390 m_disabled = false;
1391
1392 if (m_targetSpace != _parent_scene.ActiveSpace)
1393 {
1394 if (m_targetSpace != IntPtr.Zero)
1395 {
1396 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1397 if (d.SpaceQuery(m_targetSpace, prim_geom))
1398 d.SpaceRemove(m_targetSpace, prim_geom);
1399 }
1400
1401 m_targetSpace = _parent_scene.ActiveSpace;
1402 d.SpaceAdd(m_targetSpace, prim_geom);
1403 }
1404
1405 lock (childrenPrim)
1406 {
1407 foreach (OdePrim prm in childrenPrim)
1408 {
1409 if (prm.prim_geom == IntPtr.Zero)
1410 continue;
1411
1412 Vector3 ppos = prm._position;
1413 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1414
1415 prm.m_collisionCategories |= CollisionCategories.Body;
1416 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1417 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1418 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1419
1420 if (prm.m_targetSpace != _parent_scene.ActiveSpace)
1421 {
1422 if (prm.m_targetSpace != IntPtr.Zero)
1423 {
1424 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1425 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1426 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1427 }
1428 prm.m_targetSpace = _parent_scene.ActiveSpace;
1429 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1430 }
1431 d.GeomEnable(prm.prim_geom);
1432 prm.m_disabled = false;
1433 prm.m_interpenetrationcount = 0;
1434 prm.m_collisionscore = 0;
1435 _parent_scene.addActivePrim(prm);
1436 }
1437 }
1438
1439 // The body doesn't already have a finite rotation mode set here
1440 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1441 {
1442 createAMotor(m_angularlock);
1443 }
1444
1445 d.GeomEnable(prim_geom);
1446 m_disabled = false;
1447 _parent_scene.addActivePrim(this);
1448 }
1449
1450 private void DestroyBody()
1451 {
1452 if (Body != IntPtr.Zero)
1453 {
1454 _parent_scene.remActivePrim(this);
1455 m_collisionCategories &= ~CollisionCategories.Body;
1456 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1457 if (prim_geom != IntPtr.Zero)
1458 {
1459 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1460 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1461 UpdateDataFromGeom();
1462 d.GeomSetBody(prim_geom, IntPtr.Zero);
1463 SetInStaticSpace(this);
1464 }
1465
1466 if (!childPrim)
1467 {
1468 lock (childrenPrim)
1469 {
1470 foreach (OdePrim prm in childrenPrim)
1471 {
1472 _parent_scene.remActivePrim(prm);
1473 prm.m_collisionCategories &= ~CollisionCategories.Body;
1474 prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1475 if (prm.prim_geom != IntPtr.Zero)
1476 {
1477 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1478 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1479 prm.UpdateDataFromGeom();
1480 SetInStaticSpace(prm);
1481 }
1482 prm.Body = IntPtr.Zero;
1483 prm._mass = prm.primMass;
1484 prm.m_collisionscore = 0;
1485 }
1486 }
1487 d.BodyDestroy(Body);
1488 }
1489 Body = IntPtr.Zero;
1490 }
1491 _mass = primMass;
1492 m_disabled = true;
1493 m_collisionscore = 0;
1494 }
1495
1496 #region Mass Calculation
1497
1498 private float CalculatePrimVolume()
1499 {
1500 float volume = _size.X * _size.Y * _size.Z; // default
1501 float tmp;
1502
1503 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1504 float hollowVolume = hollowAmount * hollowAmount;
1505
1506 switch (_pbs.ProfileShape)
1507 {
1508 case ProfileShape.Square:
1509 // default box
1510
1511 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1512 {
1513 if (hollowAmount > 0.0)
1514 {
1515 switch (_pbs.HollowShape)
1516 {
1517 case HollowShape.Square:
1518 case HollowShape.Same:
1519 break;
1520
1521 case HollowShape.Circle:
1522
1523 hollowVolume *= 0.78539816339f;
1524 break;
1525
1526 case HollowShape.Triangle:
1527
1528 hollowVolume *= (0.5f * .5f);
1529 break;
1530
1531 default:
1532 hollowVolume = 0;
1533 break;
1534 }
1535 volume *= (1.0f - hollowVolume);
1536 }
1537 }
1538
1539 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1540 {
1541 //a tube
1542
1543 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1544 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1545 volume -= volume * tmp * tmp;
1546
1547 if (hollowAmount > 0.0)
1548 {
1549 hollowVolume *= hollowAmount;
1550
1551 switch (_pbs.HollowShape)
1552 {
1553 case HollowShape.Square:
1554 case HollowShape.Same:
1555 break;
1556
1557 case HollowShape.Circle:
1558 hollowVolume *= 0.78539816339f;
1559 break;
1560
1561 case HollowShape.Triangle:
1562 hollowVolume *= 0.5f * 0.5f;
1563 break;
1564 default:
1565 hollowVolume = 0;
1566 break;
1567 }
1568 volume *= (1.0f - hollowVolume);
1569 }
1570 }
1571
1572 break;
1573
1574 case ProfileShape.Circle:
1575
1576 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1577 {
1578 volume *= 0.78539816339f; // elipse base
1579
1580 if (hollowAmount > 0.0)
1581 {
1582 switch (_pbs.HollowShape)
1583 {
1584 case HollowShape.Same:
1585 case HollowShape.Circle:
1586 break;
1587
1588 case HollowShape.Square:
1589 hollowVolume *= 0.5f * 2.5984480504799f;
1590 break;
1591
1592 case HollowShape.Triangle:
1593 hollowVolume *= .5f * 1.27323954473516f;
1594 break;
1595
1596 default:
1597 hollowVolume = 0;
1598 break;
1599 }
1600 volume *= (1.0f - hollowVolume);
1601 }
1602 }
1603
1604 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1605 {
1606 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1607 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1608 volume *= (1.0f - tmp * tmp);
1609
1610 if (hollowAmount > 0.0)
1611 {
1612
1613 // calculate the hollow volume by it's shape compared to the prim shape
1614 hollowVolume *= hollowAmount;
1615
1616 switch (_pbs.HollowShape)
1617 {
1618 case HollowShape.Same:
1619 case HollowShape.Circle:
1620 break;
1621
1622 case HollowShape.Square:
1623 hollowVolume *= 0.5f * 2.5984480504799f;
1624 break;
1625
1626 case HollowShape.Triangle:
1627 hollowVolume *= .5f * 1.27323954473516f;
1628 break;
1629
1630 default:
1631 hollowVolume = 0;
1632 break;
1633 }
1634 volume *= (1.0f - hollowVolume);
1635 }
1636 }
1637 break;
1638
1639 case ProfileShape.HalfCircle:
1640 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1641 {
1642 volume *= 0.52359877559829887307710723054658f;
1643 }
1644 break;
1645
1646 case ProfileShape.EquilateralTriangle:
1647
1648 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1649 {
1650 volume *= 0.32475953f;
1651
1652 if (hollowAmount > 0.0)
1653 {
1654
1655 // calculate the hollow volume by it's shape compared to the prim shape
1656 switch (_pbs.HollowShape)
1657 {
1658 case HollowShape.Same:
1659 case HollowShape.Triangle:
1660 hollowVolume *= .25f;
1661 break;
1662
1663 case HollowShape.Square:
1664 hollowVolume *= 0.499849f * 3.07920140172638f;
1665 break;
1666
1667 case HollowShape.Circle:
1668 // Hollow shape is a perfect cyllinder in respect to the cube's scale
1669 // Cyllinder hollow volume calculation
1670
1671 hollowVolume *= 0.1963495f * 3.07920140172638f;
1672 break;
1673
1674 default:
1675 hollowVolume = 0;
1676 break;
1677 }
1678 volume *= (1.0f - hollowVolume);
1679 }
1680 }
1681 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1682 {
1683 volume *= 0.32475953f;
1684 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1685 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1686 volume *= (1.0f - tmp * tmp);
1687
1688 if (hollowAmount > 0.0)
1689 {
1690
1691 hollowVolume *= hollowAmount;
1692
1693 switch (_pbs.HollowShape)
1694 {
1695 case HollowShape.Same:
1696 case HollowShape.Triangle:
1697 hollowVolume *= .25f;
1698 break;
1699
1700 case HollowShape.Square:
1701 hollowVolume *= 0.499849f * 3.07920140172638f;
1702 break;
1703
1704 case HollowShape.Circle:
1705
1706 hollowVolume *= 0.1963495f * 3.07920140172638f;
1707 break;
1708
1709 default:
1710 hollowVolume = 0;
1711 break;
1712 }
1713 volume *= (1.0f - hollowVolume);
1714 }
1715 }
1716 break;
1717
1718 default:
1719 break;
1720 }
1721
1722 float taperX1;
1723 float taperY1;
1724 float taperX;
1725 float taperY;
1726 float pathBegin;
1727 float pathEnd;
1728 float profileBegin;
1729 float profileEnd;
1730
1731 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1732 {
1733 taperX1 = _pbs.PathScaleX * 0.01f;
1734 if (taperX1 > 1.0f)
1735 taperX1 = 2.0f - taperX1;
1736 taperX = 1.0f - taperX1;
1737
1738 taperY1 = _pbs.PathScaleY * 0.01f;
1739 if (taperY1 > 1.0f)
1740 taperY1 = 2.0f - taperY1;
1741 taperY = 1.0f - taperY1;
1742 }
1743 else
1744 {
1745 taperX = _pbs.PathTaperX * 0.01f;
1746 if (taperX < 0.0f)
1747 taperX = -taperX;
1748 taperX1 = 1.0f - taperX;
1749
1750 taperY = _pbs.PathTaperY * 0.01f;
1751 if (taperY < 0.0f)
1752 taperY = -taperY;
1753 taperY1 = 1.0f - taperY;
1754 }
1755
1756 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
1757
1758 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
1759 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1760 volume *= (pathEnd - pathBegin);
1761
1762 // this is crude aproximation
1763 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1764 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1765 volume *= (profileEnd - profileBegin);
1766
1767 return volume;
1768 }
1769
1770
1771 private void CalcPrimBodyData()
1772 {
1773 float volume;
1774
1775 if (prim_geom == IntPtr.Zero)
1776 {
1777 // Ubit let's have a initial basic OOB
1778 primOOBsize.X = _size.X;
1779 primOOBsize.Y = _size.Y;
1780 primOOBsize.Z = _size.Z;
1781 primOOBoffset = Vector3.Zero;
1782 }
1783 else
1784 {
1785 d.AABB AABB;
1786 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
1787
1788 primOOBsize.X = (AABB.MaxX - AABB.MinX);
1789 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
1790 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
1791 if (!hasOOBoffsetFromMesh)
1792 {
1793 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
1794 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
1795 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
1796 }
1797 }
1798
1799 // also its own inertia and mass
1800 // keep using basic shape mass for now
1801 volume = CalculatePrimVolume();
1802
1803 primMass = m_density * volume;
1804
1805 if (primMass <= 0)
1806 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1807 if (primMass > _parent_scene.maximumMassObject)
1808 primMass = _parent_scene.maximumMassObject;
1809
1810 _mass = primMass; // just in case
1811
1812 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
1813
1814 d.MassTranslate(ref primdMass,
1815 primOOBoffset.X,
1816 primOOBoffset.Y,
1817 primOOBoffset.Z);
1818
1819 primOOBsize *= 0.5f; // let obb size be a corner coords
1820 primOOBradiusSQ = primOOBsize.LengthSquared();
1821 }
1822
1823
1824 #endregion
1825
1826
1827 /// <summary>
1828 /// Add a child prim to this parent prim.
1829 /// </summary>
1830 /// <param name="prim">Child prim</param>
1831 // I'm the parent
1832 // prim is the child
1833 public void ParentPrim(OdePrim prim)
1834 {
1835 //Console.WriteLine("ParentPrim " + m_primName);
1836 if (this.m_localID != prim.m_localID)
1837 {
1838 DestroyBody(); // for now we need to rebuil entire object on link change
1839
1840 lock (childrenPrim)
1841 {
1842 // adopt the prim
1843 if (!childrenPrim.Contains(prim))
1844 childrenPrim.Add(prim);
1845
1846 // see if this prim has kids and adopt them also
1847 // should not happen for now
1848 foreach (OdePrim prm in prim.childrenPrim)
1849 {
1850 if (!childrenPrim.Contains(prm))
1851 {
1852 if (prm.Body != IntPtr.Zero)
1853 {
1854 if (prm.prim_geom != IntPtr.Zero)
1855 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1856 if (prm.Body != prim.Body)
1857 prm.DestroyBody(); // don't loose bodies around
1858 prm.Body = IntPtr.Zero;
1859 }
1860
1861 childrenPrim.Add(prm);
1862 prm._parent = this;
1863 }
1864 }
1865 }
1866 //Remove old children from the prim
1867 prim.childrenPrim.Clear();
1868
1869 if (prim.Body != IntPtr.Zero)
1870 {
1871 if (prim.prim_geom != IntPtr.Zero)
1872 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
1873 prim.DestroyBody(); // don't loose bodies around
1874 prim.Body = IntPtr.Zero;
1875 }
1876
1877 prim.childPrim = true;
1878 prim._parent = this;
1879
1880 MakeBody(); // full nasty reconstruction
1881 }
1882 }
1883
1884 private void UpdateChildsfromgeom()
1885 {
1886 if (childrenPrim.Count > 0)
1887 {
1888 foreach (OdePrim prm in childrenPrim)
1889 prm.UpdateDataFromGeom();
1890 }
1891 }
1892
1893 private void UpdateDataFromGeom()
1894 {
1895 if (prim_geom != IntPtr.Zero)
1896 {
1897 d.Vector3 lpos;
1898 d.GeomCopyPosition(prim_geom, out lpos);
1899 _position.X = lpos.X;
1900 _position.Y = lpos.Y;
1901 _position.Z = lpos.Z;
1902 d.Quaternion qtmp = new d.Quaternion { };
1903 d.GeomCopyQuaternion(prim_geom, out qtmp);
1904 _orientation.W = qtmp.W;
1905 _orientation.X = qtmp.X;
1906 _orientation.Y = qtmp.Y;
1907 _orientation.Z = qtmp.Z;
1908 }
1909 }
1910
1911 private void ChildDelink(OdePrim odePrim, bool remakebodies)
1912 {
1913 // Okay, we have a delinked child.. destroy all body and remake
1914 if (odePrim != this && !childrenPrim.Contains(odePrim))
1915 return;
1916
1917 DestroyBody();
1918
1919 if (odePrim == this) // delinking the root prim
1920 {
1921 OdePrim newroot = null;
1922 lock (childrenPrim)
1923 {
1924 if (childrenPrim.Count > 0)
1925 {
1926 newroot = childrenPrim[0];
1927 childrenPrim.RemoveAt(0);
1928 foreach (OdePrim prm in childrenPrim)
1929 {
1930 newroot.childrenPrim.Add(prm);
1931 }
1932 childrenPrim.Clear();
1933 }
1934 if (newroot != null)
1935 {
1936 newroot.childPrim = false;
1937 newroot._parent = null;
1938 if (remakebodies)
1939 newroot.MakeBody();
1940 }
1941 }
1942 }
1943
1944 else
1945 {
1946 lock (childrenPrim)
1947 {
1948 childrenPrim.Remove(odePrim);
1949 odePrim.childPrim = false;
1950 odePrim._parent = null;
1951 // odePrim.UpdateDataFromGeom();
1952 if (remakebodies)
1953 odePrim.MakeBody();
1954 }
1955 }
1956 if (remakebodies)
1957 MakeBody();
1958 }
1959
1960 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
1961 {
1962 // Okay, we have a delinked child.. destroy all body and remake
1963 if (odePrim != this && !childrenPrim.Contains(odePrim))
1964 return;
1965
1966 DestroyBody();
1967
1968 if (odePrim == this)
1969 {
1970 OdePrim newroot = null;
1971 lock (childrenPrim)
1972 {
1973 if (childrenPrim.Count > 0)
1974 {
1975 newroot = childrenPrim[0];
1976 childrenPrim.RemoveAt(0);
1977 foreach (OdePrim prm in childrenPrim)
1978 {
1979 newroot.childrenPrim.Add(prm);
1980 }
1981 childrenPrim.Clear();
1982 }
1983 if (newroot != null)
1984 {
1985 newroot.childPrim = false;
1986 newroot._parent = null;
1987 newroot.MakeBody();
1988 }
1989 }
1990 if (reMakeBody)
1991 MakeBody();
1992 return;
1993 }
1994 else
1995 {
1996 lock (childrenPrim)
1997 {
1998 childrenPrim.Remove(odePrim);
1999 odePrim.childPrim = false;
2000 odePrim._parent = null;
2001 if (reMakeBody)
2002 odePrim.MakeBody();
2003 }
2004 }
2005 MakeBody();
2006 }
2007
2008 #region changes
2009
2010 private void changeadd()
2011 {
2012 CreateGeom();
2013
2014 if (prim_geom != IntPtr.Zero)
2015 {
2016 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2017 d.Quaternion myrot = new d.Quaternion();
2018 myrot.X = _orientation.X;
2019 myrot.Y = _orientation.Y;
2020 myrot.Z = _orientation.Z;
2021 myrot.W = _orientation.W;
2022 d.GeomSetQuaternion(prim_geom, ref myrot);
2023 // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
2024 if (!m_isphysical)
2025 SetInStaticSpace(this);
2026 }
2027
2028 if (m_isphysical && Body == IntPtr.Zero)
2029 {
2030 /*
2031 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2032 {
2033 changeShape(_pbs);
2034 }
2035 else
2036 {
2037 */
2038 MakeBody();
2039 // }
2040 }
2041 }
2042
2043 private void changeAngularLock(Vector3 newLock)
2044 {
2045 // do we have a Physical object?
2046 if (Body != IntPtr.Zero)
2047 {
2048 //Check that we have a Parent
2049 //If we have a parent then we're not authorative here
2050 if (_parent == null)
2051 {
2052 if (!newLock.ApproxEquals(Vector3.One, 0f))
2053 {
2054 createAMotor(newLock);
2055 }
2056 else
2057 {
2058 if (Amotor != IntPtr.Zero)
2059 {
2060 d.JointDestroy(Amotor);
2061 Amotor = IntPtr.Zero;
2062 }
2063 }
2064 }
2065 }
2066 // Store this for later in case we get turned into a separate body
2067 m_angularlock = newLock;
2068 }
2069
2070 private void changeLink(OdePrim NewParent)
2071 {
2072 if (_parent == null && NewParent != null)
2073 {
2074 NewParent.ParentPrim(this);
2075 }
2076 else if (_parent != null)
2077 {
2078 if (_parent is OdePrim)
2079 {
2080 if (NewParent != _parent)
2081 {
2082 (_parent as OdePrim).ChildDelink(this, false); // for now...
2083 childPrim = false;
2084
2085 if (NewParent != null)
2086 {
2087 NewParent.ParentPrim(this);
2088 }
2089 }
2090 }
2091 }
2092 _parent = NewParent;
2093 }
2094
2095
2096 private void Stop()
2097 {
2098 if (!childPrim)
2099 {
2100 m_force = Vector3.Zero;
2101 m_forceacc = Vector3.Zero;
2102 m_angularForceacc = Vector3.Zero;
2103 _torque = Vector3.Zero;
2104 _velocity = Vector3.Zero;
2105 _acceleration = Vector3.Zero;
2106 m_rotationalVelocity = Vector3.Zero;
2107 _target_velocity = Vector3.Zero;
2108 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2109 m_vehicle.Stop();
2110 }
2111
2112 if (Body != IntPtr.Zero)
2113 {
2114 d.BodySetForce(Body, 0f, 0f, 0f);
2115 d.BodySetTorque(Body, 0f, 0f, 0f);
2116 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2117 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2118
2119 }
2120 }
2121
2122 private void changeSelectedStatus(bool newval)
2123 {
2124 m_isSelected = newval;
2125 Stop();
2126
2127 if (newval)
2128 {
2129 m_collisionCategories = CollisionCategories.Selected;
2130 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2131
2132 if (prim_geom != IntPtr.Zero)
2133 {
2134 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2135 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2136 }
2137
2138 disableBodySoft();
2139 }
2140 else
2141 {
2142 m_collisionCategories = CollisionCategories.Geom;
2143
2144 if (m_isphysical)
2145 m_collisionCategories |= CollisionCategories.Body;
2146
2147 m_collisionFlags = m_default_collisionFlags;
2148
2149 if (m_collidesLand)
2150 m_collisionFlags |= CollisionCategories.Land;
2151 if (m_collidesWater)
2152 m_collisionFlags |= CollisionCategories.Water;
2153
2154 if (prim_geom != IntPtr.Zero)
2155 {
2156 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2157 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2158 }
2159
2160 enableBodySoft();
2161 }
2162
2163 resetCollisionAccounting();
2164 }
2165
2166 private void changePosition(Vector3 newPos)
2167 {
2168 if (m_isphysical)
2169 {
2170 if (childPrim) // inertia is messed, must rebuild
2171 {
2172 if (m_building)
2173 {
2174 _position = newPos;
2175 }
2176 }
2177 else
2178 {
2179 if (_position != newPos)
2180 {
2181 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2182 _position = newPos;
2183 }
2184 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2185 d.BodyEnable(Body);
2186 }
2187 }
2188 else
2189 {
2190 if (prim_geom != IntPtr.Zero)
2191 {
2192 if (newPos != _position)
2193 {
2194 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2195 _position = newPos;
2196
2197 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2198 }
2199 }
2200 }
2201 givefakepos--;
2202 if (givefakepos < 0)
2203 givefakepos = 0;
2204 // changeSelectedStatus();
2205 resetCollisionAccounting();
2206 }
2207
2208 private void changeOrientation(Quaternion newOri)
2209 {
2210 if (m_isphysical)
2211 {
2212 if (childPrim) // inertia is messed, must rebuild
2213 {
2214 if (m_building)
2215 {
2216 _orientation = newOri;
2217 }
2218 }
2219 else
2220 {
2221 if (newOri != _orientation)
2222 {
2223 d.Quaternion myrot = new d.Quaternion();
2224 myrot.X = newOri.X;
2225 myrot.Y = newOri.Y;
2226 myrot.Z = newOri.Z;
2227 myrot.W = newOri.W;
2228 d.GeomSetQuaternion(prim_geom, ref myrot);
2229 _orientation = newOri;
2230 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2231 createAMotor(m_angularlock);
2232 }
2233 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2234 d.BodyEnable(Body);
2235 }
2236 }
2237 else
2238 {
2239 if (prim_geom != IntPtr.Zero)
2240 {
2241 if (newOri != _orientation)
2242 {
2243 d.Quaternion myrot = new d.Quaternion();
2244 myrot.X = newOri.X;
2245 myrot.Y = newOri.Y;
2246 myrot.Z = newOri.Z;
2247 myrot.W = newOri.W;
2248 d.GeomSetQuaternion(prim_geom, ref myrot);
2249 _orientation = newOri;
2250 }
2251 }
2252 }
2253 givefakeori--;
2254 if (givefakeori < 0)
2255 givefakeori = 0;
2256 resetCollisionAccounting();
2257 }
2258
2259 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2260 {
2261 if (m_isphysical)
2262 {
2263 if (childPrim && m_building) // inertia is messed, must rebuild
2264 {
2265 _position = newPos;
2266 _orientation = newOri;
2267 }
2268 else
2269 {
2270 if (newOri != _orientation)
2271 {
2272 d.Quaternion myrot = new d.Quaternion();
2273 myrot.X = newOri.X;
2274 myrot.Y = newOri.Y;
2275 myrot.Z = newOri.Z;
2276 myrot.W = newOri.W;
2277 d.GeomSetQuaternion(prim_geom, ref myrot);
2278 _orientation = newOri;
2279 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2280 createAMotor(m_angularlock);
2281 }
2282 if (_position != newPos)
2283 {
2284 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2285 _position = newPos;
2286 }
2287 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2288 d.BodyEnable(Body);
2289 }
2290 }
2291 else
2292 {
2293 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2294 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2295
2296 if (prim_geom != IntPtr.Zero)
2297 {
2298 if (newOri != _orientation)
2299 {
2300 d.Quaternion myrot = new d.Quaternion();
2301 myrot.X = newOri.X;
2302 myrot.Y = newOri.Y;
2303 myrot.Z = newOri.Z;
2304 myrot.W = newOri.W;
2305 d.GeomSetQuaternion(prim_geom, ref myrot);
2306 _orientation = newOri;
2307 }
2308
2309 if (newPos != _position)
2310 {
2311 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2312 _position = newPos;
2313
2314 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2315 }
2316 }
2317 }
2318 givefakepos--;
2319 if (givefakepos < 0)
2320 givefakepos = 0;
2321 givefakeori--;
2322 if (givefakeori < 0)
2323 givefakeori = 0;
2324
2325 resetCollisionAccounting();
2326 }
2327
2328
2329 private void changeDisable(bool disable)
2330 {
2331 if (disable)
2332 {
2333 if (!m_disabled)
2334 disableBodySoft();
2335 }
2336 else
2337 {
2338 if (m_disabled)
2339 enableBodySoft();
2340 }
2341 }
2342
2343 private void changePhysicsStatus(bool NewStatus)
2344 {
2345 m_isphysical = NewStatus;
2346
2347 if (!childPrim)
2348 {
2349 if (NewStatus)
2350 {
2351 if (Body == IntPtr.Zero)
2352 {
2353 /*
2354 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2355 {
2356 changeShape(_pbs);
2357 }
2358 else
2359 */
2360 {
2361 MakeBody();
2362 }
2363 }
2364 }
2365 else
2366 {
2367 if (Body != IntPtr.Zero)
2368 {
2369 // UpdateChildsfromgeom();
2370 /* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2371 {
2372 changeShape(_pbs);
2373 }
2374 else
2375 */
2376 DestroyBody();
2377 Stop();
2378 }
2379 }
2380 }
2381
2382 resetCollisionAccounting();
2383 }
2384
2385 private void changeprimsizeshape()
2386 {
2387 OdePrim parent = (OdePrim)_parent;
2388
2389 bool chp = childPrim;
2390
2391 if (chp)
2392 {
2393 if (parent != null)
2394 {
2395 parent.DestroyBody();
2396 }
2397 }
2398 else
2399 {
2400 DestroyBody();
2401 }
2402
2403 RemoveGeom();
2404
2405 // we don't need to do space calculation because the client sends a position update also.
2406 if (_size.X <= 0)
2407 _size.X = 0.01f;
2408 if (_size.Y <= 0)
2409 _size.Y = 0.01f;
2410 if (_size.Z <= 0)
2411 _size.Z = 0.01f;
2412 // Construction of new prim
2413
2414 CreateGeom();
2415
2416 if (prim_geom != IntPtr.Zero)
2417 {
2418 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2419 d.Quaternion myrot = new d.Quaternion();
2420 myrot.X = _orientation.X;
2421 myrot.Y = _orientation.Y;
2422 myrot.Z = _orientation.Z;
2423 myrot.W = _orientation.W;
2424 d.GeomSetQuaternion(prim_geom, ref myrot);
2425 }
2426
2427 if (chp)
2428 {
2429 if (parent != null)
2430 {
2431 parent.MakeBody();
2432 }
2433 }
2434 else
2435 MakeBody();
2436
2437 resetCollisionAccounting();
2438 }
2439
2440 private void changeSize(Vector3 newSize)
2441 {
2442 _size = newSize;
2443 changeprimsizeshape();
2444 }
2445
2446 private void changeShape(PrimitiveBaseShape newShape)
2447 {
2448 _pbs = newShape;
2449 changeprimsizeshape();
2450 }
2451
2452 private void changeFloatOnWater(bool newval)
2453 {
2454 m_collidesWater = newval;
2455
2456 if (prim_geom != IntPtr.Zero)
2457 {
2458 if (m_collidesWater)
2459 {
2460 m_collisionFlags |= CollisionCategories.Water;
2461 }
2462 else
2463 {
2464 m_collisionFlags &= ~CollisionCategories.Water;
2465 }
2466 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2467 }
2468 }
2469
2470 private void changeSetTorque(Vector3 newtorque)
2471 {
2472 if (!m_isSelected)
2473 {
2474 if (m_isphysical && Body != IntPtr.Zero)
2475 {
2476 if (m_disabled)
2477 enableBodySoft();
2478 else if (!d.BodyIsEnabled(Body))
2479 d.BodyEnable(Body);
2480
2481 }
2482 _torque = newtorque;
2483 }
2484 }
2485
2486 private void changeForce(Vector3 force)
2487 {
2488 m_force = force;
2489 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2490 d.BodyEnable(Body);
2491 }
2492
2493 private void changeAddForce(Vector3 force)
2494 {
2495 m_forceacc += force;
2496 if (!m_isSelected)
2497 {
2498 lock (this)
2499 {
2500 //m_log.Info("[PHYSICS]: dequeing forcelist");
2501 if (m_isphysical && Body != IntPtr.Zero)
2502 {
2503 if (m_disabled)
2504 enableBodySoft();
2505 else if (!d.BodyIsEnabled(Body))
2506 d.BodyEnable(Body);
2507 }
2508 }
2509
2510 m_collisionscore = 0;
2511 m_interpenetrationcount = 0;
2512 }
2513 }
2514
2515 private void changeAddAngularForce(Vector3 aforce)
2516 {
2517 m_angularForceacc += aforce;
2518 if (!m_isSelected)
2519 {
2520 lock (this)
2521 {
2522 if (m_isphysical && Body != IntPtr.Zero)
2523 {
2524 if (m_disabled)
2525 enableBodySoft();
2526 else if (!d.BodyIsEnabled(Body))
2527 d.BodyEnable(Body);
2528 }
2529 }
2530 m_collisionscore = 0;
2531 m_interpenetrationcount = 0;
2532 }
2533 }
2534
2535 private void changevelocity(Vector3 newVel)
2536 {
2537 if (!m_isSelected)
2538 {
2539 if (Body != IntPtr.Zero)
2540 {
2541 if (m_disabled)
2542 enableBodySoft();
2543 else if (!d.BodyIsEnabled(Body))
2544 d.BodyEnable(Body);
2545
2546 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
2547 }
2548 //resetCollisionAccounting();
2549 }
2550 _velocity = newVel;
2551 }
2552
2553 private void changeVolumedetetion(bool newVolDtc)
2554 {
2555 m_isVolumeDetect = newVolDtc;
2556 }
2557
2558 protected void changeBuilding(bool newbuilding)
2559 {
2560 if ((bool)newbuilding)
2561 {
2562 m_building = true;
2563 DestroyBody();
2564 }
2565 else
2566 {
2567 m_building = false;
2568 if (!childPrim)
2569 MakeBody();
2570 }
2571 if (!childPrim && childrenPrim.Count > 0)
2572 {
2573 foreach (OdePrim prm in childrenPrim)
2574 prm.changeBuilding(m_building); // call directly
2575 }
2576 }
2577
2578 private void changeVehicleType(int value)
2579 {
2580 if (m_vehicle == null)
2581 {
2582 if (value != (int)Vehicle.TYPE_NONE)
2583 {
2584 m_vehicle = new ODEDynamics(this);
2585 m_vehicle.ProcessTypeChange((Vehicle)value);
2586 }
2587 }
2588 else
2589 m_vehicle.ProcessTypeChange((Vehicle)value);
2590 }
2591
2592 private void changeVehicleFloatParam(strVehicleFloatParam fp)
2593 {
2594 if (m_vehicle == null)
2595 return;
2596
2597 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
2598 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2599 d.BodyEnable(Body);
2600 }
2601
2602 private void changeVehicleVectorParam(strVehicleVectorParam vp)
2603 {
2604 if (m_vehicle == null)
2605 return;
2606 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
2607 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2608 d.BodyEnable(Body);
2609 }
2610
2611 private void changeVehicleRotationParam(strVehicleQuatParam qp)
2612 {
2613 if (m_vehicle == null)
2614 return;
2615 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
2616 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2617 d.BodyEnable(Body);
2618 }
2619
2620 private void changeVehicleFlags(strVehicleBoolParam bp)
2621 {
2622 if (m_vehicle == null)
2623 return;
2624 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
2625 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2626 d.BodyEnable(Body);
2627 }
2628
2629 #endregion
2630
2631 public void Move()
2632 {
2633 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
2634 !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims.
2635 {
2636 // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
2637
2638 float timestep = _parent_scene.ODE_STEPSIZE;
2639
2640 float fx = 0;
2641 float fy = 0;
2642 float fz = 0;
2643
2644 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2645 {
2646 // 'VEHICLES' are dealt with in ODEDynamics.cs
2647 m_vehicle.Step();
2648 }
2649 else
2650 {
2651 float m_mass = _mass;
2652
2653 // fz = 0f;
2654 //m_log.Info(m_collisionFlags.ToString());
2655 if (m_usePID)
2656 {
2657
2658 // If the PID Controller isn't active then we set our force
2659 // calculating base velocity to the current position
2660
2661 if ((m_PIDTau < 1) && (m_PIDTau != 0))
2662 {
2663 //PID_G = PID_G / m_PIDTau;
2664 m_PIDTau = 1;
2665 }
2666
2667 if ((PID_G - m_PIDTau) <= 0)
2668 {
2669 PID_G = m_PIDTau + 1;
2670 }
2671
2672 d.Vector3 vel = d.BodyGetLinearVel(Body);
2673 d.Vector3 pos = d.BodyGetPosition(Body);
2674 _target_velocity =
2675 new Vector3(
2676 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
2677 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
2678 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
2679 );
2680
2681 // if velocity is zero, use position control; otherwise, velocity control
2682
2683 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
2684 {
2685 // keep track of where we stopped. No more slippin' & slidin'
2686
2687 // We only want to deactivate the PID Controller if we think we want to have our surrogate
2688 // react to the physics scene by moving it's position.
2689 // Avatar to Avatar collisions
2690 // Prim to avatar collisions
2691
2692 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
2693 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
2694 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
2695 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
2696 d.BodySetLinearVel(Body, 0, 0, 0);
2697 d.BodyAddForce(Body, 0, 0, fz);
2698 return;
2699 }
2700 else
2701 {
2702 _zeroFlag = false;
2703
2704 // We're flying and colliding with something
2705 fx = ((_target_velocity.X) - vel.X) * (PID_D);
2706 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
2707
2708 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
2709
2710 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
2711 }
2712 } // end if (m_usePID)
2713
2714 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
2715 else if (m_useHoverPID)
2716 {
2717 //Console.WriteLine("Hover " + Name);
2718
2719 // If we're using the PID controller, then we have no gravity
2720
2721 // no lock; for now it's only called from within Simulate()
2722
2723 // If the PID Controller isn't active then we set our force
2724 // calculating base velocity to the current position
2725
2726 if ((m_PIDTau < 1))
2727 {
2728 PID_G = PID_G / m_PIDTau;
2729 }
2730
2731 if ((PID_G - m_PIDTau) <= 0)
2732 {
2733 PID_G = m_PIDTau + 1;
2734 }
2735
2736 // Where are we, and where are we headed?
2737 d.Vector3 pos = d.BodyGetPosition(Body);
2738 d.Vector3 vel = d.BodyGetLinearVel(Body);
2739
2740 // Non-Vehicles have a limited set of Hover options.
2741 // determine what our target height really is based on HoverType
2742 switch (m_PIDHoverType)
2743 {
2744 case PIDHoverType.Ground:
2745 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
2746 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
2747 break;
2748 case PIDHoverType.GroundAndWater:
2749 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
2750 m_waterHeight = _parent_scene.GetWaterLevel();
2751 if (m_groundHeight > m_waterHeight)
2752 {
2753 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
2754 }
2755 else
2756 {
2757 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
2758 }
2759 break;
2760
2761 } // end switch (m_PIDHoverType)
2762
2763
2764 _target_velocity =
2765 new Vector3(0.0f, 0.0f,
2766 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
2767 );
2768
2769 // if velocity is zero, use position control; otherwise, velocity control
2770
2771 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
2772 {
2773 // keep track of where we stopped. No more slippin' & slidin'
2774
2775 // We only want to deactivate the PID Controller if we think we want to have our surrogate
2776 // react to the physics scene by moving it's position.
2777 // Avatar to Avatar collisions
2778 // Prim to avatar collisions
2779
2780 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
2781 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
2782 // ? d.BodyAddForce(Body, 0, 0, fz);
2783 return;
2784 }
2785 else
2786 {
2787 _zeroFlag = false;
2788
2789 // We're flying and colliding with something
2790 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
2791 }
2792 }
2793 else
2794 {
2795 float b = (1.0f - m_buoyancy);
2796 fx = _parent_scene.gravityx * b;
2797 fy = _parent_scene.gravityy * b;
2798 fz = _parent_scene.gravityz * b;
2799 }
2800
2801 fx *= m_mass;
2802 fy *= m_mass;
2803 fz *= m_mass;
2804
2805 // constant force
2806 fx += m_force.X;
2807 fy += m_force.Y;
2808 fz += m_force.Z;
2809
2810 fx += m_forceacc.X;
2811 fy += m_forceacc.Y;
2812 fz += m_forceacc.Z;
2813
2814 m_forceacc = Vector3.Zero;
2815
2816 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
2817 if (fx != 0 || fy != 0 || fz != 0)
2818 {
2819 d.BodyAddForce(Body, fx, fy, fz);
2820 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
2821 }
2822
2823 Vector3 trq;
2824
2825 trq = _torque;
2826 trq += m_angularForceacc;
2827 m_angularForceacc = Vector3.Zero;
2828 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
2829 {
2830 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
2831 }
2832
2833 }
2834 }
2835 else
2836 { // is not physical, or is not a body or is selected
2837 // _zeroPosition = d.BodyGetPosition(Body);
2838 return;
2839 //Console.WriteLine("Nothing " + Name);
2840
2841 }
2842 }
2843
2844
2845 public void UpdatePositionAndVelocity(float simulatedtime)
2846 {
2847 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2848 if (_parent == null && !m_disabled && !m_building)
2849 {
2850 if (Body != IntPtr.Zero)
2851 {
2852 if (m_crossingfailures != 0 && m_crossingfailures < 5)
2853 {
2854 _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f);
2855 _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f);
2856 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
2857
2858 float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y);
2859 if (_position.Z < tmp)
2860 _position.Z = tmp + 0.2f;
2861
2862 m_lastposition = _position;
2863 m_lastorientation = _orientation;
2864 _velocity.X = 0;
2865 _velocity.Y = 0;
2866 _velocity.Z = 0;
2867
2868 m_lastVelocity = _velocity;
2869 m_rotationalVelocity = _velocity;
2870 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2871 m_vehicle.Stop();
2872
2873 m_crossingfailures = 0; // do this only once
2874 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
2875 d.BodySetAngularVel(Body, 0, 0, 0);
2876 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2877 enableBodySoft();
2878 base.RequestPhysicsterseUpdate();
2879 return;
2880 }
2881
2882 else if (m_crossingfailures != 0)
2883 {
2884 return;
2885 }
2886
2887 Vector3 pv = Vector3.Zero;
2888 bool lastZeroFlag = _zeroFlag;
2889
2890 d.Vector3 lpos;
2891 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
2892
2893 // we need to use root position since that's all the rest of scene uses
2894 if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X
2895 || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y
2896 )
2897 {
2898 // we are outside current region
2899 // we can't let it keeping moving and having colisions
2900 // since it can be stucked between something like terrain and edge
2901 // so lets stop and disable it until something else kicks it
2902 if (m_crossingfailures == 0)
2903 {
2904
2905 _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f);
2906 _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f);
2907 _position.Z = Util.Clip(lpos.Z, -100f, 50000f);
2908
2909 m_lastposition = _position;
2910 m_lastorientation = _orientation;
2911
2912 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
2913 d.BodySetAngularVel(Body, 0, 0, 0);
2914 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2915 disableBodySoft(); // stop collisions
2916 m_crossingfailures++; // do this only once
2917 base.RequestPhysicsterseUpdate();
2918 return;
2919 }
2920 }
2921
2922 if (lpos.Z < -100 || lpos.Z > 100000f)
2923 {
2924 lpos.Z = Util.Clip(lpos.Z, -100f, 50000f);
2925
2926 _acceleration.X = 0;
2927 _acceleration.Y = 0;
2928 _acceleration.Z = 0;
2929
2930 _velocity.X = 0;
2931 _velocity.Y = 0;
2932 _velocity.Z = 0;
2933 m_rotationalVelocity.X = 0;
2934 m_rotationalVelocity.Y = 0;
2935 m_rotationalVelocity.Z = 0;
2936
2937 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
2938 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
2939 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
2940 m_lastposition = _position;
2941 m_lastorientation = _orientation;
2942
2943 base.RequestPhysicsterseUpdate();
2944
2945 m_throttleUpdates = false;
2946 throttleCounter = 0;
2947 _zeroFlag = true;
2948
2949 disableBodySoft(); // disable it and colisions
2950 base.RaiseOutOfBounds(_position);
2951
2952 return;
2953 }
2954
2955 d.Quaternion ori;
2956 d.GeomCopyQuaternion(prim_geom, out ori);
2957 d.Vector3 vel = d.BodyGetLinearVel(Body);
2958 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2959
2960 if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
2961 && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
2962 && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
2963 && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)
2964 && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)
2965 && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)
2966 )
2967 {
2968 _zeroFlag = true;
2969 //Console.WriteLine("ZFT 2");
2970 m_throttleUpdates = false;
2971 }
2972 else
2973 {
2974 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2975 _zeroFlag = false;
2976 m_lastUpdateSent = false;
2977 //m_throttleUpdates = false;
2978 }
2979
2980 if (_zeroFlag)
2981 {
2982 m_lastposition = _position;
2983 m_lastorientation = _orientation;
2984
2985 _velocity.X = 0.0f;
2986 _velocity.Y = 0.0f;
2987 _velocity.Z = 0.0f;
2988
2989 _acceleration.X = 0;
2990 _acceleration.Y = 0;
2991 _acceleration.Z = 0;
2992
2993 m_rotationalVelocity.X = 0;
2994 m_rotationalVelocity.Y = 0;
2995 m_rotationalVelocity.Z = 0;
2996 if (!m_lastUpdateSent)
2997 {
2998 m_throttleUpdates = false;
2999 throttleCounter = 0;
3000 m_rotationalVelocity = pv;
3001
3002 base.RequestPhysicsterseUpdate();
3003
3004 m_lastUpdateSent = true;
3005 }
3006 }
3007 else
3008 {
3009 if (lastZeroFlag != _zeroFlag)
3010 {
3011 base.RequestPhysicsterseUpdate();
3012 }
3013
3014 m_lastVelocity = _velocity;
3015
3016 _position.X = lpos.X;
3017 _position.Y = lpos.Y;
3018 _position.Z = lpos.Z;
3019
3020 _velocity.X = vel.X;
3021 _velocity.Y = vel.Y;
3022 _velocity.Z = vel.Z;
3023
3024 _orientation.X = ori.X;
3025 _orientation.Y = ori.Y;
3026 _orientation.Z = ori.Z;
3027 _orientation.W = ori.W;
3028
3029 _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);
3030
3031 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
3032 {
3033 m_rotationalVelocity = pv;
3034 }
3035 else
3036 {
3037 m_rotationalVelocity.X = rotvel.X;
3038 m_rotationalVelocity.Y = rotvel.Y;
3039 m_rotationalVelocity.Z = rotvel.Z;
3040 }
3041
3042 m_lastUpdateSent = false;
3043 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3044 {
3045 m_lastposition = _position;
3046 m_lastorientation = _orientation;
3047 base.RequestPhysicsterseUpdate();
3048 }
3049 else
3050 {
3051 throttleCounter++;
3052 }
3053 }
3054 }
3055 else if (!m_lastUpdateSent || !_zeroFlag)
3056 {
3057 // Not a body.. so Make sure the client isn't interpolating
3058 _velocity.X = 0;
3059 _velocity.Y = 0;
3060 _velocity.Z = 0;
3061
3062 _acceleration.X = 0;
3063 _acceleration.Y = 0;
3064 _acceleration.Z = 0;
3065
3066 m_rotationalVelocity.X = 0;
3067 m_rotationalVelocity.Y = 0;
3068 m_rotationalVelocity.Z = 0;
3069 _zeroFlag = true;
3070
3071 if (!m_lastUpdateSent)
3072 {
3073 m_throttleUpdates = false;
3074 throttleCounter = 0;
3075
3076 base.RequestPhysicsterseUpdate();
3077
3078 m_lastUpdateSent = true;
3079 }
3080 }
3081 }
3082 }
3083
3084 internal static bool QuaternionIsFinite(Quaternion q)
3085 {
3086 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3087 return false;
3088 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3089 return false;
3090 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3091 return false;
3092 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3093 return false;
3094 return true;
3095 }
3096
3097 internal static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3098 {
3099 dst.c.W = src.c.W;
3100 dst.c.X = src.c.X;
3101 dst.c.Y = src.c.Y;
3102 dst.c.Z = src.c.Z;
3103 dst.mass = src.mass;
3104 dst.I.M00 = src.I.M00;
3105 dst.I.M01 = src.I.M01;
3106 dst.I.M02 = src.I.M02;
3107 dst.I.M10 = src.I.M10;
3108 dst.I.M11 = src.I.M11;
3109 dst.I.M12 = src.I.M12;
3110 dst.I.M20 = src.I.M20;
3111 dst.I.M21 = src.I.M21;
3112 dst.I.M22 = src.I.M22;
3113 }
3114
3115 private static void DMassDup(ref d.Mass src, out d.Mass dst)
3116 {
3117 dst = new d.Mass { };
3118
3119 dst.c.W = src.c.W;
3120 dst.c.X = src.c.X;
3121 dst.c.Y = src.c.Y;
3122 dst.c.Z = src.c.Z;
3123 dst.mass = src.mass;
3124 dst.I.M00 = src.I.M00;
3125 dst.I.M01 = src.I.M01;
3126 dst.I.M02 = src.I.M02;
3127 dst.I.M10 = src.I.M10;
3128 dst.I.M11 = src.I.M11;
3129 dst.I.M12 = src.I.M12;
3130 dst.I.M20 = src.I.M20;
3131 dst.I.M21 = src.I.M21;
3132 dst.I.M22 = src.I.M22;
3133 }
3134 private void donullchange()
3135 {
3136 }
3137
3138 public bool DoAChange(changes what, object arg)
3139 {
3140 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3141 {
3142 return false;
3143 }
3144
3145 // nasty switch
3146 switch (what)
3147 {
3148 case changes.Add:
3149 changeadd();
3150 break;
3151 case changes.Remove:
3152 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3153 //When we return true, it destroys all of the prims in the linkset anyway
3154 if (_parent != null)
3155 {
3156 OdePrim parent = (OdePrim)_parent;
3157 parent.ChildRemove(this, false);
3158 }
3159 else
3160 ChildRemove(this, false);
3161
3162 RemoveGeom();
3163 m_targetSpace = IntPtr.Zero;
3164 if (m_eventsubscription > 0)
3165 UnSubscribeEvents();
3166 return true;
3167
3168 case changes.Link:
3169 OdePrim tmp = (OdePrim)arg;
3170 changeLink(tmp);
3171 break;
3172
3173 case changes.DeLink:
3174 changeLink(null);
3175 break;
3176
3177 case changes.Position:
3178 changePosition((Vector3)arg);
3179 break;
3180
3181 case changes.Orientation:
3182 changeOrientation((Quaternion)arg);
3183 break;
3184
3185 case changes.PosOffset:
3186 donullchange();
3187 break;
3188
3189 case changes.OriOffset:
3190 donullchange();
3191 break;
3192
3193 case changes.Velocity:
3194 changevelocity((Vector3)arg);
3195 break;
3196
3197 // case changes.Acceleration:
3198 // changeacceleration((Vector3)arg);
3199 // break;
3200 // case changes.AngVelocity:
3201 // changeangvelocity((Vector3)arg);
3202 // break;
3203
3204 case changes.Force:
3205 changeForce((Vector3)arg);
3206 break;
3207
3208 case changes.Torque:
3209 changeSetTorque((Vector3)arg);
3210 break;
3211
3212 case changes.AddForce:
3213 changeAddForce((Vector3)arg);
3214 break;
3215
3216 case changes.AddAngForce:
3217 changeAddAngularForce((Vector3)arg);
3218 break;
3219
3220 case changes.AngLock:
3221 changeAngularLock((Vector3)arg);
3222 break;
3223
3224 case changes.Size:
3225 changeSize((Vector3)arg);
3226 break;
3227
3228 case changes.Shape:
3229 changeShape((PrimitiveBaseShape)arg);
3230 break;
3231
3232 case changes.CollidesWater:
3233 changeFloatOnWater((bool)arg);
3234 break;
3235
3236 case changes.VolumeDtc:
3237 changeVolumedetetion((bool)arg);
3238 break;
3239
3240 case changes.Physical:
3241 changePhysicsStatus((bool)arg);
3242 break;
3243
3244 case changes.Selected:
3245 changeSelectedStatus((bool)arg);
3246 break;
3247
3248 case changes.disabled:
3249 changeDisable((bool)arg);
3250 break;
3251
3252 case changes.building:
3253 changeBuilding((bool)arg);
3254 break;
3255
3256 case changes.VehicleType:
3257 changeVehicleType((int)arg);
3258 break;
3259
3260 case changes.VehicleFlags:
3261 changeVehicleFlags((strVehicleBoolParam) arg);
3262 break;
3263
3264 case changes.VehicleFloatParam:
3265 changeVehicleFloatParam((strVehicleFloatParam) arg);
3266 break;
3267
3268 case changes.VehicleVectorParam:
3269 changeVehicleVectorParam((strVehicleVectorParam) arg);
3270 break;
3271
3272 case changes.VehicleRotationParam:
3273 changeVehicleRotationParam((strVehicleQuatParam) arg);
3274 break;
3275
3276 case changes.Null:
3277 donullchange();
3278 break;
3279
3280 default:
3281 donullchange();
3282 break;
3283 }
3284 return false;
3285 }
3286
3287 public void AddChange(changes what, object arg)
3288 {
3289 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3290 }
3291
3292
3293 private struct strVehicleBoolParam
3294 {
3295 public int param;
3296 public bool value;
3297 }
3298
3299 private struct strVehicleFloatParam
3300 {
3301 public int param;
3302 public float value;
3303 }
3304
3305 private struct strVehicleQuatParam
3306 {
3307 public int param;
3308 public Quaternion value;
3309 }
3310
3311 private struct strVehicleVectorParam
3312 {
3313 public int param;
3314 public Vector3 value;
3315 }
3316 }
3317} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..4b3f83b
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,443 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenMetaverse;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37
38namespace OpenSim.Region.Physics.OdePlugin
39{
40 /// <summary>
41 /// Processes raycast requests as ODE is in a state to be able to do them.
42 /// This ensures that it's thread safe and there will be no conflicts.
43 /// Requests get returned by a different thread then they were requested by.
44 /// </summary>
45 public class ODERayCastRequestManager
46 {
47 /// <summary>
48 /// Pending ray requests
49 /// </summary>
50 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
51
52 /// <summary>
53 /// Scene that created this object.
54 /// </summary>
55 private OdeScene m_scene;
56
57 IntPtr ray;
58
59 private const int ColisionContactGeomsPerTest = 5;
60
61 /// <summary>
62 /// ODE near callback delegate
63 /// </summary>
64 private d.NearCallback nearCallback;
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66 private List<ContactResult> m_contactResults = new List<ContactResult>();
67
68 public ODERayCastRequestManager(OdeScene pScene)
69 {
70 m_scene = pScene;
71 nearCallback = near;
72 ray = d.CreateRay(IntPtr.Zero, 1.0f);
73 }
74
75 /// <summary>
76 /// Queues a raycast
77 /// </summary>
78 /// <param name="position">Origin of Ray</param>
79 /// <param name="direction">Ray normal</param>
80 /// <param name="length">Ray length</param>
81 /// <param name="retMethod">Return method to send the results</param>
82 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
83 {
84 ODERayRequest req = new ODERayRequest();
85 req.geom = IntPtr.Zero;
86 req.callbackMethod = retMethod;
87 req.Count = 0;
88 req.length = length;
89 req.Normal = direction;
90 req.Origin = position;
91
92 m_PendingRequests.Enqueue(req);
93 }
94
95 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
96 {
97 ODERayRequest req = new ODERayRequest();
98 req.geom = geom;
99 req.callbackMethod = retMethod;
100 req.length = length;
101 req.Normal = direction;
102 req.Origin = position;
103 req.Count = 0;
104
105 m_PendingRequests.Enqueue(req);
106 }
107
108 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
109 {
110 ODERayRequest req = new ODERayRequest();
111 req.geom = IntPtr.Zero;
112 req.callbackMethod = retMethod;
113 req.Count = 0;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117
118 m_PendingRequests.Enqueue(req);
119 }
120
121 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
122 {
123 ODERayRequest req = new ODERayRequest();
124 req.geom = geom;
125 req.callbackMethod = retMethod;
126 req.length = length;
127 req.Normal = direction;
128 req.Origin = position;
129 req.Count = 0;
130
131 m_PendingRequests.Enqueue(req);
132 }
133
134 /// <summary>
135 /// Queues a raycast
136 /// </summary>
137 /// <param name="position">Origin of Ray</param>
138 /// <param name="direction">Ray normal</param>
139 /// <param name="length">Ray length</param>
140 /// <param name="count"></param>
141 /// <param name="retMethod">Return method to send the results</param>
142 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
143 {
144 ODERayRequest req = new ODERayRequest();
145 req.geom = IntPtr.Zero;
146 req.callbackMethod = retMethod;
147 req.length = length;
148 req.Normal = direction;
149 req.Origin = position;
150 req.Count = count;
151
152 m_PendingRequests.Enqueue(req);
153 }
154
155 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
156 {
157 ODERayRequest req = new ODERayRequest();
158 req.geom = geom;
159 req.callbackMethod = retMethod;
160 req.length = length;
161 req.Normal = direction;
162 req.Origin = position;
163 req.Count = count;
164
165 m_PendingRequests.Enqueue(req);
166 }
167
168 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
169 {
170 ODERayRequest req = new ODERayRequest();
171 req.geom = IntPtr.Zero;
172 req.callbackMethod = retMethod;
173 req.length = length;
174 req.Normal = direction;
175 req.Origin = position;
176 req.Count = count;
177
178 m_PendingRequests.Enqueue(req);
179 }
180
181 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
182 {
183 ODERayRequest req = new ODERayRequest();
184 req.geom = geom;
185 req.callbackMethod = retMethod;
186 req.length = length;
187 req.Normal = direction;
188 req.Origin = position;
189 req.Count = count;
190
191 m_PendingRequests.Enqueue(req);
192 }
193
194 /// <summary>
195 /// Process all queued raycast requests
196 /// </summary>
197 /// <returns>Time in MS the raycasts took to process.</returns>
198 public int ProcessQueuedRequests()
199 {
200 int time = System.Environment.TickCount;
201
202 if (m_PendingRequests.Count <= 0)
203 return 0;
204
205 if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still
206 {
207 m_PendingRequests.Clear();
208 return 0;
209 }
210
211 ODERayRequest req;
212
213 int i = 50; // arbitary limit of processed tests per frame
214
215 while(m_PendingRequests.Dequeue(out req))
216 {
217 if (req.geom == IntPtr.Zero)
218 doSpaceRay(req);
219 else
220 doGeomRay(req);
221 if(--i < 0)
222 break;
223 }
224
225 lock (m_contactResults)
226 m_contactResults.Clear();
227
228 return System.Environment.TickCount - time;
229 }
230 /// <summary>
231 /// Method that actually initiates the raycast with full top space
232 /// </summary>
233 /// <param name="req"></param>
234 private void doSpaceRay(ODERayRequest req)
235 {
236 // Create the ray
237// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
238 d.GeomRaySetLength(ray, req.length);
239 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
240
241 // Collide test
242 d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback);
243
244 // Remove Ray
245// d.GeomDestroy(ray);
246
247 if (req.callbackMethod == null)
248 return;
249
250 if (req.callbackMethod is RaycastCallback)
251 {
252 // Define default results
253 bool hitYN = false;
254 uint hitConsumerID = 0;
255 float distance = 999999999999f;
256 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
257 Vector3 snormal = Vector3.Zero;
258
259 // Find closest contact and object.
260 lock (m_contactResults)
261 {
262 foreach (ContactResult cResult in m_contactResults)
263 {
264 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
265 {
266 closestcontact = cResult.Pos;
267 hitConsumerID = cResult.ConsumerID;
268 distance = cResult.Depth;
269 hitYN = true;
270 snormal = cResult.Normal;
271 }
272 }
273 m_contactResults.Clear();
274 }
275
276 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
277 }
278 else
279 {
280 ((RayCallback)req.callbackMethod)(m_contactResults);
281 lock (m_PendingRequests)
282 m_contactResults.Clear();
283 }
284 }
285
286 /// <summary>
287 /// Method that actually initiates the raycast with a geom
288 /// </summary>
289 /// <param name="req"></param>
290 private void doGeomRay(ODERayRequest req)
291 {
292 // Create the ray
293// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length);
294 d.GeomRaySetLength(ray, req.length);
295 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
296
297 // Collide test
298 d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
299
300 // Remove Ray
301// d.GeomDestroy(ray);
302
303 if (req.callbackMethod == null)
304 return;
305
306 if (req.callbackMethod is RaycastCallback)
307 {
308 // Define default results
309 bool hitYN = false;
310 uint hitConsumerID = 0;
311 float distance = 999999999999f;
312 Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
313 Vector3 snormal = Vector3.Zero;
314
315 // Find closest contact and object.
316 lock (m_contactResults)
317 {
318 foreach (ContactResult cResult in m_contactResults)
319 {
320 if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
321 {
322 closestcontact = cResult.Pos;
323 hitConsumerID = cResult.ConsumerID;
324 distance = cResult.Depth;
325 hitYN = true;
326 snormal = cResult.Normal;
327 }
328 }
329 m_contactResults.Clear();
330 }
331
332 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
333 }
334 else
335 {
336 ((RayCallback)req.callbackMethod)(m_contactResults);
337 lock (m_PendingRequests)
338 m_contactResults.Clear();
339 }
340 }
341
342 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
343 {
344 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
345 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
346 return false;
347
348 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
349 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
350 return true;
351 }
352
353 // This is the standard Near. g2 is the ray
354 private void near(IntPtr space, IntPtr g1, IntPtr g2)
355 {
356 //Don't test against heightfield Geom, or you'll be sorry!
357 // Exclude heightfield geom
358
359 if (g1 == IntPtr.Zero || g1 == g2)
360 return;
361
362 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
363 return;
364
365 // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
366 if (d.GeomIsSpace(g1))
367 {
368 try
369 {
370 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
371 }
372 catch (Exception e)
373 {
374 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
375 }
376 return;
377 }
378
379 int count = 0;
380 try
381 {
382 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
383 }
384 catch (SEHException)
385 {
386 m_log.Error("[PHYSICS Ray]: 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.");
387 }
388 catch (Exception e)
389 {
390 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
391 return;
392 }
393
394 if (count == 0)
395 return;
396
397 PhysicsActor p1 = null;
398
399 if (g1 != IntPtr.Zero)
400 m_scene.actor_name_map.TryGetValue(g1, out p1);
401
402 d.ContactGeom curcontact = new d.ContactGeom();
403 // Loop over contacts, build results.
404 for (int i = 0; i < count; i++)
405 {
406 if (!GetCurContactGeom(i, ref curcontact))
407 break;
408 if (p1 != null) {
409 if (p1 is OdePrim)
410 {
411 ContactResult collisionresult = new ContactResult();
412
413 collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
414 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
415 collisionresult.Depth = curcontact.depth;
416 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
417 curcontact.normal.Z);
418 lock (m_contactResults)
419 m_contactResults.Add(collisionresult);
420 }
421 }
422 }
423 }
424
425 /// <summary>
426 /// Dereference the creator scene so that it can be garbage collected if needed.
427 /// </summary>
428 internal void Dispose()
429 {
430 m_scene = null;
431 }
432 }
433
434 public struct ODERayRequest
435 {
436 public IntPtr geom;
437 public Vector3 Origin;
438 public Vector3 Normal;
439 public int Count;
440 public float length;
441 public object callbackMethod;
442 }
443} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
new file mode 100644
index 0000000..e62746e
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs
@@ -0,0 +1,1961 @@
1
2/*
3 * based on:
4 * Ode.NET - .NET bindings for ODE
5 * Jason Perkins (starkos@industriousone.com)
6 * Licensed under the New BSD
7 * Part of the OpenDynamicsEngine
8Open Dynamics Engine
9Copyright (c) 2001-2007, Russell L. Smith.
10All rights reserved.
11
12Redistribution and use in source and binary forms, with or without
13modification, are permitted provided that the following conditions
14are met:
15
16Redistributions of source code must retain the above copyright notice,
17this list of conditions and the following disclaimer.
18
19Redistributions in binary form must reproduce the above copyright notice,
20this list of conditions and the following disclaimer in the documentation
21and/or other materials provided with the distribution.
22
23Neither the names of ODE's copyright owner nor the names of its
24contributors may be used to endorse or promote products derived from
25this software without specific prior written permission.
26
27THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
31OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
32SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
33TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
34PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
35LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
36NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
37SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
38 *
39 * changes by opensim team;
40 * changes by Aurora team http://www.aurora-sim.org/
41
42 * Revision/fixs by Ubit Umarov
43 */
44
45using System;
46using System.Runtime.InteropServices;
47using System.Security;
48
49namespace OdeAPI
50{
51//#if dDOUBLE
52// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim
53// at least we save same memory and memory access time, FPU performance on intel usually is similar
54// using dReal = System.Double;
55//#else
56 using dReal = System.Single;
57//#endif
58
59 public static class d
60 {
61 public static dReal Infinity = dReal.MaxValue;
62 public static int NTotalBodies = 0;
63 public static int NTotalGeoms = 0;
64
65 #region Flags and Enumerations
66
67 [Flags]
68 public enum AllocateODEDataFlags : uint
69 {
70 BasicData = 0,
71 CollisionData = 0x00000001,
72 All = ~0u
73 }
74
75 [Flags]
76 public enum IniteODEFlags : uint
77 {
78 dInitFlagManualThreadCleanup = 0x00000001
79 }
80
81 [Flags]
82 public enum ContactFlags : int
83 {
84 Mu2 = 0x001,
85 FDir1 = 0x002,
86 Bounce = 0x004,
87 SoftERP = 0x008,
88 SoftCFM = 0x010,
89 Motion1 = 0x020,
90 Motion2 = 0x040,
91 MotionN = 0x080,
92 Slip1 = 0x100,
93 Slip2 = 0x200,
94 Approx0 = 0x0000,
95 Approx1_1 = 0x1000,
96 Approx1_2 = 0x2000,
97 Approx1 = 0x3000
98 }
99
100 public enum GeomClassID : int
101 {
102 SphereClass,
103 BoxClass,
104 CapsuleClass,
105 CylinderClass,
106 PlaneClass,
107 RayClass,
108 ConvexClass,
109 GeomTransformClass,
110 TriMeshClass,
111 HeightfieldClass,
112 FirstSpaceClass,
113 SimpleSpaceClass = FirstSpaceClass,
114 HashSpaceClass,
115 QuadTreeSpaceClass,
116 LastSpaceClass = QuadTreeSpaceClass,
117 FirstUserClass,
118 LastUserClass = FirstUserClass + MaxUserClasses - 1,
119 NumClasses,
120 MaxUserClasses = 4
121 }
122
123 public enum JointType : int
124 {
125 None,
126 Ball,
127 Hinge,
128 Slider,
129 Contact,
130 Universal,
131 Hinge2,
132 Fixed,
133 Null,
134 AMotor,
135 LMotor,
136 Plane2D
137 }
138
139 public enum JointParam : int
140 {
141 LoStop,
142 HiStop,
143 Vel,
144 FMax,
145 FudgeFactor,
146 Bounce,
147 CFM,
148 StopERP,
149 StopCFM,
150 SuspensionERP,
151 SuspensionCFM,
152 LoStop2 = 256,
153 HiStop2,
154 Vel2,
155 FMax2,
156 FudgeFactor2,
157 Bounce2,
158 CFM2,
159 StopERP2,
160 StopCFM2,
161 SuspensionERP2,
162 SuspensionCFM2,
163 LoStop3 = 512,
164 HiStop3,
165 Vel3,
166 FMax3,
167 FudgeFactor3,
168 Bounce3,
169 CFM3,
170 StopERP3,
171 StopCFM3,
172 SuspensionERP3,
173 SuspensionCFM3
174 }
175
176 public enum dSweepAndPruneAxis : int
177 {
178 XYZ = ((0)|(1<<2)|(2<<4)),
179 XZY = ((0)|(2<<2)|(1<<4)),
180 YXZ = ((1)|(0<<2)|(2<<4)),
181 YZX = ((1)|(2<<2)|(0<<4)),
182 ZXY = ((2)|(0<<2)|(1<<4)),
183 ZYX = ((2)|(1<<2)|(0<<4))
184 }
185
186 #endregion
187
188 #region Callbacks
189
190 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
191 public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb);
192
193 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
194 public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip);
195
196 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
197 public delegate void GetAABBFn(IntPtr geom, out AABB aabb);
198
199 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
200 public delegate ColliderFn GetColliderFnFn(int num);
201
202 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
203 public delegate void GeomDtorFn(IntPtr o);
204
205 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
206 public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z);
207
208 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
209 public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2);
210
211 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
212 public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex);
213
214 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
215 public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount);
216
217 [UnmanagedFunctionPointer(CallingConvention.Cdecl)]
218 public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v);
219
220 #endregion
221
222 #region Structs
223
224 [StructLayout(LayoutKind.Sequential)]
225 public struct AABB
226 {
227 public dReal MinX, MaxX;
228 public dReal MinY, MaxY;
229 public dReal MinZ, MaxZ;
230 }
231
232
233 [StructLayout(LayoutKind.Sequential)]
234 public struct Contact
235 {
236 public SurfaceParameters surface;
237 public ContactGeom geom;
238 public Vector3 fdir1;
239 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact));
240 }
241
242
243 [StructLayout(LayoutKind.Sequential)]
244 public struct ContactGeom
245 {
246
247 public Vector3 pos;
248 public Vector3 normal;
249 public dReal depth;
250 public IntPtr g1;
251 public IntPtr g2;
252 public int side1;
253 public int side2;
254 public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom));
255 }
256
257 [StructLayout(LayoutKind.Sequential)]
258 public struct GeomClass
259 {
260 public int bytes;
261 public GetColliderFnFn collider;
262 public GetAABBFn aabb;
263 public AABBTestFn aabb_test;
264 public GeomDtorFn dtor;
265 }
266
267
268 [StructLayout(LayoutKind.Sequential)]
269 public struct JointFeedback
270 {
271 public Vector3 f1;
272 public Vector3 t1;
273 public Vector3 f2;
274 public Vector3 t2;
275 }
276
277
278 [StructLayout(LayoutKind.Sequential)]
279 public struct Mass
280 {
281 public dReal mass;
282 public Vector4 c;
283 public Matrix3 I;
284 }
285
286
287 [StructLayout(LayoutKind.Sequential)]
288 public struct Matrix3
289 {
290 public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22)
291 {
292 M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f;
293 M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f;
294 M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f;
295 }
296 public dReal M00, M10, M20;
297 private dReal _m30;
298 public dReal M01, M11, M21;
299 private dReal _m31;
300 public dReal M02, M12, M22;
301 private dReal _m32;
302 }
303
304 [StructLayout(LayoutKind.Sequential)]
305 public struct Matrix4
306 {
307 public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30,
308 dReal m01, dReal m11, dReal m21, dReal m31,
309 dReal m02, dReal m12, dReal m22, dReal m32,
310 dReal m03, dReal m13, dReal m23, dReal m33)
311 {
312 M00 = m00; M10 = m10; M20 = m20; M30 = m30;
313 M01 = m01; M11 = m11; M21 = m21; M31 = m31;
314 M02 = m02; M12 = m12; M22 = m22; M32 = m32;
315 M03 = m03; M13 = m13; M23 = m23; M33 = m33;
316 }
317 public dReal M00, M10, M20, M30;
318 public dReal M01, M11, M21, M31;
319 public dReal M02, M12, M22, M32;
320 public dReal M03, M13, M23, M33;
321 }
322
323 [StructLayout(LayoutKind.Sequential)]
324 public struct Quaternion
325 {
326 public dReal W, X, Y, Z;
327 }
328
329
330 [StructLayout(LayoutKind.Sequential)]
331 public struct SurfaceParameters
332 {
333 public ContactFlags mode;
334 public dReal mu;
335 public dReal mu2;
336 public dReal bounce;
337 public dReal bounce_vel;
338 public dReal soft_erp;
339 public dReal soft_cfm;
340 public dReal motion1;
341 public dReal motion2;
342 public dReal motionN;
343 public dReal slip1;
344 public dReal slip2;
345 }
346
347
348 [StructLayout(LayoutKind.Sequential)]
349 public struct Vector3
350 {
351 public Vector3(dReal x, dReal y, dReal z)
352 {
353 X = x; Y = y; Z = z; _w = 0.0f;
354 }
355 public dReal X, Y, Z;
356 private dReal _w;
357 }
358
359
360 [StructLayout(LayoutKind.Sequential)]
361 public struct Vector4
362 {
363 public Vector4(dReal x, dReal y, dReal z, dReal w)
364 {
365 X = x; Y = y; Z = z; W = w;
366 }
367 public dReal X, Y, Z, W;
368 }
369
370 #endregion
371
372 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity]
373 public static extern int AllocateODEDataForThread(uint ODEInitFlags);
374
375 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity]
376 public static extern bool AreConnected(IntPtr b1, IntPtr b2);
377
378 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity]
379 public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type);
380
381 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity]
382 public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz);
383
384 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity]
385 public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
386
387 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity]
388 public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
389
390 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity]
391 public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz);
392
393 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity]
394 public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
395
396 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity]
397 public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz);
398
399 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity]
400 public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
401
402 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity]
403 public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz);
404
405 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
406 public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos);
407
408 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity]
409 public static extern void BodyCopyPosition(IntPtr body, out dReal X);
410
411 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
412 public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat);
413
414 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity]
415 public static extern void BodyCopyQuaternion(IntPtr body, out dReal X);
416
417 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
418 public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R);
419
420 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity]
421 public static extern void BodyCopyRotation(IntPtr body, out dReal M00);
422
423 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity]
424 public static extern IntPtr BodyiCreate(IntPtr world);
425 public static IntPtr BodyCreate(IntPtr world)
426 {
427 NTotalBodies++;
428 return BodyiCreate(world);
429 }
430
431 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity]
432 public static extern void BodyiDestroy(IntPtr body);
433 public static void BodyDestroy(IntPtr body)
434 {
435 NTotalBodies--;
436 BodyiDestroy(body);
437 }
438
439 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity]
440 public static extern void BodyDisable(IntPtr body);
441
442 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity]
443 public static extern void BodyEnable(IntPtr body);
444
445 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
446 public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body);
447
448 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
449 public static extern bool BodyGetAutoDisableFlag(IntPtr body);
450
451 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
452 public static extern void BodyGetAutoDisableDefaults(IntPtr body);
453
454 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
455 public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body);
456
457 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
458 public static extern int BodyGetAutoDisableSteps(IntPtr body);
459
460 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
461 public static extern dReal BodyGetAutoDisableTime(IntPtr body);
462
463 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity]
464 public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body);
465 public static Vector3 BodyGetAngularVel(IntPtr body)
466 {
467 unsafe { return *(BodyGetAngularVelUnsafe(body)); }
468 }
469
470 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity]
471 public static extern IntPtr BodyGetData(IntPtr body);
472
473 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
474 public static extern int BodyGetFiniteRotationMode(IntPtr body);
475
476 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
477 public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result);
478
479 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity]
480 public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body);
481 public static Vector3 BodyGetForce(IntPtr body)
482 {
483 unsafe { return *(BodyGetForceUnsafe(body)); }
484 }
485
486 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity]
487 public static extern bool BodyGetGravityMode(IntPtr body);
488
489 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
490 public static extern int BodyGetGyroscopicMode(IntPtr body);
491
492 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity]
493 public static extern IntPtr BodyGetJoint(IntPtr body, int index);
494
495 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity]
496 public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body);
497 public static Vector3 BodyGetLinearVel(IntPtr body)
498 {
499 unsafe { return *(BodyGetLinearVelUnsafe(body)); }
500 }
501
502 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity]
503 public static extern void BodyGetMass(IntPtr body, out Mass mass);
504
505 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity]
506 public static extern int BodyGetNumJoints(IntPtr body);
507
508 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity]
509 public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
510
511 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity]
512 public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body);
513 public static Vector3 BodyGetPosition(IntPtr body)
514 {
515 unsafe { return *(BodyGetPositionUnsafe(body)); }
516 }
517
518 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity]
519 public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
520
521 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity]
522 public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body);
523 public static Quaternion BodyGetQuaternion(IntPtr body)
524 {
525 unsafe { return *(BodyGetQuaternionUnsafe(body)); }
526 }
527
528 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity]
529 public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
530
531 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity]
532 public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
533
534 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity]
535 public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body);
536 public static Matrix3 BodyGetRotation(IntPtr body)
537 {
538 unsafe { return *(BodyGetRotationUnsafe(body)); }
539 }
540
541 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity]
542 public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body);
543 public static Vector3 BodyGetTorque(IntPtr body)
544 {
545 unsafe { return *(BodyGetTorqueUnsafe(body)); }
546 }
547
548 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity]
549 public static extern IntPtr BodyGetWorld(IntPtr body);
550
551 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity]
552 public static extern IntPtr BodyGetFirstGeom(IntPtr body);
553
554 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity]
555 public static extern IntPtr dBodyGetNextGeom(IntPtr Geom);
556
557
558 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity]
559 public static extern bool BodyIsEnabled(IntPtr body);
560
561 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity]
562 public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z);
563
564 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
565 public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold);
566
567 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity]
568 public static extern void BodySetAutoDisableDefaults(IntPtr body);
569
570 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
571 public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable);
572
573 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
574 public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold);
575
576 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
577 public static extern void BodySetAutoDisableSteps(IntPtr body, int steps);
578
579 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
580 public static extern void BodySetAutoDisableTime(IntPtr body, dReal time);
581
582 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity]
583 public static extern void BodySetData(IntPtr body, IntPtr data);
584
585 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity]
586 public static extern void BodySetFiniteRotationMode(IntPtr body, int mode);
587
588 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity]
589 public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z);
590
591 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity]
592 public static extern void BodySetLinearDamping(IntPtr body, dReal scale);
593
594 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
595 public static extern void BodySetAngularDamping(IntPtr body, dReal scale);
596
597 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity]
598 public static extern dReal BodyGetLinearDamping(IntPtr body);
599
600 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity]
601 public static extern dReal BodyGetAngularDamping(IntPtr body);
602
603 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity]
604 public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale);
605
606 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
607 public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold);
608
609 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
610 public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold);
611
612 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
613 public static extern dReal BodyGetLinearDampingThreshold(IntPtr body);
614
615 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
616 public static extern dReal BodyGetAngularDampingThreshold(IntPtr body);
617
618 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity]
619 public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z);
620
621 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity]
622 public static extern void BodySetGravityMode(IntPtr body, bool mode);
623
624 /// <summary>
625 /// Sets the Gyroscopic term status on the body specified.
626 /// </summary>
627 /// <param name="body">Pointer to body</param>
628 /// <param name="enabled">NonZero enabled, Zero disabled</param>
629 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity]
630 public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled);
631
632 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity]
633 public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z);
634
635 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity]
636 public static extern void BodySetMass(IntPtr body, ref Mass mass);
637
638 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity]
639 public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z);
640
641 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
642 public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q);
643
644 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity]
645 public static extern void BodySetQuaternion(IntPtr body, ref dReal w);
646
647 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
648 public static extern void BodySetRotation(IntPtr body, ref Matrix3 R);
649
650 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity]
651 public static extern void BodySetRotation(IntPtr body, ref dReal M00);
652
653 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity]
654 public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z);
655
656 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity]
657 public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
658
659 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity]
660 public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result);
661
662 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity]
663 public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1,
664 ref Vector3 side1, ref Vector3 p2,
665 ref Matrix3 R2, ref Vector3 side2,
666 ref Vector3 normal, out dReal depth, out int return_code,
667 int maxc, out ContactGeom contact, int skip);
668
669 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity]
670 public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1,
671 ref Vector3 side1, ref Vector3 _p2,
672 ref Matrix3 R2, ref Vector3 side2);
673
674 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity]
675 public static extern void CleanupODEAllDataForThread();
676
677 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity]
678 public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2,
679 ref Vector3 b1, ref Vector3 b2,
680 ref Vector3 cp1, ref Vector3 cp2);
681
682 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity]
683 public static extern void CloseODE();
684
685 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
686 public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip);
687 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity]
688 public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip);
689
690 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity]
691 public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2);
692
693 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity]
694 public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz);
695 public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz)
696 {
697 NTotalGeoms++;
698 return CreateiBox(space, lx, ly, lz);
699 }
700
701 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity]
702 public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length);
703 public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length)
704 {
705 NTotalGeoms++;
706 return CreateiCapsule(space, radius, length);
707 }
708
709 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity]
710 public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
711 public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons)
712 {
713 NTotalGeoms++;
714 return CreateiConvex(space, planes, planeCount, points, pointCount, polygons);
715 }
716
717 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity]
718 public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length);
719 public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length)
720 {
721 NTotalGeoms++;
722 return CreateiCylinder(space, radius, length);
723 }
724
725 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity]
726 public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable);
727 public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable)
728 {
729 NTotalGeoms++;
730 return CreateiHeightfield(space, data, bPlaceable);
731 }
732
733 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity]
734 public static extern IntPtr CreateiGeom(int classnum);
735 public static IntPtr CreateGeom(int classnum)
736 {
737 NTotalGeoms++;
738 return CreateiGeom(classnum);
739 }
740
741 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity]
742 public static extern int CreateGeomClass(ref GeomClass classptr);
743
744 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity]
745 public static extern IntPtr CreateGeomTransform(IntPtr space);
746
747 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity]
748 public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d);
749 public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d)
750 {
751 NTotalGeoms++;
752 return CreateiPlane(space, a, b, c, d);
753 }
754
755 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity]
756 public static extern IntPtr CreateiRay(IntPtr space, dReal length);
757 public static IntPtr CreateRay(IntPtr space, dReal length)
758 {
759 NTotalGeoms++;
760 return CreateiRay(space, length);
761 }
762
763 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity]
764 public static extern IntPtr CreateiSphere(IntPtr space, dReal radius);
765 public static IntPtr CreateSphere(IntPtr space, dReal radius)
766 {
767 NTotalGeoms++;
768 return CreateiSphere(space, radius);
769 }
770
771 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity]
772 public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data,
773 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback);
774 public static IntPtr CreateTriMesh(IntPtr space, IntPtr data,
775 TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback)
776 {
777 NTotalGeoms++;
778 return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback);
779 }
780 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity]
781 public static extern dReal Dot(ref dReal X0, ref dReal X1, int n);
782
783 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity]
784 public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q);
785
786 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity]
787 public static extern int FactorCholesky(ref dReal A00, int n);
788
789 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity]
790 public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip);
791
792 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
793 public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len);
794
795 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity]
796 public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x);
797
798 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity]
799 public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z);
800
801 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity]
802 public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z);
803
804 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity]
805 public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length);
806
807 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity]
808 public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
809
810 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity]
811 public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length);
812
813 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity]
814 public static extern void GeomClearOffset(IntPtr geom);
815
816 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
817 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos);
818
819 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity]
820 public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X);
821
822 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
823 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q);
824
825 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
826 public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X);
827
828 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
829 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R);
830
831 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity]
832 public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00);
833
834 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
835 public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos);
836
837 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity]
838 public static extern void GeomCopyPosition(IntPtr geom, out dReal X);
839
840 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
841 public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R);
842
843 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity]
844 public static extern void GeomCopyRotation(IntPtr geom, out dReal M00);
845
846 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity]
847 public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length);
848
849 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity]
850 public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length);
851
852 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity]
853 public static extern void GeomiDestroy(IntPtr geom);
854 public static void GeomDestroy(IntPtr geom)
855 {
856 NTotalGeoms--;
857 GeomiDestroy(geom);
858 }
859
860
861 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity]
862 public static extern void GeomDisable(IntPtr geom);
863
864 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity]
865 public static extern void GeomEnable(IntPtr geom);
866
867 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
868 public static extern void GeomGetAABB(IntPtr geom, out AABB aabb);
869
870 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity]
871 public static extern void GeomGetAABB(IntPtr geom, out dReal minX);
872
873 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity]
874 public static extern IntPtr GeomGetBody(IntPtr geom);
875
876 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity]
877 public static extern int GeomGetCategoryBits(IntPtr geom);
878
879 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity]
880 public static extern IntPtr GeomGetClassData(IntPtr geom);
881
882 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity]
883 public static extern int GeomGetCollideBits(IntPtr geom);
884
885 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity]
886 public static extern GeomClassID GeomGetClass(IntPtr geom);
887
888 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity]
889 public static extern IntPtr GeomGetData(IntPtr geom);
890
891 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity]
892 public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom);
893 public static Vector3 GeomGetOffsetPosition(IntPtr geom)
894 {
895 unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); }
896 }
897
898 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity]
899 public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom);
900 public static Matrix3 GeomGetOffsetRotation(IntPtr geom)
901 {
902 unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); }
903 }
904
905 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity]
906 public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom);
907 public static Vector3 GeomGetPosition(IntPtr geom)
908 {
909 unsafe { return *(GeomGetPositionUnsafe(geom)); }
910 }
911
912 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
913 public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q);
914
915 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity]
916 public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X);
917
918 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity]
919 public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom);
920 public static Matrix3 GeomGetRotation(IntPtr geom)
921 {
922 unsafe { return *(GeomGetRotationUnsafe(geom)); }
923 }
924
925 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity]
926 public static extern IntPtr GeomGetSpace(IntPtr geom);
927
928 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
929 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData,
930 dReal width, dReal depth, int widthSamples, int depthSamples,
931 dReal scale, dReal offset, dReal thickness, int bWrap);
932
933 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity]
934 public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
935 dReal width, dReal depth, int widthSamples, int depthSamples,
936 dReal scale, dReal offset, dReal thickness, int bWrap);
937
938 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity]
939 public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback,
940 dReal width, dReal depth, int widthSamples, int depthSamples,
941 dReal scale, dReal offset, dReal thickness, int bWrap);
942
943 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
944 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData,
945 dReal width, dReal depth, int widthSamples, int depthSamples,
946 dReal scale, dReal offset, dReal thickness, int bWrap);
947
948 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
949 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData,
950 dReal width, dReal depth, int widthSamples, int depthSamples,
951 dReal scale, dReal offset, dReal thickness, int bWrap);
952
953 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity]
954 public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
955 dReal width, dReal depth, int widthSamples, int depthSamples,
956 dReal scale, dReal offset, dReal thickness, int bWrap);
957
958 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
959 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData,
960 dReal width, dReal depth, int widthSamples, int depthSamples,
961 dReal scale, dReal offset, dReal thickness, int bWrap);
962
963 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity]
964 public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
965 dReal width, dReal depth, int widthSamples, int depthSamples,
966 dReal scale, dReal offset, dReal thickness, int bWrap);
967
968 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
969 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData,
970 dReal width, dReal depth, int widthSamples, int depthSamples,
971 dReal scale, dReal offset, dReal thickness, int bWrap);
972
973 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity]
974 public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData,
975 dReal width, dReal depth, int widthSamples, int depthSamples,
976 dReal scale, dReal offset, dReal thickness, int bWrap);
977
978 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity]
979 public static extern IntPtr GeomHeightfieldDataCreate();
980
981 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity]
982 public static extern void GeomHeightfieldDataDestroy(IntPtr d);
983
984 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity]
985 public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight);
986
987 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity]
988 public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g);
989
990 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity]
991 public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d);
992
993 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity]
994 public static extern bool GeomIsEnabled(IntPtr geom);
995
996 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity]
997 public static extern bool GeomIsOffset(IntPtr geom);
998
999 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity]
1000 public static extern bool GeomIsSpace(IntPtr geom);
1001
1002 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1003 public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result);
1004
1005 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity]
1006 public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A);
1007
1008 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity]
1009 public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1010
1011 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity]
1012 public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d);
1013
1014 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1015 public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir);
1016
1017 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity]
1018 public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX);
1019
1020 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity]
1021 public static extern int GeomRayGetClosestHit(IntPtr ray);
1022
1023 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity]
1024 public static extern dReal GeomRayGetLength(IntPtr ray);
1025
1026 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity]
1027 public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull);
1028
1029 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity]
1030 public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz);
1031
1032 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity]
1033 public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit);
1034
1035 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity]
1036 public static extern void GeomRaySetLength(IntPtr ray, dReal length);
1037
1038 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity]
1039 public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull);
1040
1041 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity]
1042 public static extern void GeomSetBody(IntPtr geom, IntPtr body);
1043
1044 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity]
1045 public static extern void GeomSetCategoryBits(IntPtr geom, int bits);
1046
1047 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity]
1048 public static extern void GeomSetCollideBits(IntPtr geom, int bits);
1049
1050 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity]
1051 public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons);
1052
1053 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity]
1054 public static extern void GeomSetData(IntPtr geom, IntPtr data);
1055
1056 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity]
1057 public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1058
1059 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1060 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q);
1061
1062 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity]
1063 public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X);
1064
1065 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1066 public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R);
1067
1068 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity]
1069 public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00);
1070
1071 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity]
1072 public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z);
1073
1074 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1075 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q);
1076
1077 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity]
1078 public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X);
1079
1080 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1081 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R);
1082
1083 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity]
1084 public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00);
1085
1086 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity]
1087 public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z);
1088
1089 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1090 public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat);
1091
1092 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity]
1093 public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w);
1094
1095 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1096 public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R);
1097
1098 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity]
1099 public static extern void GeomSetRotation(IntPtr geom, ref dReal M00);
1100
1101 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity]
1102 public static extern dReal GeomSphereGetRadius(IntPtr geom);
1103
1104 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity]
1105 public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z);
1106
1107 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity]
1108 public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius);
1109
1110 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity]
1111 public static extern int GeomTransformGetCleanup(IntPtr geom);
1112
1113 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity]
1114 public static extern IntPtr GeomTransformGetGeom(IntPtr geom);
1115
1116 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity]
1117 public static extern int GeomTransformGetInfo(IntPtr geom);
1118
1119 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity]
1120 public static extern void GeomTransformSetCleanup(IntPtr geom, int mode);
1121
1122 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity]
1123 public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj);
1124
1125 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity]
1126 public static extern void GeomTransformSetInfo(IntPtr geom, int info);
1127
1128 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1129 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1130 double[] vertices, int vertexStride, int vertexCount,
1131 int[] indices, int indexCount, int triStride);
1132
1133 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity]
1134 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1135 IntPtr vertices, int vertexStride, int vertexCount,
1136 IntPtr indices, int indexCount, int triStride);
1137
1138 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1139 public static extern void GeomTriMeshDataBuildDouble1(IntPtr d,
1140 double[] vertices, int vertexStride, int vertexCount,
1141 int[] indices, int indexCount, int triStride,
1142 double[] normals);
1143
1144 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity]
1145 public static extern void GeomTriMeshDataBuildDouble(IntPtr d,
1146 IntPtr vertices, int vertexStride, int vertexCount,
1147 IntPtr indices, int indexCount, int triStride,
1148 IntPtr normals);
1149
1150 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1151 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1152 dReal[] vertices, int vertexStride, int vertexCount,
1153 int[] indices, int indexCount, int triStride);
1154
1155 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity]
1156 public static extern void GeomTriMeshDataBuildSingle(IntPtr d,
1157 IntPtr vertices, int vertexStride, int vertexCount,
1158 IntPtr indices, int indexCount, int triStride);
1159
1160 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1161 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1162 dReal[] vertices, int vertexStride, int vertexCount,
1163 int[] indices, int indexCount, int triStride,
1164 dReal[] normals);
1165
1166 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity]
1167 public static extern void GeomTriMeshDataBuildSingle1(IntPtr d,
1168 IntPtr vertices, int vertexStride, int vertexCount,
1169 IntPtr indices, int indexCount, int triStride,
1170 IntPtr normals);
1171
1172 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1173 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1174 float[] vertices, int vertexStride, int vertexCount,
1175 int[] indices, int indexCount, int triStride);
1176
1177 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity]
1178 public static extern void GeomTriMeshDataBuildSimple(IntPtr d,
1179 IntPtr vertices, int vertexStride, int vertexCount,
1180 IntPtr indices, int indexCount, int triStride);
1181
1182 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1183 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1184 float[] vertices, int vertexStride, int vertexCount,
1185 int[] indices, int indexCount, int triStride,
1186 float[] normals);
1187
1188 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity]
1189 public static extern void GeomTriMeshDataBuildSimple1(IntPtr d,
1190 IntPtr vertices, int vertexStride, int vertexCount,
1191 IntPtr indices, int indexCount, int triStride,
1192 IntPtr normals);
1193
1194 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity]
1195 public static extern void GeomTriMeshClearTCCache(IntPtr g);
1196
1197 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity]
1198 public static extern IntPtr GeomTriMeshDataCreate();
1199
1200 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity]
1201 public static extern void GeomTriMeshDataDestroy(IntPtr d);
1202
1203 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity]
1204 public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id);
1205
1206 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity]
1207 public static extern void GeomTriMeshDataPreprocess(IntPtr d);
1208
1209 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity]
1210 public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data);
1211
1212 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity]
1213 public static extern void GeomTriMeshDataUpdate(IntPtr d);
1214
1215 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity]
1216 public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable);
1217
1218 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity]
1219 public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g);
1220
1221 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity]
1222 public static extern TriCallback GeomTriMeshGetCallback(IntPtr g);
1223
1224 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity]
1225 public static extern IntPtr GeomTriMeshGetData(IntPtr g);
1226
1227 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity]
1228 public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom);
1229 public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom)
1230 {
1231 unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); }
1232 }
1233
1234 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity]
1235 public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec);
1236
1237 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity]
1238 public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g);
1239
1240 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity]
1241 public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2);
1242
1243 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity]
1244 public extern static int GeomTriMeshGetTriangleCount(IntPtr g);
1245
1246 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity]
1247 public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g);
1248
1249 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity]
1250 public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass);
1251
1252 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity]
1253 public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback);
1254
1255 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity]
1256 public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback);
1257
1258 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity]
1259 public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data);
1260
1261 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1262 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans);
1263
1264 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity]
1265 public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00);
1266
1267 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity]
1268 public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback);
1269
1270 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity]
1271 public static extern string GetConfiguration(string str);
1272
1273 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity]
1274 public static extern IntPtr HashSpaceCreate(IntPtr space);
1275
1276 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity]
1277 public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel);
1278
1279 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity]
1280 public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel);
1281
1282 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity]
1283 public static extern void InfiniteAABB(IntPtr geom, out AABB aabb);
1284
1285 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity]
1286 public static extern void InitODE();
1287
1288 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity]
1289 public static extern int InitODE2(uint ODEInitFlags);
1290
1291 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity]
1292 public static extern int IsPositiveDefinite(ref dReal A, int n);
1293
1294 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity]
1295 public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n);
1296
1297 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity]
1298 public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3);
1299
1300 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity]
1301 public static extern void JointAddHingeTorque(IntPtr joint, dReal torque);
1302
1303 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity]
1304 public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2);
1305
1306 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity]
1307 public static extern void JointAddPRTorque(IntPtr joint, dReal torque);
1308
1309 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity]
1310 public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2);
1311
1312 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity]
1313 public static extern void JointAddSliderForce(IntPtr joint, dReal force);
1314
1315 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity]
1316 public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2);
1317
1318 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity]
1319 public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group);
1320
1321 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity]
1322 public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group);
1323
1324 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1325 public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact);
1326 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity]
1327 public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact);
1328
1329 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity]
1330 public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group);
1331
1332 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity]
1333 public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group);
1334
1335 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity]
1336 public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group);
1337
1338 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity]
1339 public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group);
1340
1341 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity]
1342 public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group);
1343
1344 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity]
1345 public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group);
1346
1347 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity]
1348 public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group);
1349
1350 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity]
1351 public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group);
1352
1353 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity]
1354 public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group);
1355
1356 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity]
1357 public static extern void JointDestroy(IntPtr j);
1358
1359 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1360 public static extern dReal JointGetAMotorAngle(IntPtr j, int anum);
1361
1362 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity]
1363 public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum);
1364
1365 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1366 public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result);
1367
1368 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity]
1369 public static extern int JointGetAMotorAxisRel(IntPtr j, int anum);
1370
1371 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity]
1372 public static extern int JointGetAMotorMode(IntPtr j);
1373
1374 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1375 public static extern int JointGetAMotorNumAxes(IntPtr j);
1376
1377 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity]
1378 public static extern dReal JointGetAMotorParam(IntPtr j, int parameter);
1379
1380 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity]
1381 public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result);
1382
1383 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1384 public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result);
1385
1386 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity]
1387 public static extern IntPtr JointGetBody(IntPtr j);
1388
1389 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity]
1390 public static extern IntPtr JointGetData(IntPtr j);
1391
1392 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity]
1393 public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j);
1394 public static JointFeedback JointGetFeedback(IntPtr j)
1395 {
1396 unsafe { return *(JointGetFeedbackUnsafe(j)); }
1397 }
1398
1399 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1400 public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result);
1401
1402 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity]
1403 public static extern dReal JointGetHingeAngle(IntPtr j);
1404
1405 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity]
1406 public static extern dReal JointGetHingeAngleRate(IntPtr j);
1407
1408 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity]
1409 public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result);
1410
1411 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity]
1412 public static extern dReal JointGetHingeParam(IntPtr j, int parameter);
1413
1414 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity]
1415 public static extern dReal JointGetHinge2Angle1(IntPtr j);
1416
1417 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity]
1418 public static extern dReal JointGetHinge2Angle1Rate(IntPtr j);
1419
1420 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity]
1421 public static extern dReal JointGetHinge2Angle2Rate(IntPtr j);
1422
1423 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity]
1424 public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result);
1425
1426 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1427 public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result);
1428
1429 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity]
1430 public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result);
1431
1432 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1433 public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result);
1434
1435 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1436 public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result);
1437
1438 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity]
1439 public static extern dReal JointGetHinge2Param(IntPtr j, int parameter);
1440
1441 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1442 public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result);
1443
1444 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1445 public static extern int JointGetLMotorNumAxes(IntPtr j);
1446
1447 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity]
1448 public static extern dReal JointGetLMotorParam(IntPtr j, int parameter);
1449
1450 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity]
1451 public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result);
1452
1453 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity]
1454 public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result);
1455
1456 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity]
1457 public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result);
1458
1459 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity]
1460 public static extern dReal JointGetPRParam(IntPtr j, int parameter);
1461
1462 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity]
1463 public static extern dReal JointGetPRPosition(IntPtr j);
1464
1465 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity]
1466 public static extern dReal JointGetPRPositionRate(IntPtr j);
1467
1468 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity]
1469 public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result);
1470
1471 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity]
1472 public static extern dReal JointGetSliderParam(IntPtr j, int parameter);
1473
1474 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity]
1475 public static extern dReal JointGetSliderPosition(IntPtr j);
1476
1477 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity]
1478 public static extern dReal JointGetSliderPositionRate(IntPtr j);
1479
1480 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity]
1481 public static extern JointType JointGetType(IntPtr j);
1482
1483 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1484 public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result);
1485
1486 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity]
1487 public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result);
1488
1489 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity]
1490 public static extern dReal JointGetUniversalAngle1(IntPtr j);
1491
1492 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity]
1493 public static extern dReal JointGetUniversalAngle1Rate(IntPtr j);
1494
1495 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity]
1496 public static extern dReal JointGetUniversalAngle2(IntPtr j);
1497
1498 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity]
1499 public static extern dReal JointGetUniversalAngle2Rate(IntPtr j);
1500
1501 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity]
1502 public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2);
1503
1504 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1505 public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result);
1506
1507 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1508 public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result);
1509
1510 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity]
1511 public static extern dReal JointGetUniversalParam(IntPtr j, int parameter);
1512
1513 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity]
1514 public static extern IntPtr JointGroupCreate(int max_size);
1515
1516 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity]
1517 public static extern void JointGroupDestroy(IntPtr group);
1518
1519 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity]
1520 public static extern void JointGroupEmpty(IntPtr group);
1521
1522 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity]
1523 public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle);
1524
1525 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity]
1526 public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1527
1528 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity]
1529 public static extern void JointSetAMotorMode(IntPtr j, int mode);
1530
1531 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1532 public static extern void JointSetAMotorNumAxes(IntPtr group, int num);
1533
1534 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity]
1535 public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value);
1536
1537 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity]
1538 public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z);
1539
1540 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity]
1541 public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z);
1542
1543 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity]
1544 public static extern void JointSetData(IntPtr j, IntPtr data);
1545
1546 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity]
1547 public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback);
1548
1549 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity]
1550 public static extern void JointSetFixed(IntPtr j);
1551
1552 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity]
1553 public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z);
1554
1555 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity]
1556 public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1557
1558 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity]
1559 public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z);
1560
1561 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity]
1562 public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value);
1563
1564 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity]
1565 public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z);
1566
1567 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity]
1568 public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z);
1569
1570 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity]
1571 public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z);
1572
1573 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity]
1574 public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value);
1575
1576 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity]
1577 public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z);
1578
1579 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity]
1580 public static extern void JointSetLMotorNumAxes(IntPtr j, int num);
1581
1582 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity]
1583 public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value);
1584
1585 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity]
1586 public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value);
1587
1588 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity]
1589 public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value);
1590
1591 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity]
1592 public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value);
1593
1594 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity]
1595 public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z);
1596
1597 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity]
1598 public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z);
1599
1600 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity]
1601 public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z);
1602
1603 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity]
1604 public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value);
1605
1606 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity]
1607 public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z);
1608
1609 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity]
1610 public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az);
1611
1612 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity]
1613 public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value);
1614
1615 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity]
1616 public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z);
1617
1618 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity]
1619 public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z);
1620
1621 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity]
1622 public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z);
1623
1624 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity]
1625 public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value);
1626
1627 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity]
1628 public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip);
1629
1630 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity]
1631 public static extern void MassAdd(ref Mass a, ref Mass b);
1632
1633 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity]
1634 public static extern void MassAdjust(ref Mass m, dReal newmass);
1635
1636 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity]
1637 public static extern bool MassCheck(ref Mass m);
1638
1639 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1640 public static extern void MassRotate(ref Mass mass, ref Matrix3 R);
1641
1642 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity]
1643 public static extern void MassRotate(ref Mass mass, ref dReal M00);
1644
1645 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity]
1646 public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz);
1647
1648 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity]
1649 public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz);
1650
1651 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity]
1652 public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1653
1654 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity]
1655 public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1656
1657 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity]
1658 public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length);
1659
1660 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity]
1661 public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length);
1662
1663 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity]
1664 public static extern void MassSetParameters(out Mass mass, dReal themass,
1665 dReal cgx, dReal cgy, dReal cgz,
1666 dReal i11, dReal i22, dReal i33,
1667 dReal i12, dReal i13, dReal i23);
1668
1669 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity]
1670 public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius);
1671
1672 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity]
1673 public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius);
1674
1675 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity]
1676 public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g);
1677
1678 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity]
1679 public static extern void MassSetZero(out Mass mass);
1680
1681 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity]
1682 public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z);
1683
1684 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1685 public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1686
1687 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity]
1688 private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r);
1689 public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector)
1690 {
1691 MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1);
1692 }
1693
1694 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity]
1695 public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1696
1697 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity]
1698 public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r);
1699
1700 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1701 public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle);
1702
1703 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity]
1704 public static extern void QfromR(out Quaternion q, ref Matrix3 R);
1705
1706 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity]
1707 public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1708
1709 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity]
1710 public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1711
1712 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity]
1713 public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1714
1715 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity]
1716 public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc);
1717
1718 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity]
1719 public static extern void QSetIdentity(out Quaternion q);
1720
1721 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1722 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth);
1723
1724 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity]
1725 public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth);
1726
1727 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity]
1728 public static extern dReal RandReal();
1729
1730 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity]
1731 public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz);
1732
1733 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity]
1734 public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle);
1735
1736 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity]
1737 public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi);
1738
1739 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity]
1740 public static extern void RfromQ(out Matrix3 R, ref Quaternion q);
1741
1742 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity]
1743 public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az);
1744
1745 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity]
1746 public static extern void RSetIdentity(out Matrix3 R);
1747
1748 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity]
1749 public static extern void SetValue(out dReal a, int n);
1750
1751 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity]
1752 public static extern void SetZero(out dReal a, int n);
1753
1754 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity]
1755 public static extern IntPtr SimpleSpaceCreate(IntPtr space);
1756
1757 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity]
1758 public static extern void SolveCholesky(ref dReal L, out dReal b, int n);
1759
1760 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity]
1761 public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip);
1762
1763 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity]
1764 public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip);
1765
1766 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity]
1767 public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip);
1768
1769 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity]
1770 public static extern void SpaceAdd(IntPtr space, IntPtr geom);
1771
1772 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity]
1773 public static extern bool SpaceLockQuery(IntPtr space);
1774
1775 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity]
1776 public static extern void SpaceClean(IntPtr space);
1777
1778 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity]
1779 public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback);
1780
1781 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity]
1782 public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback);
1783
1784 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity]
1785 public static extern void SpaceDestroy(IntPtr space);
1786
1787 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity]
1788 public static extern bool SpaceGetCleanup(IntPtr space);
1789
1790 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity]
1791 public static extern int SpaceGetNumGeoms(IntPtr space);
1792
1793 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity]
1794 public static extern IntPtr SpaceGetGeom(IntPtr space, int i);
1795
1796 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity]
1797 public static extern int SpaceGetSublevel(IntPtr space);
1798
1799 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity]
1800 public static extern bool SpaceQuery(IntPtr space, IntPtr geom);
1801
1802 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity]
1803 public static extern void SpaceRemove(IntPtr space, IntPtr geom);
1804
1805 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity]
1806 public static extern void SpaceSetCleanup(IntPtr space, bool mode);
1807
1808 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity]
1809 public static extern void SpaceSetSublevel(IntPtr space, int sublevel);
1810
1811 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity]
1812 public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder);
1813
1814 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity]
1815 public static extern void VectorScale(out dReal a, ref dReal d, int n);
1816
1817 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity]
1818 public static extern IntPtr WorldCreate();
1819
1820 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity]
1821 public static extern void WorldDestroy(IntPtr world);
1822
1823 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1824 public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world);
1825
1826 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1827 public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world);
1828
1829 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1830 public static extern bool WorldGetAutoDisableFlag(IntPtr world);
1831
1832 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1833 public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world);
1834
1835 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1836 public static extern int WorldGetAutoDisableSteps(IntPtr world);
1837
1838 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1839 public static extern dReal WorldGetAutoDisableTime(IntPtr world);
1840
1841 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1842 public static extern int WorldGetAutoEnableDepthSF1(IntPtr world);
1843
1844 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity]
1845 public static extern dReal WorldGetCFM(IntPtr world);
1846
1847 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity]
1848 public static extern dReal WorldGetERP(IntPtr world);
1849
1850 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1851 public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity);
1852
1853 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity]
1854 public static extern void WorldGetGravity(IntPtr world, out dReal X);
1855
1856 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1857 public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world);
1858
1859 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1860 public static extern dReal WorldGetContactSurfaceLayer(IntPtr world);
1861
1862 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity]
1863 public static extern dReal WorldGetAngularDamping(IntPtr world);
1864
1865 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1866 public static extern dReal WorldGetAngularDampingThreshold(IntPtr world);
1867
1868 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity]
1869 public static extern dReal WorldGetLinearDamping(IntPtr world);
1870
1871 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1872 public static extern dReal WorldGetLinearDampingThreshold(IntPtr world);
1873
1874 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1875 public static extern int WorldGetQuickStepNumIterations(IntPtr world);
1876
1877 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity]
1878 public static extern dReal WorldGetQuickStepW(IntPtr world);
1879
1880 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1881 public static extern dReal WorldGetMaxAngularSpeed(IntPtr world);
1882
1883 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1884 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force);
1885
1886 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity]
1887 public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX);
1888
1889 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity]
1890 public static extern void WorldQuickStep(IntPtr world, dReal stepsize);
1891
1892 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity]
1893 public static extern void WorldSetAngularDamping(IntPtr world, dReal scale);
1894
1895 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity]
1896 public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold);
1897
1898 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity]
1899 public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold);
1900
1901 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity]
1902 public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count);
1903
1904 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity]
1905 public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable);
1906
1907 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity]
1908 public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold);
1909
1910 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity]
1911 public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps);
1912
1913 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity]
1914 public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time);
1915
1916 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity]
1917 public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth);
1918
1919 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity]
1920 public static extern void WorldSetCFM(IntPtr world, dReal cfm);
1921
1922 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity]
1923 public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel);
1924
1925 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity]
1926 public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth);
1927
1928 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity]
1929 public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale);
1930
1931 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity]
1932 public static extern void WorldSetERP(IntPtr world, dReal erp);
1933
1934 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity]
1935 public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z);
1936
1937 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity]
1938 public static extern void WorldSetLinearDamping(IntPtr world, dReal scale);
1939
1940 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity]
1941 public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold);
1942
1943 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity]
1944 public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num);
1945
1946 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity]
1947 public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation);
1948
1949 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity]
1950 public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed);
1951
1952 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity]
1953 public static extern void WorldStep(IntPtr world, dReal stepsize);
1954
1955 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity]
1956 public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations);
1957
1958 [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity]
1959 public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix);
1960 }
1961}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
new file mode 100644
index 0000000..215d47a
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs
@@ -0,0 +1,86 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Threading;
33using System.IO;
34using System.Diagnostics;
35using log4net;
36using Nini.Config;
37using OdeAPI;
38using OpenSim.Framework;
39using OpenSim.Region.Physics.Manager;
40using OpenMetaverse;
41
42namespace OpenSim.Region.Physics.OdePlugin
43{
44 /// <summary>
45 /// ODE plugin
46 /// </summary>
47 public class OdePlugin : IPhysicsPlugin
48 {
49 //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
50
51 private OdeScene m_scene;
52
53 public bool Init()
54 {
55 if (d.InitODE2(0) != 0)
56 {
57 if (d.AllocateODEDataForThread(~0U) == 0)
58 {
59 d.CloseODE();
60 return false;
61 }
62 return true;
63 }
64 return false;
65 }
66
67 public PhysicsScene GetScene(String sceneIdentifier)
68 {
69 if (m_scene == null)
70 {
71 m_scene = new OdeScene(sceneIdentifier);
72 }
73 return (m_scene);
74 }
75
76 public string GetName()
77 {
78 return ("UbitODE");
79 }
80
81 public void Dispose()
82 {
83 d.CloseODE();
84 }
85 }
86} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
new file mode 100644
index 0000000..56f3786
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2540 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28//#define SPAM
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using OdeAPI;
40using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43
44namespace OpenSim.Region.Physics.OdePlugin
45{
46 public enum StatusIndicators : int
47 {
48 Generic = 0,
49 Start = 1,
50 End = 2
51 }
52
53 public struct sCollisionData
54 {
55 public uint ColliderLocalId;
56 public uint CollidedWithLocalId;
57 public int NumberOfCollisions;
58 public int CollisionType;
59 public int StatusIndicator;
60 public int lastframe;
61 }
62
63 [Flags]
64 public enum CollisionCategories : int
65 {
66 Disabled = 0,
67 Geom = 0x00000001,
68 Body = 0x00000002,
69 Space = 0x00000004,
70 Character = 0x00000008,
71 Land = 0x00000010,
72 Water = 0x00000020,
73 Wind = 0x00000040,
74 Sensor = 0x00000080,
75 Selected = 0x00000100
76 }
77
78 /// <summary>
79 /// Material type for a primitive
80 /// </summary>
81 public enum Material : int
82 {
83 /// <summary></summary>
84 Stone = 0,
85 /// <summary></summary>
86 Metal = 1,
87 /// <summary></summary>
88 Glass = 2,
89 /// <summary></summary>
90 Wood = 3,
91 /// <summary></summary>
92 Flesh = 4,
93 /// <summary></summary>
94 Plastic = 5,
95 /// <summary></summary>
96 Rubber = 6,
97
98 light = 7 // compatibility with old viewers
99 }
100
101 public enum changes : int
102 {
103 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
104 Remove,
105 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
106 // or removes from a object if arg is null
107 DeLink,
108 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
109 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
110 PosOffset, // not in use
111 // arg Vector3 new position in local coords. Changes prim position in object
112 OriOffset, // not in use
113 // arg Vector3 new position in local coords. Changes prim position in object
114 Velocity,
115 AngVelocity,
116 Acceleration,
117 Force,
118 Torque,
119
120 AddForce,
121 AddAngForce,
122 AngLock,
123
124 Size,
125 Shape,
126
127 CollidesWater,
128 VolumeDtc,
129
130 Physical,
131 Selected,
132 disabled,
133 building,
134
135 VehicleType,
136 VehicleFloatParam,
137 VehicleVectorParam,
138 VehicleRotationParam,
139 VehicleFlags,
140
141 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
142 }
143
144 public struct ODEchangeitem
145 {
146 public PhysicsActor actor;
147 public OdeCharacter character;
148 public changes what;
149 public Object arg;
150 }
151
152 public class OdeScene : PhysicsScene
153 {
154 private readonly ILog m_log;
155 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
156
157 private int threadid = 0;
158 private Random fluidRandomizer = new Random(Environment.TickCount);
159
160 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
161 const float comumContactERP = 0.6f;
162 const float comumSoftContactERP = 0.1f;
163 const float comumContactCFM = 0.0001f;
164
165 float frictionScale = 1.0f;
166
167 float frictionMovementMult = 0.3f;
168
169 float TerrainBounce = 0.3f;
170 float TerrainFriction = 0.3f;
171
172 public float AvatarBounce = 0.3f;
173 public float AvatarFriction = 0;// 0.9f * 0.5f;
174
175 private const uint m_regionWidth = Constants.RegionSize;
176 private const uint m_regionHeight = Constants.RegionSize;
177
178 public float ODE_STEPSIZE = 0.020f;
179 private float metersInSpace = 25.6f;
180 private float m_timeDilation = 1.0f;
181
182 public float gravityx = 0f;
183 public float gravityy = 0f;
184 public float gravityz = -9.8f;
185
186 private float waterlevel = 0f;
187 private int framecount = 0;
188
189 internal IntPtr WaterGeom;
190
191 public float avPIDD = 3200f; // make it visible
192 public float avPIDP = 1400f; // make it visible
193 private float avCapRadius = 0.37f;
194 private float avDensity = 3f;
195 private float avMovementDivisorWalk = 1.3f;
196 private float avMovementDivisorRun = 0.8f;
197 private float minimumGroundFlightOffset = 3f;
198 public float maximumMassObject = 10000.01f;
199
200 public bool meshSculptedPrim = true;
201 public bool forceSimplePrimMeshing = false;
202
203 public float meshSculptLOD = 32;
204 public float MeshSculptphysicalLOD = 16;
205
206 public float geomDefaultDensity = 10.000006836f;
207
208 public int geomContactPointsStartthrottle = 3;
209 public int geomUpdatesPerThrottledUpdate = 15;
210
211 public float bodyPIDD = 35f;
212 public float bodyPIDG = 25;
213
214 public int geomCrossingFailuresBeforeOutofbounds = 6;
215
216 public int bodyFramesAutoDisable = 20;
217
218 private float[] _watermap;
219 private bool m_filterCollisions = true;
220
221 private d.NearCallback nearCallback;
222
223 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
224 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
225 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
226
227 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
228
229 /// <summary>
230 /// A list of actors that should receive collision events.
231 /// </summary>
232 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
233
234 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
235 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
236 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
237
238 private float contactsurfacelayer = 0.002f;
239
240 private int contactsPerCollision = 80;
241 internal IntPtr ContactgeomsArray = IntPtr.Zero;
242 private IntPtr GlobalContactsArray = IntPtr.Zero;
243
244 const int maxContactsbeforedeath = 4000;
245 private volatile int m_global_contactcount = 0;
246
247
248 private readonly IntPtr contactgroup;
249
250 public ContactData[] m_materialContactsData = new ContactData[8];
251
252 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
253 private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
254 private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
255
256 private int m_physicsiterations = 10;
257 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
258 private readonly PhysicsActor PANull = new NullPhysicsActor();
259 private float step_time = 0.0f;
260
261 public IntPtr world;
262
263 private uint obj2LocalID = 0;
264 private OdeCharacter cc1;
265 private OdePrim cp1;
266 private OdeCharacter cc2;
267 private OdePrim cp2;
268
269 // split the spaces acording to contents type
270 // ActiveSpace contains characters and active prims
271 // StaticSpace contains land and other that is mostly static in enviroment
272 // this can contain subspaces, like the grid in staticspace
273 // as now space only contains this 2 top spaces
274
275 public IntPtr TopSpace; // the global space
276 public IntPtr ActiveSpace; // space for active prims
277 public IntPtr StaticSpace; // space for the static things around
278
279 // some speedup variables
280 private int spaceGridMaxX;
281 private int spaceGridMaxY;
282 private float spacesPerMeter;
283
284 // split static geometry collision into a grid as before
285 private IntPtr[,] staticPrimspace;
286
287 private Object OdeLock;
288 private static Object SimulationLock;
289
290 public IMesher mesher;
291
292 private IConfigSource m_config;
293
294 public bool physics_logging = false;
295 public int physics_logging_interval = 0;
296 public bool physics_logging_append_existing_logfile = false;
297
298 private Vector3 m_worldOffset = Vector3.Zero;
299 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
300 private PhysicsScene m_parentScene = null;
301
302 private ODERayCastRequestManager m_rayCastManager;
303
304
305/* maybe needed if ode uses tls
306 private void checkThread()
307 {
308
309 int th = Thread.CurrentThread.ManagedThreadId;
310 if(th != threadid)
311 {
312 threadid = th;
313 d.AllocateODEDataForThread(~0U);
314 }
315 }
316 */
317 /// <summary>
318 /// Initiailizes the scene
319 /// Sets many properties that ODE requires to be stable
320 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
321 /// </summary>
322 public OdeScene(string sceneIdentifier)
323 {
324 m_log
325 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
326
327// checkThread();
328 Name = sceneIdentifier;
329
330 OdeLock = new Object();
331 SimulationLock = new Object();
332
333 nearCallback = near;
334
335 m_rayCastManager = new ODERayCastRequestManager(this);
336 lock (OdeLock)
337 {
338 // Create the world and the first space
339 try
340 {
341 world = d.WorldCreate();
342 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
343
344 // now the major subspaces
345 ActiveSpace = d.HashSpaceCreate(TopSpace);
346 StaticSpace = d.HashSpaceCreate(TopSpace);
347 }
348 catch
349 {
350 // i must RtC#FM
351 }
352
353 d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does
354 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
355 d.HashSpaceSetLevels(StaticSpace, -2, 8);
356
357 // demote to second level
358 d.SpaceSetSublevel(ActiveSpace, 1);
359 d.SpaceSetSublevel(StaticSpace, 1);
360
361 contactgroup = d.JointGroupCreate(0);
362 //contactgroup
363
364 d.WorldSetAutoDisableFlag(world, false);
365 }
366
367 _watermap = new float[258 * 258];
368 }
369
370 // Initialize the mesh plugin
371// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
372 public override void Initialise(IMesher meshmerizer, IConfigSource config)
373 {
374// checkThread();
375 mesher = meshmerizer;
376 m_config = config;
377
378// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE"));
379 /*
380 if (region != null)
381 {
382 WorldExtents.X = region.RegionSizeX;
383 WorldExtents.Y = region.RegionSizeY;
384 }
385 */
386
387 // Defaults
388
389 avPIDD = 2200.0f;
390 avPIDP = 900.0f;
391
392 int contactsPerCollision = 80;
393
394 if (m_config != null)
395 {
396 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
397 if (physicsconfig != null)
398 {
399 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
400 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
401 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
402
403 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
404
405 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
406
407 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
408 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
409
410 avDensity = physicsconfig.GetFloat("av_density", avDensity);
411 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
412 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
413 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
414
415 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
416
417 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
418 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
419 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
420
421 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
422 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
423
424 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
425 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
426
427 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
428 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
429 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
430 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
431 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
432
433 if (Environment.OSVersion.Platform == PlatformID.Unix)
434 {
435 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
436 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
437 }
438 else
439 {
440 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
441 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
442 }
443
444 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
445 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
446 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
447
448 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
449 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
450 }
451 }
452
453 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
454 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
455
456 m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f;
457 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
458
459 m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f;
460 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
461
462 m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f;
463 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
464
465 m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f;
466 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
467
468 m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f;
469 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
470
471 m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f;
472 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
473
474 m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 0.9f;
475 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
476
477 m_materialContactsData[(int)Material.light].mu = 0.0f;
478 m_materialContactsData[(int)Material.light].bounce = 0.0f;
479
480 TerrainFriction *= frictionScale;
481// AvatarFriction *= frictionScale;
482
483 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
484
485 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
486 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
487
488 d.WorldSetLinearDamping(world, 0.001f);
489 d.WorldSetAngularDamping(world, 0.001f);
490 d.WorldSetAngularDampingThreshold(world, 0f);
491 d.WorldSetLinearDampingThreshold(world, 0f);
492 d.WorldSetMaxAngularSpeed(world, 256f);
493
494 d.WorldSetCFM(world,1e-6f); // a bit harder than default
495 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
496 d.WorldSetERP(world, 0.6f); // higher than original
497
498 // Set how many steps we go without running collision testing
499 // This is in addition to the step size.
500 // Essentially Steps * m_physicsiterations
501 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
502 d.WorldSetContactMaxCorrectingVel(world, 100.0f);
503
504 spacesPerMeter = 1 / metersInSpace;
505 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
506 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
507
508 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
509
510 // create all spaces now
511 int i, j;
512 IntPtr newspace;
513 for (i = 0; i < spaceGridMaxX; i++)
514 for (j = 0; j < spaceGridMaxY; j++)
515 {
516 newspace = d.HashSpaceCreate(StaticSpace);
517 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
518 waitForSpaceUnlock(newspace);
519 d.SpaceSetSublevel(newspace, 2);
520 d.HashSpaceSetLevels(newspace, -2, 8);
521 staticPrimspace[i, j] = newspace;
522 }
523 // let this now be real maximum values
524 spaceGridMaxX--;
525 spaceGridMaxY--;
526 }
527
528 internal void waitForSpaceUnlock(IntPtr space)
529 {
530 //if (space != IntPtr.Zero)
531 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
532 }
533
534 #region Collision Detection
535
536 // sets a global contact for a joint for contactgeom , and base contact description)
537
538 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp)
539 {
540 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
541 return IntPtr.Zero;
542
543 d.Contact newcontact = new d.Contact();
544 newcontact.geom.depth = contactGeom.depth;
545 newcontact.geom.g1 = contactGeom.g1;
546 newcontact.geom.g2 = contactGeom.g2;
547 newcontact.geom.pos = contactGeom.pos;
548 newcontact.geom.normal = contactGeom.normal;
549 newcontact.geom.side1 = contactGeom.side1;
550 newcontact.geom.side2 = contactGeom.side2;
551
552 // this needs bounce also
553 newcontact.surface.mode = comumContactFlags;
554 newcontact.surface.mu = mu;
555 newcontact.surface.bounce = bounce;
556 newcontact.surface.soft_cfm = comumContactCFM;
557 if (softerp)
558 newcontact.surface.soft_erp = comumSoftContactERP;
559 else
560 newcontact.surface.soft_erp = comumContactERP;
561
562 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
563 Marshal.StructureToPtr(newcontact, contact, true);
564 return d.JointCreateContactPtr(world, contactgroup, contact);
565 }
566
567
568 /// <summary>
569 /// This is our near callback. A geometry is near a body
570 /// </summary>
571 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
572 /// <param name="g1">a geometry or space</param>
573 /// <param name="g2">another geometry or space</param>
574 ///
575
576 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
577 {
578 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
579 return false;
580
581 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
582 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
583 return true;
584 }
585
586
587
588 private void near(IntPtr space, IntPtr g1, IntPtr g2)
589 {
590 // no lock here! It's invoked from within Simulate(), which is thread-locked
591
592 if (m_global_contactcount >= maxContactsbeforedeath)
593 return;
594
595 // Test if we're colliding a geom with a space.
596 // If so we have to drill down into the space recursively
597
598 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
599 return;
600
601 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
602 {
603 // We'll be calling near recursivly if one
604 // of them is a space to find all of the
605 // contact points in the space
606 try
607 {
608 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
609 }
610 catch (AccessViolationException)
611 {
612 m_log.Warn("[PHYSICS]: Unable to collide test a space");
613 return;
614 }
615 //here one should check collisions of geoms inside a space
616 // but on each space we only should have geoms that not colide amoung each other
617 // so we don't dig inside spaces
618 return;
619 }
620
621 // get geom bodies to check if we already a joint contact
622 // guess this shouldn't happen now
623 IntPtr b1 = d.GeomGetBody(g1);
624 IntPtr b2 = d.GeomGetBody(g2);
625
626 // d.GeomClassID id = d.GeomGetClass(g1);
627
628 // Figure out how many contact points we have
629 int count = 0;
630 try
631 {
632 // Colliding Geom To Geom
633 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
634
635 if (g1 == g2)
636 return; // Can't collide with yourself
637
638 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
639 return;
640
641 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
642 }
643 catch (SEHException)
644 {
645 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.");
646// ode.drelease(world);
647 base.TriggerPhysicsBasedRestart();
648 }
649 catch (Exception e)
650 {
651 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
652 return;
653 }
654
655 // id contacts done
656 if (count == 0)
657 return;
658
659 // try get physical actors
660 PhysicsActor p1;
661 PhysicsActor p2;
662
663 if (!actor_name_map.TryGetValue(g1, out p1))
664 {
665 p1 = PANull;
666 }
667
668 if (!actor_name_map.TryGetValue(g2, out p2))
669 {
670 p2 = PANull;
671 }
672
673 // update actors collision score
674 if (p1.CollisionScore >= float.MaxValue - count)
675 p1.CollisionScore = 0;
676 p1.CollisionScore += count;
677
678 if (p2.CollisionScore >= float.MaxValue - count)
679 p2.CollisionScore = 0;
680 p2.CollisionScore += count;
681
682
683 // get first contact
684 d.ContactGeom curContact = new d.ContactGeom();
685 if (!GetCurContactGeom(0, ref curContact))
686 return;
687 // for now it's the one with max depth
688 ContactPoint maxDepthContact = new ContactPoint(
689 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
690 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
691 curContact.depth
692 );
693 // do volume detection case
694 if (
695 (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) ||
696 (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
697 {
698 collision_accounting_events(p1, p2, maxDepthContact);
699 return;
700 }
701
702 // big messy collision analises
703 float mu = 0;
704 float bounce = 0;
705 ContactData contactdata1;
706 ContactData contactdata2;
707 bool erpSoft = false;
708
709 String name = null;
710 bool dop1foot = false;
711 bool dop2foot = false;
712 bool ignore = false;
713
714 switch (p1.PhysicsActorType)
715 {
716 case (int)ActorTypes.Agent:
717 switch (p2.PhysicsActorType)
718 {
719 case (int)ActorTypes.Agent:
720 contactdata1 = p1.ContactData;
721 contactdata2 = p2.ContactData;
722 bounce = contactdata1.bounce * contactdata2.bounce;
723
724 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
725
726 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
727 mu *= frictionMovementMult;
728
729 p1.CollidingObj = true;
730 p2.CollidingObj = true;
731 break;
732 case (int)ActorTypes.Prim:
733 contactdata1 = p1.ContactData;
734 contactdata2 = p2.ContactData;
735 bounce = contactdata1.bounce * contactdata2.bounce;
736
737 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
738
739 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
740 mu *= frictionMovementMult;
741 if (p2.Velocity.LengthSquared() > 0.0f)
742 p2.CollidingObj = true;
743 dop1foot = true;
744 break;
745 default:
746 ignore=true; // avatar to terrain and water ignored
747 break;
748 }
749 break;
750
751 case (int)ActorTypes.Prim:
752 switch (p2.PhysicsActorType)
753 {
754 case (int)ActorTypes.Agent:
755 contactdata1 = p1.ContactData;
756 contactdata2 = p2.ContactData;
757 bounce = contactdata1.bounce * contactdata2.bounce;
758
759 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
760
761 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
762 mu *= frictionMovementMult;
763
764 dop2foot = true;
765 if (p1.Velocity.LengthSquared() > 0.0f)
766 p1.CollidingObj = true;
767 break;
768 case (int)ActorTypes.Prim:
769 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
770 {
771 p1.CollidingObj = true;
772 p2.CollidingObj = true;
773 }
774 contactdata1 = p1.ContactData;
775 contactdata2 = p2.ContactData;
776 bounce = contactdata1.bounce * contactdata2.bounce;
777 erpSoft = true;
778 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
779
780 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
781 mu *= frictionMovementMult;
782
783 break;
784 default:
785 if (geom_name_map.TryGetValue(g2, out name))
786 {
787 if (name == "Terrain")
788 {
789 erpSoft = true;
790 contactdata1 = p1.ContactData;
791 bounce = contactdata1.bounce * TerrainBounce;
792 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
793 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
794 mu *= frictionMovementMult;
795 p1.CollidingGround = true;
796 }
797 else if (name == "Water")
798 {
799 erpSoft = true;
800 }
801 }
802 else
803 ignore=true;
804 break;
805 }
806 break;
807
808 default:
809 if (geom_name_map.TryGetValue(g1, out name))
810 {
811 if (name == "Terrain")
812 {
813 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
814 {
815 erpSoft = true;
816 p2.CollidingGround = true;
817 contactdata2 = p2.ContactData;
818 bounce = contactdata2.bounce * TerrainBounce;
819 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
820
821 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
822 mu *= frictionMovementMult;
823 }
824 else
825 ignore = true;
826
827 }
828 else if (name == "Water" &&
829 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
830 {
831 erpSoft = true;
832 }
833 }
834 else
835 ignore = true;
836 break;
837 }
838
839 if (ignore)
840 return;
841
842 IntPtr Joint;
843
844 int i = 0;
845 while(true)
846 {
847 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
848 p1.IsColliding = true;
849 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
850 p2.IsColliding = true;
851
852 Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft);
853 d.JointAttach(Joint, b1, b2);
854
855 if (++m_global_contactcount >= maxContactsbeforedeath)
856 break;
857
858 if(++i >= count)
859 break;
860
861 if (!GetCurContactGeom(i, ref curContact))
862 break;
863
864 if (curContact.depth > maxDepthContact.PenetrationDepth)
865 {
866 maxDepthContact.Position.X = curContact.pos.X;
867 maxDepthContact.Position.Y = curContact.pos.Y;
868 maxDepthContact.Position.Z = curContact.pos.Z;
869 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
870 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
871 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
872 maxDepthContact.PenetrationDepth = curContact.depth;
873 }
874 }
875
876 collision_accounting_events(p1, p2, maxDepthContact);
877
878/*
879 if (notskipedcount > geomContactPointsStartthrottle)
880 {
881 // If there are more then 3 contact points, it's likely
882 // that we've got a pile of objects, so ...
883 // We don't want to send out hundreds of terse updates over and over again
884 // so lets throttle them and send them again after it's somewhat sorted out.
885 this needs checking so out for now
886 if (b1 != IntPtr.Zero)
887 p1.ThrottleUpdates = true;
888 if (b2 != IntPtr.Zero)
889 p2.ThrottleUpdates = true;
890
891 }
892 */
893 }
894
895 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
896 {
897 // obj1LocalID = 0;
898 //returncollisions = false;
899 obj2LocalID = 0;
900 //ctype = 0;
901 //cStartStop = 0;
902 if (!(p2.SubscribedEvents() || p1.SubscribedEvents()))
903 return;
904
905 switch ((ActorTypes)p1.PhysicsActorType)
906 {
907 case ActorTypes.Agent:
908 cc1 = (OdeCharacter)p1;
909 switch ((ActorTypes)p2.PhysicsActorType)
910 {
911 case ActorTypes.Agent:
912 cc2 = (OdeCharacter)p2;
913 obj2LocalID = cc2.m_localID;
914 if (p2.SubscribedEvents())
915 cc2.AddCollisionEvent(cc1.m_localID, contact);
916 break;
917
918 case ActorTypes.Prim:
919 if (p2 is OdePrim)
920 {
921 cp2 = (OdePrim)p2;
922 obj2LocalID = cp2.m_localID;
923 if (p2.SubscribedEvents())
924 cp2.AddCollisionEvent(cc1.m_localID, contact);
925 }
926 break;
927
928 case ActorTypes.Ground:
929 case ActorTypes.Unknown:
930 default:
931 obj2LocalID = 0;
932 break;
933 }
934 if (p1.SubscribedEvents())
935 {
936 contact.SurfaceNormal = -contact.SurfaceNormal;
937 cc1.AddCollisionEvent(obj2LocalID, contact);
938 }
939 break;
940
941 case ActorTypes.Prim:
942
943 if (p1 is OdePrim)
944 {
945 cp1 = (OdePrim)p1;
946
947 // obj1LocalID = cp2.m_localID;
948 switch ((ActorTypes)p2.PhysicsActorType)
949 {
950 case ActorTypes.Agent:
951 if (p2 is OdeCharacter)
952 {
953 cc2 = (OdeCharacter)p2;
954 obj2LocalID = cc2.m_localID;
955 if (p2.SubscribedEvents())
956 cc2.AddCollisionEvent(cp1.m_localID, contact);
957 }
958 break;
959 case ActorTypes.Prim:
960
961 if (p2 is OdePrim)
962 {
963 cp2 = (OdePrim)p2;
964 obj2LocalID = cp2.m_localID;
965 if (p2.SubscribedEvents())
966 cp2.AddCollisionEvent(cp1.m_localID, contact);
967 }
968 break;
969
970 case ActorTypes.Ground:
971 case ActorTypes.Unknown:
972 default:
973 obj2LocalID = 0;
974 break;
975 }
976 if (p1.SubscribedEvents())
977 {
978 contact.SurfaceNormal = -contact.SurfaceNormal;
979 cp1.AddCollisionEvent(obj2LocalID, contact);
980 }
981 }
982 break;
983 }
984 }
985
986 /// <summary>
987 /// This is our collision testing routine in ODE
988 /// </summary>
989 /// <param name="timeStep"></param>
990 private void collision_optimized()
991 {
992// _perloopContact.Clear();
993// clear characts IsColliding until we do it some other way
994
995 lock (_characters)
996 {
997 foreach (OdeCharacter chr in _characters)
998 {
999 // this are odd checks if they are needed something is wrong elsewhere
1000 // keep for now
1001 if (chr == null)
1002 continue;
1003
1004 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1005 continue;
1006
1007 chr.IsColliding = false;
1008 // chr.CollidingGround = false; not done here
1009 chr.CollidingObj = false;
1010 }
1011 }
1012
1013 // now let ode do its job
1014 // colide active things amoung them
1015
1016 int st = Util.EnvironmentTickCount();
1017 int ta;
1018 int ts;
1019 try
1020 {
1021 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1022 }
1023 catch (AccessViolationException)
1024 {
1025 m_log.Warn("[PHYSICS]: Unable to Active space collide");
1026 }
1027 ta = Util.EnvironmentTickCountSubtract(st);
1028 // then active things with static enviroment
1029 try
1030 {
1031 d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback);
1032 }
1033 catch (AccessViolationException)
1034 {
1035 m_log.Warn("[PHYSICS]: Unable to Active to static space collide");
1036 }
1037 ts = Util.EnvironmentTickCountSubtract(st);
1038// _perloopContact.Clear();
1039 }
1040
1041 #endregion
1042
1043
1044 public float GetTerrainHeightAtXY(float x, float y)
1045 {
1046 // assumes 1m size grid and constante size square regions
1047 // region offset in mega position
1048
1049 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1050 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1051
1052 IntPtr heightFieldGeom = IntPtr.Zero;
1053
1054 // get region map
1055 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
1056 return 0f;
1057
1058 if (heightFieldGeom == IntPtr.Zero)
1059 return 0f;
1060
1061 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1062 return 0f;
1063
1064 // TerrainHeightField for ODE as offset 1m
1065 x += 1f - offsetX;
1066 y += 1f - offsetY;
1067
1068 // make position fit into array
1069 if (x < 0)
1070 x = 0;
1071 if (y < 0)
1072 y = 0;
1073
1074 // integer indexs
1075 int ix;
1076 int iy;
1077 // interpolators offset
1078 float dx;
1079 float dy;
1080
1081 int regsize = (int)Constants.RegionSize + 2; // map size see setterrain
1082
1083 // we still have square fixed size regions
1084 // also flip x and y because of how map is done for ODE fliped axis
1085 // so ix,iy,dx and dy are inter exchanged
1086 if (x < regsize - 1)
1087 {
1088 iy = (int)x;
1089 dy = x - (float)iy;
1090 }
1091 else // out world use external height
1092 {
1093 iy = regsize - 1;
1094 dy = 0;
1095 }
1096 if (y < regsize - 1)
1097 {
1098 ix = (int)y;
1099 dx = y - (float)ix;
1100 }
1101 else
1102 {
1103 ix = regsize - 1;
1104 dx = 0;
1105 }
1106
1107 float h0;
1108 float h1;
1109 float h2;
1110
1111 iy *= regsize;
1112 iy += ix; // all indexes have iy + ix
1113
1114 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
1115
1116 if ((dx + dy) <= 1.0f)
1117 {
1118 h0 = ((float)heights[iy]); // 0,0 vertice
1119 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
1120 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
1121 }
1122 else
1123 {
1124 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
1125 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
1126 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
1127 }
1128
1129 return h0 + h1 + h2;
1130 }
1131
1132 /// <summary>
1133 /// Add actor to the list that should receive collision events in the simulate loop.
1134 /// </summary>
1135 /// <param name="obj"></param>
1136 public void AddCollisionEventReporting(PhysicsActor obj)
1137 {
1138 lock (_collisionEventPrim)
1139 {
1140 if (!_collisionEventPrim.Contains(obj))
1141 _collisionEventPrim.Add(obj);
1142 }
1143 }
1144
1145 /// <summary>
1146 /// Remove actor from the list that should receive collision events in the simulate loop.
1147 /// </summary>
1148 /// <param name="obj"></param>
1149 public void RemoveCollisionEventReporting(PhysicsActor obj)
1150 {
1151 lock (_collisionEventPrim)
1152 {
1153 if (_collisionEventPrim.Contains(obj))
1154 _collisionEventPrim.Remove(obj);
1155 }
1156 }
1157
1158 #region Add/Remove Entities
1159
1160 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1161 {
1162 Vector3 pos;
1163 pos.X = position.X;
1164 pos.Y = position.Y;
1165 pos.Z = position.Z;
1166 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1167 newAv.Flying = isFlying;
1168 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1169
1170 return newAv;
1171 }
1172
1173 public void AddCharacter(OdeCharacter chr)
1174 {
1175 lock (_characters)
1176 {
1177 if (!_characters.Contains(chr))
1178 {
1179 _characters.Add(chr);
1180 if (chr.bad)
1181 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1182 }
1183 }
1184 }
1185
1186 public void RemoveCharacter(OdeCharacter chr)
1187 {
1188 lock (_characters)
1189 {
1190 if (_characters.Contains(chr))
1191 {
1192 _characters.Remove(chr);
1193 }
1194 }
1195 }
1196
1197 public void BadCharacter(OdeCharacter chr)
1198 {
1199 lock (_badCharacter)
1200 {
1201 if (!_badCharacter.Contains(chr))
1202 _badCharacter.Add(chr);
1203 }
1204 }
1205
1206 public override void RemoveAvatar(PhysicsActor actor)
1207 {
1208 //m_log.Debug("[PHYSICS]:ODELOCK");
1209 ((OdeCharacter) actor).Destroy();
1210 }
1211
1212 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1213 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1214 {
1215 Vector3 pos = position;
1216 Vector3 siz = size;
1217 Quaternion rot = rotation;
1218
1219 OdePrim newPrim;
1220 lock (OdeLock)
1221 {
1222 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
1223
1224 lock (_prims)
1225 _prims.Add(newPrim);
1226 }
1227 newPrim.LocalID = localID;
1228 return newPrim;
1229 }
1230
1231 public void addActivePrim(OdePrim activatePrim)
1232 {
1233 // adds active prim.. (ones that should be iterated over in collisions_optimized
1234 lock (_activeprims)
1235 {
1236 if (!_activeprims.Contains(activatePrim))
1237 _activeprims.Add(activatePrim);
1238 //else
1239 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1240 }
1241 }
1242
1243 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1244 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1245 {
1246#if SPAM
1247 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1248#endif
1249
1250 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1251 }
1252
1253 public override float TimeDilation
1254 {
1255 get { return m_timeDilation; }
1256 }
1257
1258 public override bool SupportsNINJAJoints
1259 {
1260 get { return false; }
1261 }
1262
1263
1264 public void remActivePrim(OdePrim deactivatePrim)
1265 {
1266 lock (_activeprims)
1267 {
1268 _activeprims.Remove(deactivatePrim);
1269 }
1270 }
1271
1272 public override void RemovePrim(PhysicsActor prim)
1273 {
1274 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1275 // removed in the next physics simulate pass.
1276 if (prim is OdePrim)
1277 {
1278// lock (OdeLock)
1279 {
1280 OdePrim p = (OdePrim)prim;
1281 p.setPrimForRemoval();
1282 }
1283 }
1284 }
1285 /// <summary>
1286 /// This is called from within simulate but outside the locked portion
1287 /// We need to do our own locking here
1288 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1289 /// Simulate() -- justincc).
1290 ///
1291 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1292 ///
1293 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1294 /// that the space was using.
1295 /// </summary>
1296 /// <param name="prim"></param>
1297 public void RemovePrimThreadLocked(OdePrim prim)
1298 {
1299 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1300 lock (prim)
1301 {
1302 RemoveCollisionEventReporting(prim);
1303 lock (_prims)
1304 _prims.Remove(prim);
1305 }
1306
1307 }
1308 #endregion
1309
1310 #region Space Separation Calculation
1311
1312 /// <summary>
1313 /// Called when a static prim moves or becomes static
1314 /// Places the prim in a space one the static sub-spaces grid
1315 /// </summary>
1316 /// <param name="geom">the pointer to the geom that moved</param>
1317 /// <param name="pos">the position that the geom moved to</param>
1318 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1319 /// <returns>a pointer to the new space it's in</returns>
1320 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1321 {
1322 // moves a prim into another static sub-space or from another space into a static sub-space
1323
1324 // Called ODEPrim so
1325 // it's already in locked space.
1326
1327 if (geom == IntPtr.Zero) // shouldn't happen
1328 return IntPtr.Zero;
1329
1330 // get the static sub-space for current position
1331 IntPtr newspace = calculateSpaceForGeom(pos);
1332
1333 if (newspace == currentspace) // if we are there all done
1334 return newspace;
1335
1336 // else remove it from its current space
1337 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1338 {
1339 if (d.GeomIsSpace(currentspace))
1340 {
1341 waitForSpaceUnlock(currentspace);
1342 d.SpaceRemove(currentspace, geom);
1343 }
1344 else
1345 {
1346 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1347 " Geom:" + geom);
1348 }
1349 }
1350 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1351 {
1352 currentspace = d.GeomGetSpace(geom);
1353 if (currentspace != IntPtr.Zero)
1354 {
1355 if (d.GeomIsSpace(currentspace))
1356 {
1357 waitForSpaceUnlock(currentspace);
1358 d.SpaceRemove(currentspace, geom);
1359 }
1360 }
1361 }
1362
1363 // put the geom in the newspace
1364 waitForSpaceUnlock(newspace);
1365 d.SpaceAdd(newspace, geom);
1366
1367 // let caller know this newspace
1368 return newspace;
1369 }
1370
1371 /// <summary>
1372 /// Calculates the space the prim should be in by its position
1373 /// </summary>
1374 /// <param name="pos"></param>
1375 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1376 public IntPtr calculateSpaceForGeom(Vector3 pos)
1377 {
1378 int x, y;
1379 x = (int)(pos.X * spacesPerMeter);
1380 if (x < 0)
1381 x = 0;
1382 else if (x > spaceGridMaxX)
1383 x = spaceGridMaxX;
1384
1385 y = (int)(pos.Y * spacesPerMeter);
1386 if (y < 0)
1387 y = 0;
1388 else if (y >spaceGridMaxY)
1389 y = spaceGridMaxY;
1390
1391 IntPtr tmpSpace = staticPrimspace[x, y];
1392 return tmpSpace;
1393 }
1394
1395 #endregion
1396
1397 /// <summary>
1398 /// Routine to figure out if we need to mesh this prim with our mesher
1399 /// </summary>
1400 /// <param name="pbs"></param>
1401 /// <returns></returns>
1402 public bool needsMeshing(PrimitiveBaseShape pbs)
1403 {
1404 // most of this is redundant now as the mesher will return null if it cant mesh a prim
1405 // but we still need to check for sculptie meshing being enabled so this is the most
1406 // convenient place to do it for now...
1407
1408 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
1409 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
1410 int iPropertiesNotSupportedDefault = 0;
1411
1412 if (pbs.SculptEntry)
1413 {
1414 if(!meshSculptedPrim)
1415 return false;
1416 }
1417
1418 // 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
1419 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
1420 {
1421 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1422 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1423 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1424 {
1425
1426 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1427 && pbs.ProfileHollow == 0
1428 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1429 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1430 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1431 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1432 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1433 {
1434#if SPAM
1435 m_log.Warn("NonMesh");
1436#endif
1437 return false;
1438 }
1439 }
1440 }
1441
1442 // following code doesn't give meshs to boxes and spheres ever
1443 // and it's odd.. so for now just return true if asked to force meshs
1444 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1445
1446 if (forceSimplePrimMeshing)
1447 return true;
1448
1449 if (pbs.ProfileHollow != 0)
1450 iPropertiesNotSupportedDefault++;
1451
1452 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1453 iPropertiesNotSupportedDefault++;
1454
1455 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1456 iPropertiesNotSupportedDefault++;
1457
1458 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1459 iPropertiesNotSupportedDefault++;
1460
1461 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1462 iPropertiesNotSupportedDefault++;
1463
1464 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1465 iPropertiesNotSupportedDefault++;
1466
1467 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1468 iPropertiesNotSupportedDefault++;
1469
1470 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))
1471 iPropertiesNotSupportedDefault++;
1472
1473 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1474 iPropertiesNotSupportedDefault++;
1475
1476 // test for torus
1477 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1478 {
1479 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1480 {
1481 iPropertiesNotSupportedDefault++;
1482 }
1483 }
1484 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1485 {
1486 if (pbs.PathCurve == (byte)Extrusion.Straight)
1487 {
1488 iPropertiesNotSupportedDefault++;
1489 }
1490
1491 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1492 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1493 {
1494 iPropertiesNotSupportedDefault++;
1495 }
1496 }
1497 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1498 {
1499 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1500 {
1501 iPropertiesNotSupportedDefault++;
1502 }
1503 }
1504 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1505 {
1506 if (pbs.PathCurve == (byte)Extrusion.Straight)
1507 {
1508 iPropertiesNotSupportedDefault++;
1509 }
1510 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1511 {
1512 iPropertiesNotSupportedDefault++;
1513 }
1514 }
1515
1516 if (pbs.SculptEntry && meshSculptedPrim)
1517 iPropertiesNotSupportedDefault++;
1518
1519 if (iPropertiesNotSupportedDefault == 0)
1520 {
1521#if SPAM
1522 m_log.Warn("NonMesh");
1523#endif
1524 return false;
1525 }
1526#if SPAM
1527 m_log.Debug("Mesh");
1528#endif
1529 return true;
1530 }
1531
1532 /// <summary>
1533 /// Called to queue a change to a actor
1534 /// to use in place of old taint mechanism so changes do have a time sequence
1535 /// </summary>
1536
1537 public void AddChange(PhysicsActor actor, changes what, Object arg)
1538 {
1539 ODEchangeitem item = new ODEchangeitem();
1540 item.actor = actor;
1541 item.what = what;
1542 item.arg = arg;
1543 ChangesQueue.Enqueue(item);
1544 }
1545
1546 /// <summary>
1547 /// Called after our prim properties are set Scale, position etc.
1548 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1549 /// This assures us that we have no race conditions
1550 /// </summary>
1551 /// <param name="prim"></param>
1552 public override void AddPhysicsActorTaint(PhysicsActor prim)
1553 {
1554 }
1555
1556 /// <summary>
1557 /// This is our main simulate loop
1558 /// It's thread locked by a Mutex in the scene.
1559 /// It holds Collisions, it instructs ODE to step through the physical reactions
1560 /// It moves the objects around in memory
1561 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1562 /// </summary>
1563 /// <param name="timeStep"></param>
1564 /// <returns></returns>
1565 public override float Simulate(float timeStep)
1566 {
1567 int statstart;
1568 int statchanges = 0;
1569 int statchmove = 0;
1570 int statactmove = 0;
1571 int statray = 0;
1572 int statcol = 0;
1573 int statstep = 0;
1574 int statmovchar = 0;
1575 int statmovprim;
1576 int totjcontact = 0;
1577
1578 // acumulate time so we can reduce error
1579 step_time += timeStep;
1580
1581 if (step_time < ODE_STEPSIZE)
1582 return 0;
1583
1584 if (framecount >= int.MaxValue)
1585 framecount = 0;
1586
1587 framecount++;
1588
1589 int curphysiteractions = m_physicsiterations;
1590
1591 if (step_time >= m_SkipFramesAtms)
1592 {
1593 // if in trouble reduce step resolution
1594 curphysiteractions /= 2;
1595 }
1596
1597 int nodeframes = 0;
1598
1599// checkThread();
1600
1601 lock (SimulationLock)
1602 {
1603 // adjust number of iterations per step
1604 try
1605 {
1606 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1607 }
1608 catch (StackOverflowException)
1609 {
1610 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1611// ode.drelease(world);
1612 base.TriggerPhysicsBasedRestart();
1613 }
1614
1615
1616 while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever
1617 {
1618 try
1619 {
1620 statstart = Util.EnvironmentTickCount();
1621
1622 // clear pointer/counter to contacts to pass into joints
1623 m_global_contactcount = 0;
1624
1625 ODEchangeitem item;
1626
1627 if(ChangesQueue.Count >0)
1628 {
1629 int ttmpstart = Util.EnvironmentTickCount();
1630 int ttmp;
1631 int ttmp2;
1632
1633 while(ChangesQueue.Dequeue(out item))
1634 {
1635 if (item.actor != null)
1636 {
1637 try
1638 {
1639 if (item.actor is OdeCharacter)
1640 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1641 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1642 RemovePrimThreadLocked((OdePrim)item.actor);
1643 }
1644 catch
1645 {
1646 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1647 };
1648 }
1649 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1650 if (ttmp > 20)
1651 break;
1652 }
1653
1654 ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart);
1655 if (ttmp2 > 50)
1656 ttmp2 = 0;
1657
1658 }
1659
1660 statchanges += Util.EnvironmentTickCountSubtract(statstart);
1661
1662 // Move characters
1663 lock (_characters)
1664 {
1665 List<OdeCharacter> defects = new List<OdeCharacter>();
1666 foreach (OdeCharacter actor in _characters)
1667 {
1668 if (actor != null)
1669 actor.Move(ODE_STEPSIZE, defects);
1670 }
1671 if (defects.Count != 0)
1672 {
1673 foreach (OdeCharacter defect in defects)
1674 {
1675 RemoveCharacter(defect);
1676 }
1677 }
1678 }
1679 statchmove += Util.EnvironmentTickCountSubtract(statstart);
1680
1681 // Move other active objects
1682 lock (_activeprims)
1683 {
1684 foreach (OdePrim aprim in _activeprims)
1685 {
1686 aprim.CollisionScore = 0;
1687 aprim.IsColliding = false;
1688 aprim.Move();
1689 }
1690 }
1691
1692 statactmove += Util.EnvironmentTickCountSubtract(statstart);
1693 //if ((framecount % m_randomizeWater) == 0)
1694 // randomizeWater(waterlevel);
1695
1696 m_rayCastManager.ProcessQueuedRequests();
1697
1698 statray += Util.EnvironmentTickCountSubtract(statstart);
1699 collision_optimized();
1700 statcol += Util.EnvironmentTickCountSubtract(statstart);
1701
1702 lock (_collisionEventPrim)
1703 {
1704 foreach (PhysicsActor obj in _collisionEventPrim)
1705 {
1706 if (obj == null)
1707 continue;
1708
1709 switch ((ActorTypes)obj.PhysicsActorType)
1710 {
1711 case ActorTypes.Agent:
1712 OdeCharacter cobj = (OdeCharacter)obj;
1713 cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f));
1714 cobj.SendCollisions();
1715 break;
1716
1717 case ActorTypes.Prim:
1718 OdePrim pobj = (OdePrim)obj;
1719 pobj.SendCollisions();
1720 break;
1721 }
1722 }
1723 }
1724
1725 d.WorldQuickStep(world, ODE_STEPSIZE);
1726 statstep += Util.EnvironmentTickCountSubtract(statstart);
1727 d.JointGroupEmpty(contactgroup);
1728 totjcontact += m_global_contactcount;
1729 //ode.dunlock(world);
1730 }
1731 catch (Exception e)
1732 {
1733 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1734// ode.dunlock(world);
1735 }
1736
1737 step_time -= ODE_STEPSIZE;
1738 nodeframes++;
1739 }
1740
1741 statstart = Util.EnvironmentTickCount();
1742
1743 lock (_characters)
1744 {
1745 foreach (OdeCharacter actor in _characters)
1746 {
1747 if (actor != null)
1748 {
1749 if (actor.bad)
1750 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1751
1752 actor.UpdatePositionAndVelocity();
1753 }
1754 }
1755 }
1756
1757 lock (_badCharacter)
1758 {
1759 if (_badCharacter.Count > 0)
1760 {
1761 foreach (OdeCharacter chr in _badCharacter)
1762 {
1763 RemoveCharacter(chr);
1764 }
1765
1766 _badCharacter.Clear();
1767 }
1768 }
1769 statmovchar = Util.EnvironmentTickCountSubtract(statstart);
1770
1771 lock (_activeprims)
1772 {
1773 {
1774 foreach (OdePrim actor in _activeprims)
1775 {
1776 if (actor.IsPhysical)
1777 {
1778 actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
1779 }
1780 }
1781 }
1782 }
1783
1784 statmovprim = Util.EnvironmentTickCountSubtract(statstart);
1785
1786 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1787 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1788 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
1789 int nbodies = d.NTotalBodies;
1790 int ngeoms = d.NTotalGeoms;
1791
1792 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1793 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1794 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
1795 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
1796 {
1797 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
1798 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
1799
1800 if (physics_logging_append_existing_logfile)
1801 {
1802 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
1803 TextWriter fwriter = File.AppendText(fname);
1804 fwriter.WriteLine(header);
1805 fwriter.Close();
1806 }
1807
1808 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1809 }
1810
1811 // think time dilation is not a physics issue alone.. but ok let's fake something
1812 if (step_time < ODE_STEPSIZE) // we did the required loops
1813 m_timeDilation = 1.0f;
1814 else
1815 { // we didn't forget the lost ones and let user know something
1816 m_timeDilation = 1 - step_time / timeStep;
1817 if (m_timeDilation < 0)
1818 m_timeDilation = 0;
1819 step_time = 0;
1820 }
1821 }
1822
1823// return nodeframes * ODE_STEPSIZE; // return real simulated time
1824 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
1825 }
1826
1827 /// <summary>
1828 public override void GetResults()
1829 {
1830 }
1831
1832 public override bool IsThreaded
1833 {
1834 // for now we won't be multithreaded
1835 get { return (false); }
1836 }
1837
1838 #region ODE Specific Terrain Fixes
1839 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
1840 {
1841 float[] returnarr = new float[262144];
1842 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
1843
1844 // Filling out the array into its multi-dimensional components
1845 for (int y = 0; y < WorldExtents.Y; y++)
1846 {
1847 for (int x = 0; x < WorldExtents.X; x++)
1848 {
1849 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
1850 }
1851 }
1852
1853 // Resize using Nearest Neighbour
1854
1855 // This particular way is quick but it only works on a multiple of the original
1856
1857 // The idea behind this method can be described with the following diagrams
1858 // second pass and third pass happen in the same loop really.. just separated
1859 // them to show what this does.
1860
1861 // First Pass
1862 // ResultArr:
1863 // 1,1,1,1,1,1
1864 // 1,1,1,1,1,1
1865 // 1,1,1,1,1,1
1866 // 1,1,1,1,1,1
1867 // 1,1,1,1,1,1
1868 // 1,1,1,1,1,1
1869
1870 // Second Pass
1871 // ResultArr2:
1872 // 1,,1,,1,,1,,1,,1,
1873 // ,,,,,,,,,,
1874 // 1,,1,,1,,1,,1,,1,
1875 // ,,,,,,,,,,
1876 // 1,,1,,1,,1,,1,,1,
1877 // ,,,,,,,,,,
1878 // 1,,1,,1,,1,,1,,1,
1879 // ,,,,,,,,,,
1880 // 1,,1,,1,,1,,1,,1,
1881 // ,,,,,,,,,,
1882 // 1,,1,,1,,1,,1,,1,
1883
1884 // Third pass fills in the blanks
1885 // ResultArr2:
1886 // 1,1,1,1,1,1,1,1,1,1,1,1
1887 // 1,1,1,1,1,1,1,1,1,1,1,1
1888 // 1,1,1,1,1,1,1,1,1,1,1,1
1889 // 1,1,1,1,1,1,1,1,1,1,1,1
1890 // 1,1,1,1,1,1,1,1,1,1,1,1
1891 // 1,1,1,1,1,1,1,1,1,1,1,1
1892 // 1,1,1,1,1,1,1,1,1,1,1,1
1893 // 1,1,1,1,1,1,1,1,1,1,1,1
1894 // 1,1,1,1,1,1,1,1,1,1,1,1
1895 // 1,1,1,1,1,1,1,1,1,1,1,1
1896 // 1,1,1,1,1,1,1,1,1,1,1,1
1897
1898 // X,Y = .
1899 // X+1,y = ^
1900 // X,Y+1 = *
1901 // X+1,Y+1 = #
1902
1903 // Filling in like this;
1904 // .*
1905 // ^#
1906 // 1st .
1907 // 2nd *
1908 // 3rd ^
1909 // 4th #
1910 // on single loop.
1911
1912 float[,] resultarr2 = new float[512, 512];
1913 for (int y = 0; y < WorldExtents.Y; y++)
1914 {
1915 for (int x = 0; x < WorldExtents.X; x++)
1916 {
1917 resultarr2[y * 2, x * 2] = resultarr[y, x];
1918
1919 if (y < WorldExtents.Y)
1920 {
1921 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
1922 }
1923 if (x < WorldExtents.X)
1924 {
1925 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
1926 }
1927 if (x < WorldExtents.X && y < WorldExtents.Y)
1928 {
1929 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
1930 }
1931 }
1932 }
1933
1934 //Flatten out the array
1935 int i = 0;
1936 for (int y = 0; y < 512; y++)
1937 {
1938 for (int x = 0; x < 512; x++)
1939 {
1940 if (resultarr2[y, x] <= 0)
1941 returnarr[i] = 0.0000001f;
1942 else
1943 returnarr[i] = resultarr2[y, x];
1944
1945 i++;
1946 }
1947 }
1948
1949 return returnarr;
1950 }
1951
1952 public float[] ResizeTerrain512Interpolation(float[] heightMap)
1953 {
1954 float[] returnarr = new float[262144];
1955 float[,] resultarr = new float[512,512];
1956
1957 // Filling out the array into its multi-dimensional components
1958 for (int y = 0; y < 256; y++)
1959 {
1960 for (int x = 0; x < 256; x++)
1961 {
1962 resultarr[y, x] = heightMap[y * 256 + x];
1963 }
1964 }
1965
1966 // Resize using interpolation
1967
1968 // This particular way is quick but it only works on a multiple of the original
1969
1970 // The idea behind this method can be described with the following diagrams
1971 // second pass and third pass happen in the same loop really.. just separated
1972 // them to show what this does.
1973
1974 // First Pass
1975 // ResultArr:
1976 // 1,1,1,1,1,1
1977 // 1,1,1,1,1,1
1978 // 1,1,1,1,1,1
1979 // 1,1,1,1,1,1
1980 // 1,1,1,1,1,1
1981 // 1,1,1,1,1,1
1982
1983 // Second Pass
1984 // ResultArr2:
1985 // 1,,1,,1,,1,,1,,1,
1986 // ,,,,,,,,,,
1987 // 1,,1,,1,,1,,1,,1,
1988 // ,,,,,,,,,,
1989 // 1,,1,,1,,1,,1,,1,
1990 // ,,,,,,,,,,
1991 // 1,,1,,1,,1,,1,,1,
1992 // ,,,,,,,,,,
1993 // 1,,1,,1,,1,,1,,1,
1994 // ,,,,,,,,,,
1995 // 1,,1,,1,,1,,1,,1,
1996
1997 // Third pass fills in the blanks
1998 // ResultArr2:
1999 // 1,1,1,1,1,1,1,1,1,1,1,1
2000 // 1,1,1,1,1,1,1,1,1,1,1,1
2001 // 1,1,1,1,1,1,1,1,1,1,1,1
2002 // 1,1,1,1,1,1,1,1,1,1,1,1
2003 // 1,1,1,1,1,1,1,1,1,1,1,1
2004 // 1,1,1,1,1,1,1,1,1,1,1,1
2005 // 1,1,1,1,1,1,1,1,1,1,1,1
2006 // 1,1,1,1,1,1,1,1,1,1,1,1
2007 // 1,1,1,1,1,1,1,1,1,1,1,1
2008 // 1,1,1,1,1,1,1,1,1,1,1,1
2009 // 1,1,1,1,1,1,1,1,1,1,1,1
2010
2011 // X,Y = .
2012 // X+1,y = ^
2013 // X,Y+1 = *
2014 // X+1,Y+1 = #
2015
2016 // Filling in like this;
2017 // .*
2018 // ^#
2019 // 1st .
2020 // 2nd *
2021 // 3rd ^
2022 // 4th #
2023 // on single loop.
2024
2025 float[,] resultarr2 = new float[512,512];
2026 for (int y = 0; y < (int)Constants.RegionSize; y++)
2027 {
2028 for (int x = 0; x < (int)Constants.RegionSize; x++)
2029 {
2030 resultarr2[y*2, x*2] = resultarr[y, x];
2031
2032 if (y < (int)Constants.RegionSize)
2033 {
2034 if (y + 1 < (int)Constants.RegionSize)
2035 {
2036 if (x + 1 < (int)Constants.RegionSize)
2037 {
2038 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
2039 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
2040 }
2041 else
2042 {
2043 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
2044 }
2045 }
2046 else
2047 {
2048 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
2049 }
2050 }
2051 if (x < (int)Constants.RegionSize)
2052 {
2053 if (x + 1 < (int)Constants.RegionSize)
2054 {
2055 if (y + 1 < (int)Constants.RegionSize)
2056 {
2057 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
2058 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
2059 }
2060 else
2061 {
2062 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
2063 }
2064 }
2065 else
2066 {
2067 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
2068 }
2069 }
2070 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
2071 {
2072 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
2073 {
2074 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
2075 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
2076 }
2077 else
2078 {
2079 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
2080 }
2081 }
2082 }
2083 }
2084 //Flatten out the array
2085 int i = 0;
2086 for (int y = 0; y < 512; y++)
2087 {
2088 for (int x = 0; x < 512; x++)
2089 {
2090 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
2091 {
2092 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
2093 resultarr2[y, x] = 0;
2094 }
2095 returnarr[i] = resultarr2[y, x];
2096 i++;
2097 }
2098 }
2099
2100 return returnarr;
2101 }
2102
2103 #endregion
2104
2105 public override void SetTerrain(float[] heightMap)
2106 {
2107 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2108 {
2109 if (m_parentScene is OdeScene)
2110 {
2111 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2112 }
2113 }
2114 else
2115 {
2116 SetTerrain(heightMap, m_worldOffset);
2117 }
2118 }
2119
2120 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2121 {
2122 SetTerrain(heightMap, pOffset);
2123 }
2124
2125 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2126 {
2127
2128 float[] _heightmap;
2129 _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))];
2130
2131 uint heightmapWidth = Constants.RegionSize + 2;
2132 uint heightmapHeight = Constants.RegionSize + 2;
2133
2134 uint heightmapWidthSamples;
2135
2136 uint heightmapHeightSamples;
2137
2138 heightmapWidthSamples = (uint)Constants.RegionSize + 2;
2139 heightmapHeightSamples = (uint)Constants.RegionSize + 2;
2140
2141 const float scale = 1.0f;
2142 const float offset = 0.0f;
2143 const float thickness = 10f;
2144 const int wrap = 0;
2145
2146 int regionsize = (int) Constants.RegionSize + 2;
2147
2148 float hfmin = float.MaxValue;
2149 float hfmax = float.MinValue;
2150 float val;
2151 int xx;
2152 int yy;
2153
2154 int maxXXYY = regionsize - 3;
2155 // flipping map adding one margin all around so things don't fall in edges
2156
2157 int xt = 0;
2158 xx = 0;
2159
2160 for (int x = 0; x < heightmapWidthSamples; x++)
2161 {
2162 if (x > 1 && xx < maxXXYY)
2163 xx++;
2164 yy = 0;
2165 for (int y = 0; y < heightmapHeightSamples; y++)
2166 {
2167 if (y > 1 && y < maxXXYY)
2168 yy += (int)Constants.RegionSize;
2169
2170 val = heightMap[yy + xx];
2171 _heightmap[xt + y] = val;
2172
2173 if (hfmin > val)
2174 hfmin = val;
2175 if (hfmax < val)
2176 hfmax = val;
2177
2178 }
2179
2180 xt += regionsize;
2181 }
2182 lock (OdeLock)
2183 {
2184 IntPtr GroundGeom = IntPtr.Zero;
2185 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2186 {
2187 RegionTerrain.Remove(pOffset);
2188 if (GroundGeom != IntPtr.Zero)
2189 {
2190 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2191 {
2192 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2193 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2194 TerrainHeightFieldHeights.Remove(GroundGeom);
2195 }
2196 d.SpaceRemove(StaticSpace, GroundGeom);
2197 d.GeomDestroy(GroundGeom);
2198 }
2199 }
2200 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2201
2202 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2203
2204 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2205 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2206 offset, thickness, wrap);
2207
2208 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2209 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2210 if (GroundGeom != IntPtr.Zero)
2211 {
2212 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2213 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2214
2215 }
2216 geom_name_map[GroundGeom] = "Terrain";
2217
2218 d.Matrix3 R = new d.Matrix3();
2219
2220 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2221 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2222
2223
2224 q1 = q1 * q2;
2225
2226 Vector3 v3;
2227 float angle;
2228 q1.GetAxisAngle(out v3, out angle);
2229
2230 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2231 d.GeomSetRotation(GroundGeom, ref R);
2232 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0);
2233 IntPtr testGround = IntPtr.Zero;
2234 if (RegionTerrain.TryGetValue(pOffset, out testGround))
2235 {
2236 RegionTerrain.Remove(pOffset);
2237 }
2238 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2239// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2240 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2241 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2242
2243 }
2244 }
2245
2246 public override void DeleteTerrain()
2247 {
2248 }
2249
2250 public float GetWaterLevel()
2251 {
2252 return waterlevel;
2253 }
2254
2255 public override bool SupportsCombining()
2256 {
2257 return true;
2258 }
2259/*
2260 public override void UnCombine(PhysicsScene pScene)
2261 {
2262 IntPtr localGround = IntPtr.Zero;
2263// float[] localHeightfield;
2264 bool proceed = false;
2265 List<IntPtr> geomDestroyList = new List<IntPtr>();
2266
2267 lock (OdeLock)
2268 {
2269 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2270 {
2271 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2272 {
2273 if (geom == localGround)
2274 {
2275// localHeightfield = TerrainHeightFieldHeights[geom];
2276 proceed = true;
2277 }
2278 else
2279 {
2280 geomDestroyList.Add(geom);
2281 }
2282 }
2283
2284 if (proceed)
2285 {
2286 m_worldOffset = Vector3.Zero;
2287 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2288 m_parentScene = null;
2289
2290 foreach (IntPtr g in geomDestroyList)
2291 {
2292 // removingHeightField needs to be done or the garbage collector will
2293 // collect the terrain data before we tell ODE to destroy it causing
2294 // memory corruption
2295 if (TerrainHeightFieldHeights.ContainsKey(g))
2296 {
2297// float[] removingHeightField = TerrainHeightFieldHeights[g];
2298 TerrainHeightFieldHeights.Remove(g);
2299
2300 if (RegionTerrain.ContainsKey(g))
2301 {
2302 RegionTerrain.Remove(g);
2303 }
2304
2305 d.GeomDestroy(g);
2306 //removingHeightField = new float[0];
2307 }
2308 }
2309
2310 }
2311 else
2312 {
2313 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2314 }
2315 }
2316 }
2317 }
2318*/
2319 public override void SetWaterLevel(float baseheight)
2320 {
2321 waterlevel = baseheight;
2322 randomizeWater(waterlevel);
2323 }
2324
2325 public void randomizeWater(float baseheight)
2326 {
2327 const uint heightmapWidth = m_regionWidth + 2;
2328 const uint heightmapHeight = m_regionHeight + 2;
2329 const uint heightmapWidthSamples = m_regionWidth + 2;
2330 const uint heightmapHeightSamples = m_regionHeight + 2;
2331 const float scale = 1.0f;
2332 const float offset = 0.0f;
2333 const float thickness = 2.9f;
2334 const int wrap = 0;
2335
2336 for (int i = 0; i < (258 * 258); i++)
2337 {
2338 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
2339 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
2340 }
2341
2342 lock (OdeLock)
2343 {
2344 if (WaterGeom != IntPtr.Zero)
2345 {
2346 d.SpaceRemove(StaticSpace, WaterGeom);
2347 }
2348 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2349 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
2350 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2351 offset, thickness, wrap);
2352 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
2353 WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2354 if (WaterGeom != IntPtr.Zero)
2355 {
2356 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
2357 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
2358
2359 }
2360 geom_name_map[WaterGeom] = "Water";
2361
2362 d.Matrix3 R = new d.Matrix3();
2363
2364 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2365 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2366
2367 q1 = q1 * q2;
2368 Vector3 v3;
2369 float angle;
2370 q1.GetAxisAngle(out v3, out angle);
2371
2372 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2373 d.GeomSetRotation(WaterGeom, ref R);
2374 d.GeomSetPosition(WaterGeom, 128, 128, 0);
2375
2376 }
2377
2378 }
2379
2380 public override void Dispose()
2381 {
2382 m_rayCastManager.Dispose();
2383 m_rayCastManager = null;
2384
2385 lock (OdeLock)
2386 {
2387 lock (_prims)
2388 {
2389 foreach (OdePrim prm in _prims)
2390 {
2391 RemovePrim(prm);
2392 }
2393 }
2394
2395 if (ContactgeomsArray != IntPtr.Zero)
2396 Marshal.FreeHGlobal(ContactgeomsArray);
2397 if (GlobalContactsArray != IntPtr.Zero)
2398 Marshal.FreeHGlobal(GlobalContactsArray);
2399
2400 d.WorldDestroy(world);
2401 //d.CloseODE();
2402 }
2403 }
2404
2405 public override Dictionary<uint, float> GetTopColliders()
2406 {
2407 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2408 int cnt = 0;
2409 lock (_prims)
2410 {
2411 foreach (OdePrim prm in _prims)
2412 {
2413 if (prm.CollisionScore > 0)
2414 {
2415 returncolliders.Add(prm.m_localID, prm.CollisionScore);
2416 cnt++;
2417 prm.CollisionScore = 0f;
2418 if (cnt > 25)
2419 {
2420 break;
2421 }
2422 }
2423 }
2424 }
2425 return returncolliders;
2426 }
2427
2428 public override bool SupportsRayCast()
2429 {
2430 return true;
2431 }
2432
2433 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2434 {
2435 if (retMethod != null)
2436 {
2437 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2438 }
2439 }
2440
2441 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2442 {
2443 if (retMethod != null)
2444 {
2445 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2446 }
2447 }
2448
2449 // don't like this
2450 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2451 {
2452 ContactResult[] ourResults = null;
2453 RayCallback retMethod = delegate(List<ContactResult> results)
2454 {
2455 ourResults = new ContactResult[results.Count];
2456 results.CopyTo(ourResults, 0);
2457 };
2458 int waitTime = 0;
2459 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2460 while (ourResults == null && waitTime < 1000)
2461 {
2462 Thread.Sleep(1);
2463 waitTime++;
2464 }
2465 if (ourResults == null)
2466 return new List<ContactResult>();
2467 return new List<ContactResult>(ourResults);
2468 }
2469
2470 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2471 {
2472 if (retMethod != null && actor !=null)
2473 {
2474 IntPtr geom;
2475 if (actor is OdePrim)
2476 geom = ((OdePrim)actor).prim_geom;
2477 else if (actor is OdeCharacter)
2478 geom = ((OdePrim)actor).prim_geom;
2479 else
2480 return;
2481 if (geom == IntPtr.Zero)
2482 return;
2483 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2484 }
2485 }
2486
2487 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2488 {
2489 if (retMethod != null && actor != null)
2490 {
2491 IntPtr geom;
2492 if (actor is OdePrim)
2493 geom = ((OdePrim)actor).prim_geom;
2494 else if (actor is OdeCharacter)
2495 geom = ((OdePrim)actor).prim_geom;
2496 else
2497 return;
2498 if (geom == IntPtr.Zero)
2499 return;
2500
2501 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2502 }
2503 }
2504
2505 // don't like this
2506 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2507 {
2508 if (actor != null)
2509 {
2510 IntPtr geom;
2511 if (actor is OdePrim)
2512 geom = ((OdePrim)actor).prim_geom;
2513 else if (actor is OdeCharacter)
2514 geom = ((OdePrim)actor).prim_geom;
2515 else
2516 return new List<ContactResult>();
2517 if (geom == IntPtr.Zero)
2518 return new List<ContactResult>();
2519
2520 ContactResult[] ourResults = null;
2521 RayCallback retMethod = delegate(List<ContactResult> results)
2522 {
2523 ourResults = new ContactResult[results.Count];
2524 results.CopyTo(ourResults, 0);
2525 };
2526 int waitTime = 0;
2527 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2528 while (ourResults == null && waitTime < 1000)
2529 {
2530 Thread.Sleep(1);
2531 waitTime++;
2532 }
2533 if (ourResults == null)
2534 return new List<ContactResult>();
2535 return new List<ContactResult>(ourResults);
2536 }
2537 return new List<ContactResult>();
2538 }
2539 }
2540}
generated by cgit v1.1 at 2024-11-08 06:08:29 +0000