diff options
author | UbitUmarov | 2015-09-08 17:21:45 +0100 |
---|---|---|
committer | UbitUmarov | 2015-09-08 17:21:45 +0100 |
commit | 5c619fe0ab607be471f6d6fc786da28feff94837 (patch) | |
tree | 376535da35dbbdef4a8edf37cf6434e1c465bd77 /OpenSim/Region/PhysicsModules/UbitOde | |
parent | rename Ubitode (diff) | |
download | opensim-SC_OLD-5c619fe0ab607be471f6d6fc786da28feff94837.zip opensim-SC_OLD-5c619fe0ab607be471f6d6fc786da28feff94837.tar.gz opensim-SC_OLD-5c619fe0ab607be471f6d6fc786da28feff94837.tar.bz2 opensim-SC_OLD-5c619fe0ab607be471f6d6fc786da28feff94837.tar.xz |
opps my files?
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/AssemblyInfo.cs | 58 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs | 1847 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs | 1096 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs | 933 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs | 3901 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs | 683 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs | 356 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs | 2025 | ||||
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs | 2930 |
9 files changed, 13829 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/UbitOde/AssemblyInfo.cs new file mode 100644 index 0000000..d46341b --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/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 | |||
28 | using System.Reflection; | ||
29 | using 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/PhysicsModules/UbitOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs new file mode 100644 index 0000000..e461dcd --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODECharacter.cs | |||
@@ -0,0 +1,1847 @@ | |||
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 | |||
31 | using System; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using OpenMetaverse; | ||
35 | using OdeAPI; | ||
36 | using OpenSim.Framework; | ||
37 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
38 | using log4net; | ||
39 | |||
40 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
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 Vector3 _velocity; | ||
78 | private Vector3 _target_velocity; | ||
79 | private Vector3 _acceleration; | ||
80 | private Vector3 m_rotationalVelocity; | ||
81 | private Vector3 m_size; | ||
82 | private Vector3 m_collideNormal; | ||
83 | private Quaternion m_orientation; | ||
84 | private Quaternion m_orientation2D; | ||
85 | private float m_mass = 80f; | ||
86 | public float m_density = 60f; | ||
87 | private bool m_pidControllerActive = true; | ||
88 | |||
89 | const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) | ||
90 | const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) | ||
91 | public float PID_D; | ||
92 | public float PID_P; | ||
93 | |||
94 | private float timeStep; | ||
95 | private float invtimeStep; | ||
96 | |||
97 | private float m_feetOffset = 0; | ||
98 | private float feetOff = 0; | ||
99 | private float boneOff = 0; | ||
100 | private float AvaAvaSizeXsq = 0.3f; | ||
101 | private float AvaAvaSizeYsq = 0.2f; | ||
102 | |||
103 | public float walkDivisor = 1.3f; | ||
104 | public float runDivisor = 0.8f; | ||
105 | private bool flying = false; | ||
106 | private bool m_iscolliding = false; | ||
107 | private bool m_iscollidingGround = false; | ||
108 | private bool m_iscollidingObj = false; | ||
109 | private bool m_alwaysRun = false; | ||
110 | |||
111 | private bool _zeroFlag = false; | ||
112 | |||
113 | |||
114 | private uint m_localID = 0; | ||
115 | public bool m_returnCollisions = false; | ||
116 | // taints and their non-tainted counterparts | ||
117 | public bool m_isPhysical = false; // the current physical status | ||
118 | public float MinimumGroundFlightOffset = 3f; | ||
119 | |||
120 | private float m_buoyancy = 0f; | ||
121 | |||
122 | private bool m_freemove = false; | ||
123 | // private CollisionLocker ode; | ||
124 | |||
125 | // private string m_name = String.Empty; | ||
126 | // other filter control | ||
127 | int m_colliderfilter = 0; | ||
128 | int m_colliderGroundfilter = 0; | ||
129 | int m_colliderObjectfilter = 0; | ||
130 | |||
131 | // Default we're a Character | ||
132 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); | ||
133 | |||
134 | // Default, Collide with Other Geometries, spaces, bodies and characters. | ||
135 | private CollisionCategories m_collisionFlags = (CollisionCategories.Character | ||
136 | | CollisionCategories.Geom | ||
137 | | CollisionCategories.VolumeDtc | ||
138 | ); | ||
139 | // we do land collisions not ode | CollisionCategories.Land); | ||
140 | public IntPtr Body = IntPtr.Zero; | ||
141 | private OdeScene _parent_scene; | ||
142 | private IntPtr capsule = IntPtr.Zero; | ||
143 | public IntPtr collider = IntPtr.Zero; | ||
144 | |||
145 | public IntPtr Amotor = IntPtr.Zero; | ||
146 | |||
147 | public d.Mass ShellMass; | ||
148 | |||
149 | public int m_eventsubscription = 0; | ||
150 | private int m_cureventsubscription = 0; | ||
151 | private CollisionEventUpdate CollisionEventsThisFrame = null; | ||
152 | private bool SentEmptyCollisionsEvent; | ||
153 | |||
154 | // unique UUID of this character object | ||
155 | public UUID m_uuid; | ||
156 | public bool bad = false; | ||
157 | |||
158 | float mu; | ||
159 | |||
160 | public OdeCharacter(uint localID, String avName, OdeScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) | ||
161 | { | ||
162 | m_uuid = UUID.Random(); | ||
163 | m_localID = localID; | ||
164 | |||
165 | timeStep = parent_scene.ODE_STEPSIZE; | ||
166 | invtimeStep = 1 / timeStep; | ||
167 | |||
168 | if (pos.IsFinite()) | ||
169 | { | ||
170 | if (pos.Z > 99999f) | ||
171 | { | ||
172 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
173 | } | ||
174 | if (pos.Z < -100f) // shouldn't this be 0 ? | ||
175 | { | ||
176 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
177 | } | ||
178 | _position = pos; | ||
179 | } | ||
180 | else | ||
181 | { | ||
182 | _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); | ||
183 | m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); | ||
184 | } | ||
185 | |||
186 | _parent_scene = parent_scene; | ||
187 | |||
188 | |||
189 | m_size.X = pSize.X; | ||
190 | m_size.Y = pSize.Y; | ||
191 | m_size.Z = pSize.Z; | ||
192 | |||
193 | if(m_size.X <0.01f) | ||
194 | m_size.X = 0.01f; | ||
195 | if(m_size.Y <0.01f) | ||
196 | m_size.Y = 0.01f; | ||
197 | if(m_size.Z <0.01f) | ||
198 | m_size.Z = 0.01f; | ||
199 | |||
200 | m_feetOffset = pfeetOffset; | ||
201 | m_orientation = Quaternion.Identity; | ||
202 | m_orientation2D = Quaternion.Identity; | ||
203 | m_density = density; | ||
204 | |||
205 | // force lower density for testing | ||
206 | m_density = 3.0f; | ||
207 | |||
208 | mu = parent_scene.AvatarFriction; | ||
209 | |||
210 | walkDivisor = walk_divisor; | ||
211 | runDivisor = rundivisor; | ||
212 | |||
213 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default | ||
214 | |||
215 | PID_D = basePID_D * m_mass * invtimeStep; | ||
216 | PID_P = basePID_P * m_mass * invtimeStep; | ||
217 | |||
218 | m_isPhysical = false; // current status: no ODE information exists | ||
219 | |||
220 | Name = avName; | ||
221 | |||
222 | AddChange(changes.Add, null); | ||
223 | } | ||
224 | |||
225 | public override int PhysicsActorType | ||
226 | { | ||
227 | get { return (int)ActorTypes.Agent; } | ||
228 | set { return; } | ||
229 | } | ||
230 | |||
231 | public override void getContactData(ref ContactData cdata) | ||
232 | { | ||
233 | cdata.mu = mu; | ||
234 | cdata.bounce = 0; | ||
235 | cdata.softcolide = false; | ||
236 | } | ||
237 | |||
238 | public override bool Building { get; set; } | ||
239 | |||
240 | /// <summary> | ||
241 | /// If this is set, the avatar will move faster | ||
242 | /// </summary> | ||
243 | public override bool SetAlwaysRun | ||
244 | { | ||
245 | get { return m_alwaysRun; } | ||
246 | set { m_alwaysRun = value; } | ||
247 | } | ||
248 | |||
249 | public override uint LocalID | ||
250 | { | ||
251 | get { return m_localID; } | ||
252 | set { m_localID = value; } | ||
253 | } | ||
254 | |||
255 | public override PhysicsActor ParentActor | ||
256 | { | ||
257 | get { return (PhysicsActor)this; } | ||
258 | } | ||
259 | |||
260 | public override bool Grabbed | ||
261 | { | ||
262 | set { return; } | ||
263 | } | ||
264 | |||
265 | public override bool Selected | ||
266 | { | ||
267 | set { return; } | ||
268 | } | ||
269 | |||
270 | public override float Buoyancy | ||
271 | { | ||
272 | get { return m_buoyancy; } | ||
273 | set { m_buoyancy = value; } | ||
274 | } | ||
275 | |||
276 | public override bool FloatOnWater | ||
277 | { | ||
278 | set { return; } | ||
279 | } | ||
280 | |||
281 | public override bool IsPhysical | ||
282 | { | ||
283 | get { return m_isPhysical; } | ||
284 | set { return; } | ||
285 | } | ||
286 | |||
287 | public override bool ThrottleUpdates | ||
288 | { | ||
289 | get { return false; } | ||
290 | set { return; } | ||
291 | } | ||
292 | |||
293 | public override bool Flying | ||
294 | { | ||
295 | get { return flying; } | ||
296 | set | ||
297 | { | ||
298 | flying = value; | ||
299 | // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); | ||
300 | } | ||
301 | } | ||
302 | |||
303 | /// <summary> | ||
304 | /// Returns if the avatar is colliding in general. | ||
305 | /// This includes the ground and objects and avatar. | ||
306 | /// </summary> | ||
307 | public override bool IsColliding | ||
308 | { | ||
309 | get { return (m_iscolliding || m_iscollidingGround); } | ||
310 | set | ||
311 | { | ||
312 | if (value) | ||
313 | { | ||
314 | m_colliderfilter += 3; | ||
315 | if (m_colliderfilter > 3) | ||
316 | m_colliderfilter = 3; | ||
317 | } | ||
318 | else | ||
319 | { | ||
320 | m_colliderfilter--; | ||
321 | if (m_colliderfilter < 0) | ||
322 | m_colliderfilter = 0; | ||
323 | } | ||
324 | |||
325 | if (m_colliderfilter == 0) | ||
326 | m_iscolliding = false; | ||
327 | else | ||
328 | { | ||
329 | m_pidControllerActive = true; | ||
330 | m_iscolliding = true; | ||
331 | m_freemove = false; | ||
332 | } | ||
333 | } | ||
334 | } | ||
335 | |||
336 | /// <summary> | ||
337 | /// Returns if an avatar is colliding with the ground | ||
338 | /// </summary> | ||
339 | public override bool CollidingGround | ||
340 | { | ||
341 | get { return m_iscollidingGround; } | ||
342 | set | ||
343 | { | ||
344 | /* we now control this | ||
345 | if (value) | ||
346 | { | ||
347 | m_colliderGroundfilter += 2; | ||
348 | if (m_colliderGroundfilter > 2) | ||
349 | m_colliderGroundfilter = 2; | ||
350 | } | ||
351 | else | ||
352 | { | ||
353 | m_colliderGroundfilter--; | ||
354 | if (m_colliderGroundfilter < 0) | ||
355 | m_colliderGroundfilter = 0; | ||
356 | } | ||
357 | |||
358 | if (m_colliderGroundfilter == 0) | ||
359 | m_iscollidingGround = false; | ||
360 | else | ||
361 | m_iscollidingGround = true; | ||
362 | */ | ||
363 | } | ||
364 | |||
365 | } | ||
366 | |||
367 | /// <summary> | ||
368 | /// Returns if the avatar is colliding with an object | ||
369 | /// </summary> | ||
370 | public override bool CollidingObj | ||
371 | { | ||
372 | get { return m_iscollidingObj; } | ||
373 | set | ||
374 | { | ||
375 | // Ubit filter this also | ||
376 | if (value) | ||
377 | { | ||
378 | m_colliderObjectfilter += 2; | ||
379 | if (m_colliderObjectfilter > 2) | ||
380 | m_colliderObjectfilter = 2; | ||
381 | } | ||
382 | else | ||
383 | { | ||
384 | m_colliderObjectfilter--; | ||
385 | if (m_colliderObjectfilter < 0) | ||
386 | m_colliderObjectfilter = 0; | ||
387 | } | ||
388 | |||
389 | if (m_colliderObjectfilter == 0) | ||
390 | m_iscollidingObj = false; | ||
391 | else | ||
392 | m_iscollidingObj = true; | ||
393 | |||
394 | // m_iscollidingObj = value; | ||
395 | |||
396 | if (m_iscollidingObj) | ||
397 | m_pidControllerActive = false; | ||
398 | else | ||
399 | m_pidControllerActive = true; | ||
400 | } | ||
401 | } | ||
402 | |||
403 | /// <summary> | ||
404 | /// turn the PID controller on or off. | ||
405 | /// The PID Controller will turn on all by itself in many situations | ||
406 | /// </summary> | ||
407 | /// <param name="status"></param> | ||
408 | public void SetPidStatus(bool status) | ||
409 | { | ||
410 | m_pidControllerActive = status; | ||
411 | } | ||
412 | |||
413 | public override bool Stopped | ||
414 | { | ||
415 | get { return _zeroFlag; } | ||
416 | } | ||
417 | |||
418 | /// <summary> | ||
419 | /// This 'puts' an avatar somewhere in the physics space. | ||
420 | /// Not really a good choice unless you 'know' it's a good | ||
421 | /// spot otherwise you're likely to orbit the avatar. | ||
422 | /// </summary> | ||
423 | public override Vector3 Position | ||
424 | { | ||
425 | get { return _position; } | ||
426 | set | ||
427 | { | ||
428 | if (value.IsFinite()) | ||
429 | { | ||
430 | if (value.Z > 9999999f) | ||
431 | { | ||
432 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
433 | } | ||
434 | if (value.Z < -100f) | ||
435 | { | ||
436 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
437 | } | ||
438 | AddChange(changes.Position, value); | ||
439 | } | ||
440 | else | ||
441 | { | ||
442 | m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); | ||
443 | } | ||
444 | } | ||
445 | } | ||
446 | |||
447 | public override Vector3 RotationalVelocity | ||
448 | { | ||
449 | get { return m_rotationalVelocity; } | ||
450 | set { m_rotationalVelocity = value; } | ||
451 | } | ||
452 | |||
453 | /// <summary> | ||
454 | /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight | ||
455 | /// and use it to offset landings properly | ||
456 | /// </summary> | ||
457 | public override Vector3 Size | ||
458 | { | ||
459 | get | ||
460 | { | ||
461 | return m_size; | ||
462 | } | ||
463 | set | ||
464 | { | ||
465 | if (value.IsFinite()) | ||
466 | { | ||
467 | if(value.X <0.01f) | ||
468 | value.X = 0.01f; | ||
469 | if(value.Y <0.01f) | ||
470 | value.Y = 0.01f; | ||
471 | if(value.Z <0.01f) | ||
472 | value.Z = 0.01f; | ||
473 | |||
474 | AddChange(changes.Size, value); | ||
475 | } | ||
476 | else | ||
477 | { | ||
478 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
479 | } | ||
480 | } | ||
481 | } | ||
482 | |||
483 | public override void setAvatarSize(Vector3 size, float feetOffset) | ||
484 | { | ||
485 | if (size.IsFinite()) | ||
486 | { | ||
487 | if (size.X < 0.01f) | ||
488 | size.X = 0.01f; | ||
489 | if (size.Y < 0.01f) | ||
490 | size.Y = 0.01f; | ||
491 | if (size.Z < 0.01f) | ||
492 | size.Z = 0.01f; | ||
493 | |||
494 | strAvatarSize st = new strAvatarSize(); | ||
495 | st.size = size; | ||
496 | st.offset = feetOffset; | ||
497 | AddChange(changes.AvatarSize, st); | ||
498 | } | ||
499 | else | ||
500 | { | ||
501 | m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character"); | ||
502 | } | ||
503 | |||
504 | } | ||
505 | /// <summary> | ||
506 | /// This creates the Avatar's physical Surrogate at the position supplied | ||
507 | /// </summary> | ||
508 | /// <param name="npositionX"></param> | ||
509 | /// <param name="npositionY"></param> | ||
510 | /// <param name="npositionZ"></param> | ||
511 | |||
512 | // | ||
513 | /// <summary> | ||
514 | /// Uses the capped cyllinder volume formula to calculate the avatar's mass. | ||
515 | /// This may be used in calculations in the scene/scenepresence | ||
516 | /// </summary> | ||
517 | public override float Mass | ||
518 | { | ||
519 | get | ||
520 | { | ||
521 | return m_mass; | ||
522 | } | ||
523 | } | ||
524 | public override void link(PhysicsActor obj) | ||
525 | { | ||
526 | |||
527 | } | ||
528 | |||
529 | public override void delink() | ||
530 | { | ||
531 | |||
532 | } | ||
533 | |||
534 | public override void LockAngularMotion(Vector3 axis) | ||
535 | { | ||
536 | |||
537 | } | ||
538 | |||
539 | |||
540 | public override Vector3 Force | ||
541 | { | ||
542 | get { return _target_velocity; } | ||
543 | set { return; } | ||
544 | } | ||
545 | |||
546 | public override int VehicleType | ||
547 | { | ||
548 | get { return 0; } | ||
549 | set { return; } | ||
550 | } | ||
551 | |||
552 | public override void VehicleFloatParam(int param, float value) | ||
553 | { | ||
554 | |||
555 | } | ||
556 | |||
557 | public override void VehicleVectorParam(int param, Vector3 value) | ||
558 | { | ||
559 | |||
560 | } | ||
561 | |||
562 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
563 | { | ||
564 | |||
565 | } | ||
566 | |||
567 | public override void VehicleFlags(int param, bool remove) | ||
568 | { | ||
569 | |||
570 | } | ||
571 | |||
572 | public override void SetVolumeDetect(int param) | ||
573 | { | ||
574 | |||
575 | } | ||
576 | |||
577 | public override Vector3 CenterOfMass | ||
578 | { | ||
579 | get | ||
580 | { | ||
581 | Vector3 pos = _position; | ||
582 | return pos; | ||
583 | } | ||
584 | } | ||
585 | |||
586 | public override Vector3 GeometricCenter | ||
587 | { | ||
588 | get | ||
589 | { | ||
590 | Vector3 pos = _position; | ||
591 | return pos; | ||
592 | } | ||
593 | } | ||
594 | |||
595 | public override PrimitiveBaseShape Shape | ||
596 | { | ||
597 | set { return; } | ||
598 | } | ||
599 | |||
600 | public override Vector3 Velocity | ||
601 | { | ||
602 | get | ||
603 | { | ||
604 | return _velocity; | ||
605 | } | ||
606 | set | ||
607 | { | ||
608 | if (value.IsFinite()) | ||
609 | { | ||
610 | AddChange(changes.Velocity, value); | ||
611 | } | ||
612 | else | ||
613 | { | ||
614 | m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); | ||
615 | } | ||
616 | } | ||
617 | } | ||
618 | |||
619 | public override Vector3 Torque | ||
620 | { | ||
621 | get { return Vector3.Zero; } | ||
622 | set { return; } | ||
623 | } | ||
624 | |||
625 | public override float CollisionScore | ||
626 | { | ||
627 | get { return 0f; } | ||
628 | set { } | ||
629 | } | ||
630 | |||
631 | public override bool Kinematic | ||
632 | { | ||
633 | get { return false; } | ||
634 | set { } | ||
635 | } | ||
636 | |||
637 | public override Quaternion Orientation | ||
638 | { | ||
639 | get { return m_orientation; } | ||
640 | set | ||
641 | { | ||
642 | // fakeori = value; | ||
643 | // givefakeori++; | ||
644 | value.Normalize(); | ||
645 | AddChange(changes.Orientation, value); | ||
646 | } | ||
647 | } | ||
648 | |||
649 | public override Vector3 Acceleration | ||
650 | { | ||
651 | get { return _acceleration; } | ||
652 | set { } | ||
653 | } | ||
654 | |||
655 | public void SetAcceleration(Vector3 accel) | ||
656 | { | ||
657 | m_pidControllerActive = true; | ||
658 | _acceleration = accel; | ||
659 | } | ||
660 | |||
661 | /// <summary> | ||
662 | /// Adds the force supplied to the Target Velocity | ||
663 | /// The PID controller takes this target velocity and tries to make it a reality | ||
664 | /// </summary> | ||
665 | /// <param name="force"></param> | ||
666 | public override void AddForce(Vector3 force, bool pushforce) | ||
667 | { | ||
668 | if (force.IsFinite()) | ||
669 | { | ||
670 | if (pushforce) | ||
671 | { | ||
672 | AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f)); | ||
673 | } | ||
674 | else | ||
675 | { | ||
676 | AddChange(changes.Velocity, force); | ||
677 | } | ||
678 | } | ||
679 | else | ||
680 | { | ||
681 | m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); | ||
682 | } | ||
683 | //m_lastUpdateSent = false; | ||
684 | } | ||
685 | |||
686 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
687 | { | ||
688 | |||
689 | } | ||
690 | |||
691 | public override void SetMomentum(Vector3 momentum) | ||
692 | { | ||
693 | if (momentum.IsFinite()) | ||
694 | AddChange(changes.Momentum, momentum); | ||
695 | } | ||
696 | |||
697 | |||
698 | private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) | ||
699 | { | ||
700 | // sizes one day should came from visual parameters | ||
701 | float sx = m_size.X; | ||
702 | float sy = m_size.Y; | ||
703 | float sz = m_size.Z; | ||
704 | |||
705 | float bot = -sz * 0.5f + m_feetOffset; | ||
706 | boneOff = bot + 0.3f; | ||
707 | |||
708 | float feetsz = sz * 0.45f; | ||
709 | if (feetsz > 0.6f) | ||
710 | feetsz = 0.6f; | ||
711 | |||
712 | feetOff = bot + feetsz; | ||
713 | |||
714 | AvaAvaSizeXsq = 0.4f * sx; | ||
715 | AvaAvaSizeXsq *= AvaAvaSizeXsq; | ||
716 | AvaAvaSizeYsq = 0.5f * sy; | ||
717 | AvaAvaSizeYsq *= AvaAvaSizeYsq; | ||
718 | |||
719 | _parent_scene.waitForSpaceUnlock(_parent_scene.CharsSpace); | ||
720 | |||
721 | collider = d.HashSpaceCreate(_parent_scene.CharsSpace); | ||
722 | d.HashSpaceSetLevels(collider, -4, 3); | ||
723 | d.SpaceSetSublevel(collider, 3); | ||
724 | d.SpaceSetCleanup(collider, false); | ||
725 | d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); | ||
726 | d.GeomSetCollideBits(collider, (uint)m_collisionFlags); | ||
727 | |||
728 | float r = m_size.X; | ||
729 | if (m_size.Y > r) | ||
730 | r = m_size.Y; | ||
731 | float l = m_size.Z - r; | ||
732 | r *= 0.5f; | ||
733 | |||
734 | capsule = d.CreateCapsule(collider, r, l); | ||
735 | |||
736 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass | ||
737 | |||
738 | d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); | ||
739 | |||
740 | PID_D = basePID_D * m_mass / _parent_scene.ODE_STEPSIZE; | ||
741 | PID_P = basePID_P * m_mass / _parent_scene.ODE_STEPSIZE; | ||
742 | |||
743 | Body = d.BodyCreate(_parent_scene.world); | ||
744 | |||
745 | _zeroFlag = false; | ||
746 | m_pidControllerActive = true; | ||
747 | m_freemove = false; | ||
748 | |||
749 | _velocity = Vector3.Zero; | ||
750 | |||
751 | d.BodySetAutoDisableFlag(Body, false); | ||
752 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); | ||
753 | |||
754 | _position.X = npositionX; | ||
755 | _position.Y = npositionY; | ||
756 | _position.Z = npositionZ; | ||
757 | |||
758 | d.BodySetMass(Body, ref ShellMass); | ||
759 | d.GeomSetBody(capsule, Body); | ||
760 | |||
761 | // The purpose of the AMotor here is to keep the avatar's physical | ||
762 | // surrogate from rotating while moving | ||
763 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
764 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
765 | |||
766 | d.JointSetAMotorMode(Amotor, 0); | ||
767 | d.JointSetAMotorNumAxes(Amotor, 3); | ||
768 | d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); | ||
769 | d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); | ||
770 | d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); | ||
771 | |||
772 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
773 | d.JointSetAMotorAngle(Amotor, 1, 0); | ||
774 | d.JointSetAMotorAngle(Amotor, 2, 0); | ||
775 | |||
776 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD | ||
777 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); | ||
778 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); | ||
779 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); | ||
780 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); | ||
781 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); | ||
782 | |||
783 | // These lowstops and high stops are effectively (no wiggle room) | ||
784 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); | ||
785 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); | ||
786 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); | ||
787 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); | ||
788 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); | ||
789 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); | ||
790 | |||
791 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
792 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); | ||
793 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); | ||
794 | |||
795 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f); | ||
796 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); | ||
797 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); | ||
798 | } | ||
799 | |||
800 | /// <summary> | ||
801 | /// Destroys the avatar body and geom | ||
802 | |||
803 | private void AvatarGeomAndBodyDestroy() | ||
804 | { | ||
805 | // Kill the Amotor | ||
806 | if (Amotor != IntPtr.Zero) | ||
807 | { | ||
808 | d.JointDestroy(Amotor); | ||
809 | Amotor = IntPtr.Zero; | ||
810 | } | ||
811 | |||
812 | if (Body != IntPtr.Zero) | ||
813 | { | ||
814 | //kill the body | ||
815 | d.BodyDestroy(Body); | ||
816 | Body = IntPtr.Zero; | ||
817 | } | ||
818 | |||
819 | //kill the Geoms | ||
820 | if (capsule != IntPtr.Zero) | ||
821 | { | ||
822 | _parent_scene.actor_name_map.Remove(capsule); | ||
823 | _parent_scene.waitForSpaceUnlock(collider); | ||
824 | d.GeomDestroy(capsule); | ||
825 | capsule = IntPtr.Zero; | ||
826 | } | ||
827 | |||
828 | if (collider != IntPtr.Zero) | ||
829 | { | ||
830 | d.SpaceDestroy(collider); | ||
831 | collider = IntPtr.Zero; | ||
832 | } | ||
833 | |||
834 | } | ||
835 | |||
836 | //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z | ||
837 | public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
838 | { | ||
839 | float sin = 2.0f * rot.Z * rot.W; | ||
840 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
841 | float tx = x; | ||
842 | |||
843 | x = tx * cos - y * sin; | ||
844 | y = tx * sin + y * cos; | ||
845 | } | ||
846 | public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
847 | { | ||
848 | float tx = x; | ||
849 | x = tx * cos - y * sin; | ||
850 | y = tx * sin + y * cos; | ||
851 | } | ||
852 | public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
853 | { | ||
854 | float tx = x; | ||
855 | x = tx * cos + y * sin; | ||
856 | y = -tx * sin + y * cos; | ||
857 | } | ||
858 | |||
859 | public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
860 | { | ||
861 | float sin = - 2.0f * rot.Z * rot.W; | ||
862 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
863 | float tx = x; | ||
864 | |||
865 | x = tx * cos - y * sin; | ||
866 | y = tx * sin + y * cos; | ||
867 | } | ||
868 | |||
869 | public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, | ||
870 | ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) | ||
871 | { | ||
872 | feetcollision = false; | ||
873 | useAltcontact = false; | ||
874 | |||
875 | if (me == capsule) | ||
876 | { | ||
877 | Vector3 offset; | ||
878 | |||
879 | float h = contact.pos.Z - _position.Z; | ||
880 | offset.Z = h - feetOff; | ||
881 | |||
882 | offset.X = contact.pos.X - _position.X; | ||
883 | offset.Y = contact.pos.Y - _position.Y; | ||
884 | |||
885 | d.GeomClassID gtype = d.GeomGetClass(other); | ||
886 | if (gtype == d.GeomClassID.CapsuleClass) | ||
887 | { | ||
888 | Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); | ||
889 | float r = roff.X *roff.X / AvaAvaSizeXsq; | ||
890 | r += (roff.Y * roff.Y) / AvaAvaSizeYsq; | ||
891 | if (r > 1.0f) | ||
892 | return false; | ||
893 | |||
894 | float dp = 1.0f -(float)Math.Sqrt((double)r); | ||
895 | if (dp > 0.05f) | ||
896 | dp = 0.05f; | ||
897 | |||
898 | contact.depth = dp; | ||
899 | |||
900 | if (offset.Z < 0) | ||
901 | { | ||
902 | feetcollision = true; | ||
903 | if (h < boneOff) | ||
904 | { | ||
905 | m_collideNormal.X = contact.normal.X; | ||
906 | m_collideNormal.Y = contact.normal.Y; | ||
907 | m_collideNormal.Z = contact.normal.Z; | ||
908 | IsColliding = true; | ||
909 | } | ||
910 | } | ||
911 | return true; | ||
912 | } | ||
913 | /* | ||
914 | d.AABB aabb; | ||
915 | d.GeomGetAABB(other,out aabb); | ||
916 | float othertop = aabb.MaxZ - _position.Z; | ||
917 | */ | ||
918 | // if (offset.Z > 0 || othertop > -feetOff || contact.normal.Z > 0.35f) | ||
919 | if (offset.Z > 0 || contact.normal.Z > 0.35f) | ||
920 | { | ||
921 | if (offset.Z <= 0) | ||
922 | { | ||
923 | feetcollision = true; | ||
924 | if (h < boneOff) | ||
925 | { | ||
926 | m_collideNormal.X = contact.normal.X; | ||
927 | m_collideNormal.Y = contact.normal.Y; | ||
928 | m_collideNormal.Z = contact.normal.Z; | ||
929 | IsColliding = true; | ||
930 | } | ||
931 | } | ||
932 | return true; | ||
933 | } | ||
934 | |||
935 | altContact = contact; | ||
936 | useAltcontact = true; | ||
937 | |||
938 | offset.Z -= 0.2f; | ||
939 | |||
940 | offset.Normalize(); | ||
941 | |||
942 | if (contact.depth > 0.1f) | ||
943 | contact.depth = 0.1f; | ||
944 | |||
945 | if (reverse) | ||
946 | { | ||
947 | altContact.normal.X = offset.X; | ||
948 | altContact.normal.Y = offset.Y; | ||
949 | altContact.normal.Z = offset.Z; | ||
950 | } | ||
951 | else | ||
952 | { | ||
953 | altContact.normal.X = -offset.X; | ||
954 | altContact.normal.Y = -offset.Y; | ||
955 | altContact.normal.Z = -offset.Z; | ||
956 | } | ||
957 | |||
958 | feetcollision = true; | ||
959 | if (h < boneOff) | ||
960 | { | ||
961 | m_collideNormal.X = contact.normal.X; | ||
962 | m_collideNormal.Y = contact.normal.Y; | ||
963 | m_collideNormal.Z = contact.normal.Z; | ||
964 | IsColliding = true; | ||
965 | } | ||
966 | return true; | ||
967 | } | ||
968 | return false; | ||
969 | } | ||
970 | |||
971 | /// <summary> | ||
972 | /// Called from Simulate | ||
973 | /// This is the avatar's movement control + PID Controller | ||
974 | /// </summary> | ||
975 | /// <param name="timeStep"></param> | ||
976 | public void Move(List<OdeCharacter> defects) | ||
977 | { | ||
978 | if (Body == IntPtr.Zero) | ||
979 | return; | ||
980 | |||
981 | d.Vector3 dtmp = d.BodyGetPosition(Body); | ||
982 | Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
983 | |||
984 | // the Amotor still lets avatar rotation to drift during colisions | ||
985 | // so force it back to identity | ||
986 | |||
987 | d.Quaternion qtmp; | ||
988 | qtmp.W = m_orientation2D.W; | ||
989 | qtmp.X = m_orientation2D.X; | ||
990 | qtmp.Y = m_orientation2D.Y; | ||
991 | qtmp.Z = m_orientation2D.Z; | ||
992 | d.BodySetQuaternion(Body, ref qtmp); | ||
993 | |||
994 | if (m_pidControllerActive == false) | ||
995 | { | ||
996 | _zeroPosition = localpos; | ||
997 | } | ||
998 | |||
999 | if (!localpos.IsFinite()) | ||
1000 | { | ||
1001 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
1002 | defects.Add(this); | ||
1003 | // _parent_scene.RemoveCharacter(this); | ||
1004 | |||
1005 | // destroy avatar capsule and related ODE data | ||
1006 | AvatarGeomAndBodyDestroy(); | ||
1007 | return; | ||
1008 | } | ||
1009 | |||
1010 | // check outbounds forcing to be in world | ||
1011 | bool fixbody = false; | ||
1012 | if (localpos.X < 0.0f) | ||
1013 | { | ||
1014 | fixbody = true; | ||
1015 | localpos.X = 0.1f; | ||
1016 | } | ||
1017 | else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f) | ||
1018 | { | ||
1019 | fixbody = true; | ||
1020 | localpos.X = _parent_scene.WorldExtents.X - 0.1f; | ||
1021 | } | ||
1022 | if (localpos.Y < 0.0f) | ||
1023 | { | ||
1024 | fixbody = true; | ||
1025 | localpos.Y = 0.1f; | ||
1026 | } | ||
1027 | else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1) | ||
1028 | { | ||
1029 | fixbody = true; | ||
1030 | localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; | ||
1031 | } | ||
1032 | if (fixbody) | ||
1033 | { | ||
1034 | m_freemove = false; | ||
1035 | d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); | ||
1036 | } | ||
1037 | |||
1038 | float breakfactor; | ||
1039 | |||
1040 | Vector3 vec = Vector3.Zero; | ||
1041 | dtmp = d.BodyGetLinearVel(Body); | ||
1042 | Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
1043 | float velLengthSquared = vel.LengthSquared(); | ||
1044 | |||
1045 | |||
1046 | Vector3 ctz = _target_velocity; | ||
1047 | |||
1048 | float movementdivisor = 1f; | ||
1049 | //Ubit change divisions into multiplications below | ||
1050 | if (!m_alwaysRun) | ||
1051 | movementdivisor = 1 / walkDivisor; | ||
1052 | else | ||
1053 | movementdivisor = 1 / runDivisor; | ||
1054 | |||
1055 | ctz.X *= movementdivisor; | ||
1056 | ctz.Y *= movementdivisor; | ||
1057 | |||
1058 | //****************************************** | ||
1059 | // colide with land | ||
1060 | |||
1061 | d.AABB aabb; | ||
1062 | // d.GeomGetAABB(feetbox, out aabb); | ||
1063 | d.GeomGetAABB(capsule, out aabb); | ||
1064 | float chrminZ = aabb.MinZ; // move up a bit | ||
1065 | Vector3 posch = localpos; | ||
1066 | |||
1067 | float ftmp; | ||
1068 | |||
1069 | if (flying) | ||
1070 | { | ||
1071 | ftmp = timeStep; | ||
1072 | posch.X += vel.X * ftmp; | ||
1073 | posch.Y += vel.Y * ftmp; | ||
1074 | } | ||
1075 | |||
1076 | float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); | ||
1077 | if (chrminZ < terrainheight) | ||
1078 | { | ||
1079 | if (ctz.Z < 0) | ||
1080 | ctz.Z = 0; | ||
1081 | |||
1082 | Vector3 n = _parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); | ||
1083 | float depth = terrainheight - chrminZ; | ||
1084 | |||
1085 | vec.Z = depth * PID_P * 50; | ||
1086 | |||
1087 | if (!flying) | ||
1088 | vec.Z += -vel.Z * PID_D; | ||
1089 | |||
1090 | if (depth < 0.2f) | ||
1091 | { | ||
1092 | m_colliderGroundfilter++; | ||
1093 | if (m_colliderGroundfilter > 2) | ||
1094 | { | ||
1095 | m_iscolliding = true; | ||
1096 | m_colliderfilter = 2; | ||
1097 | |||
1098 | if (m_colliderGroundfilter > 10) | ||
1099 | { | ||
1100 | m_colliderGroundfilter = 10; | ||
1101 | m_freemove = false; | ||
1102 | } | ||
1103 | |||
1104 | m_collideNormal.X = n.X; | ||
1105 | m_collideNormal.Y = n.Y; | ||
1106 | m_collideNormal.Z = n.Z; | ||
1107 | |||
1108 | m_iscollidingGround = true; | ||
1109 | |||
1110 | |||
1111 | ContactPoint contact = new ContactPoint(); | ||
1112 | contact.PenetrationDepth = depth; | ||
1113 | contact.Position.X = localpos.X; | ||
1114 | contact.Position.Y = localpos.Y; | ||
1115 | contact.Position.Z = terrainheight; | ||
1116 | contact.SurfaceNormal.X = -n.X; | ||
1117 | contact.SurfaceNormal.Y = -n.Y; | ||
1118 | contact.SurfaceNormal.Z = -n.Z; | ||
1119 | contact.RelativeSpeed = -vel.Z; | ||
1120 | contact.CharacterFeet = true; | ||
1121 | AddCollisionEvent(0, contact); | ||
1122 | |||
1123 | // vec.Z *= 0.5f; | ||
1124 | } | ||
1125 | } | ||
1126 | |||
1127 | else | ||
1128 | { | ||
1129 | m_colliderGroundfilter -= 5; | ||
1130 | if (m_colliderGroundfilter <= 0) | ||
1131 | { | ||
1132 | m_colliderGroundfilter = 0; | ||
1133 | m_iscollidingGround = false; | ||
1134 | } | ||
1135 | } | ||
1136 | } | ||
1137 | else | ||
1138 | { | ||
1139 | m_colliderGroundfilter -= 5; | ||
1140 | if (m_colliderGroundfilter <= 0) | ||
1141 | { | ||
1142 | m_colliderGroundfilter = 0; | ||
1143 | m_iscollidingGround = false; | ||
1144 | } | ||
1145 | } | ||
1146 | |||
1147 | |||
1148 | //****************************************** | ||
1149 | if (!m_iscolliding) | ||
1150 | m_collideNormal.Z = 0; | ||
1151 | |||
1152 | bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); | ||
1153 | |||
1154 | |||
1155 | |||
1156 | if (!tviszero) | ||
1157 | { | ||
1158 | m_freemove = false; | ||
1159 | |||
1160 | // movement relative to surface if moving on it | ||
1161 | // dont disturbe vertical movement, ie jumps | ||
1162 | if (m_iscolliding && !flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) | ||
1163 | { | ||
1164 | float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; | ||
1165 | ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); | ||
1166 | ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); | ||
1167 | ctz.Z -= p; | ||
1168 | if (ctz.Z < 0) | ||
1169 | ctz.Z *= 2; | ||
1170 | |||
1171 | } | ||
1172 | |||
1173 | } | ||
1174 | |||
1175 | |||
1176 | if (!m_freemove) | ||
1177 | { | ||
1178 | |||
1179 | // if velocity is zero, use position control; otherwise, velocity control | ||
1180 | if (tviszero && m_iscolliding && !flying) | ||
1181 | { | ||
1182 | // keep track of where we stopped. No more slippin' & slidin' | ||
1183 | if (!_zeroFlag) | ||
1184 | { | ||
1185 | _zeroFlag = true; | ||
1186 | _zeroPosition = localpos; | ||
1187 | } | ||
1188 | if (m_pidControllerActive) | ||
1189 | { | ||
1190 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
1191 | // react to the physics scene by moving it's position. | ||
1192 | // Avatar to Avatar collisions | ||
1193 | // Prim to avatar collisions | ||
1194 | |||
1195 | vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); | ||
1196 | vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); | ||
1197 | if(vel.Z > 0) | ||
1198 | vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; | ||
1199 | else | ||
1200 | vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; | ||
1201 | /* | ||
1202 | if (flying) | ||
1203 | { | ||
1204 | vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; | ||
1205 | } | ||
1206 | */ | ||
1207 | } | ||
1208 | //PidStatus = true; | ||
1209 | } | ||
1210 | else | ||
1211 | { | ||
1212 | m_pidControllerActive = true; | ||
1213 | _zeroFlag = false; | ||
1214 | |||
1215 | if (m_iscolliding) | ||
1216 | { | ||
1217 | if (!flying) | ||
1218 | { | ||
1219 | // we are on a surface | ||
1220 | if (ctz.Z > 0f) | ||
1221 | { | ||
1222 | // moving up or JUMPING | ||
1223 | vec.Z += (ctz.Z - vel.Z) * PID_D * 2f; | ||
1224 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1225 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1226 | } | ||
1227 | else | ||
1228 | { | ||
1229 | // we are moving down on a surface | ||
1230 | if (ctz.Z == 0) | ||
1231 | { | ||
1232 | if (vel.Z > 0) | ||
1233 | vec.Z -= vel.Z * PID_D * 2f; | ||
1234 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1235 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1236 | } | ||
1237 | // intencionally going down | ||
1238 | else | ||
1239 | { | ||
1240 | if (ctz.Z < vel.Z) | ||
1241 | vec.Z += (ctz.Z - vel.Z) * PID_D; | ||
1242 | else | ||
1243 | { | ||
1244 | } | ||
1245 | |||
1246 | if (Math.Abs(ctz.X) > Math.Abs(vel.X)) | ||
1247 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1248 | if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) | ||
1249 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1250 | } | ||
1251 | } | ||
1252 | |||
1253 | // We're standing on something | ||
1254 | } | ||
1255 | else | ||
1256 | { | ||
1257 | // We're flying and colliding with something | ||
1258 | vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); | ||
1259 | vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); | ||
1260 | vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); | ||
1261 | } | ||
1262 | } | ||
1263 | else // ie not colliding | ||
1264 | { | ||
1265 | if (flying) //(!m_iscolliding && flying) | ||
1266 | { | ||
1267 | // we're in mid air suspended | ||
1268 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1269 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1270 | vec.Z += (ctz.Z - vel.Z) * (PID_D); | ||
1271 | } | ||
1272 | |||
1273 | else | ||
1274 | { | ||
1275 | // we're not colliding and we're not flying so that means we're falling! | ||
1276 | // m_iscolliding includes collisions with the ground. | ||
1277 | |||
1278 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
1279 | vec.X += (ctz.X - vel.X) * PID_D * 0.833f; | ||
1280 | vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; | ||
1281 | // hack for breaking on fall | ||
1282 | if (ctz.Z == -9999f) | ||
1283 | vec.Z += -vel.Z * PID_D - _parent_scene.gravityz * m_mass; | ||
1284 | } | ||
1285 | } | ||
1286 | } | ||
1287 | |||
1288 | if (velLengthSquared > 2500.0f) // 50m/s apply breaks | ||
1289 | { | ||
1290 | breakfactor = 0.16f * m_mass; | ||
1291 | vec.X -= breakfactor * vel.X; | ||
1292 | vec.Y -= breakfactor * vel.Y; | ||
1293 | vec.Z -= breakfactor * vel.Z; | ||
1294 | } | ||
1295 | } | ||
1296 | else | ||
1297 | { | ||
1298 | breakfactor = m_mass; | ||
1299 | vec.X -= breakfactor * vel.X; | ||
1300 | vec.Y -= breakfactor * vel.Y; | ||
1301 | if (flying) | ||
1302 | vec.Z -= 0.5f * breakfactor * vel.Z; | ||
1303 | else | ||
1304 | vec.Z -= .16f* m_mass * vel.Z; | ||
1305 | } | ||
1306 | |||
1307 | if (flying) | ||
1308 | { | ||
1309 | vec.Z -= _parent_scene.gravityz * m_mass; | ||
1310 | |||
1311 | //Added for auto fly height. Kitto Flora | ||
1312 | float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; | ||
1313 | |||
1314 | if (localpos.Z < target_altitude) | ||
1315 | { | ||
1316 | vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; | ||
1317 | } | ||
1318 | // end add Kitto Flora | ||
1319 | } | ||
1320 | |||
1321 | if (vec.IsFinite()) | ||
1322 | { | ||
1323 | if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) | ||
1324 | d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); | ||
1325 | } | ||
1326 | else | ||
1327 | { | ||
1328 | m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); | ||
1329 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
1330 | defects.Add(this); | ||
1331 | // _parent_scene.RemoveCharacter(this); | ||
1332 | // destroy avatar capsule and related ODE data | ||
1333 | AvatarGeomAndBodyDestroy(); | ||
1334 | return; | ||
1335 | } | ||
1336 | |||
1337 | // update our local ideia of position velocity and aceleration | ||
1338 | // _position = localpos; | ||
1339 | _position = localpos; | ||
1340 | |||
1341 | if (_zeroFlag) | ||
1342 | { | ||
1343 | _velocity = Vector3.Zero; | ||
1344 | _acceleration = Vector3.Zero; | ||
1345 | m_rotationalVelocity = Vector3.Zero; | ||
1346 | } | ||
1347 | else | ||
1348 | { | ||
1349 | Vector3 a =_velocity; // previus velocity | ||
1350 | SetSmooth(ref _velocity, ref vel, 2); | ||
1351 | a = (_velocity - a) * invtimeStep; | ||
1352 | SetSmooth(ref _acceleration, ref a, 2); | ||
1353 | |||
1354 | dtmp = d.BodyGetAngularVel(Body); | ||
1355 | m_rotationalVelocity.X = 0f; | ||
1356 | m_rotationalVelocity.Y = 0f; | ||
1357 | m_rotationalVelocity.Z = dtmp.Z; | ||
1358 | Math.Round(m_rotationalVelocity.Z,3); | ||
1359 | } | ||
1360 | } | ||
1361 | |||
1362 | public void round(ref Vector3 v, int digits) | ||
1363 | { | ||
1364 | v.X = (float)Math.Round(v.X, digits); | ||
1365 | v.Y = (float)Math.Round(v.Y, digits); | ||
1366 | v.Z = (float)Math.Round(v.Z, digits); | ||
1367 | } | ||
1368 | |||
1369 | public void SetSmooth(ref Vector3 dst, ref Vector3 value) | ||
1370 | { | ||
1371 | dst.X = 0.1f * dst.X + 0.9f * value.X; | ||
1372 | dst.Y = 0.1f * dst.Y + 0.9f * value.Y; | ||
1373 | dst.Z = 0.1f * dst.Z + 0.9f * value.Z; | ||
1374 | } | ||
1375 | |||
1376 | public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) | ||
1377 | { | ||
1378 | dst.X = 0.4f * dst.X + 0.6f * value.X; | ||
1379 | dst.X = (float)Math.Round(dst.X, rounddigits); | ||
1380 | |||
1381 | dst.Y = 0.4f * dst.Y + 0.6f * value.Y; | ||
1382 | dst.Y = (float)Math.Round(dst.Y, rounddigits); | ||
1383 | |||
1384 | dst.Z = 0.4f * dst.Z + 0.6f * value.Z; | ||
1385 | dst.Z = (float)Math.Round(dst.Z, rounddigits); | ||
1386 | } | ||
1387 | |||
1388 | |||
1389 | /// <summary> | ||
1390 | /// Updates the reported position and velocity. | ||
1391 | /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording | ||
1392 | /// also outbounds checking | ||
1393 | /// copy and outbounds now done in move(..) at ode rate | ||
1394 | /// | ||
1395 | /// </summary> | ||
1396 | public void UpdatePositionAndVelocity() | ||
1397 | { | ||
1398 | return; | ||
1399 | |||
1400 | // if (Body == IntPtr.Zero) | ||
1401 | // return; | ||
1402 | |||
1403 | } | ||
1404 | |||
1405 | /// <summary> | ||
1406 | /// Cleanup the things we use in the scene. | ||
1407 | /// </summary> | ||
1408 | public void Destroy() | ||
1409 | { | ||
1410 | AddChange(changes.Remove, null); | ||
1411 | } | ||
1412 | |||
1413 | public override void CrossingFailure() | ||
1414 | { | ||
1415 | } | ||
1416 | |||
1417 | public override Vector3 PIDTarget { set { return; } } | ||
1418 | public override bool PIDActive {get {return m_pidControllerActive;} set { return; } } | ||
1419 | public override float PIDTau { set { return; } } | ||
1420 | |||
1421 | public override float PIDHoverHeight { set { return; } } | ||
1422 | public override bool PIDHoverActive { set { return; } } | ||
1423 | public override PIDHoverType PIDHoverType { set { return; } } | ||
1424 | public override float PIDHoverTau { set { return; } } | ||
1425 | |||
1426 | public override Quaternion APIDTarget { set { return; } } | ||
1427 | |||
1428 | public override bool APIDActive { set { return; } } | ||
1429 | |||
1430 | public override float APIDStrength { set { return; } } | ||
1431 | |||
1432 | public override float APIDDamping { set { return; } } | ||
1433 | |||
1434 | |||
1435 | public override void SubscribeEvents(int ms) | ||
1436 | { | ||
1437 | m_eventsubscription = ms; | ||
1438 | m_cureventsubscription = 0; | ||
1439 | if (CollisionEventsThisFrame == null) | ||
1440 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1441 | SentEmptyCollisionsEvent = false; | ||
1442 | } | ||
1443 | |||
1444 | public override void UnSubscribeEvents() | ||
1445 | { | ||
1446 | if (CollisionEventsThisFrame != null) | ||
1447 | { | ||
1448 | lock (CollisionEventsThisFrame) | ||
1449 | { | ||
1450 | CollisionEventsThisFrame.Clear(); | ||
1451 | CollisionEventsThisFrame = null; | ||
1452 | } | ||
1453 | } | ||
1454 | m_eventsubscription = 0; | ||
1455 | } | ||
1456 | |||
1457 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1458 | { | ||
1459 | if (CollisionEventsThisFrame == null) | ||
1460 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1461 | lock (CollisionEventsThisFrame) | ||
1462 | { | ||
1463 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1464 | _parent_scene.AddCollisionEventReporting(this); | ||
1465 | } | ||
1466 | } | ||
1467 | |||
1468 | public void SendCollisions() | ||
1469 | { | ||
1470 | if (CollisionEventsThisFrame == null) | ||
1471 | return; | ||
1472 | |||
1473 | lock (CollisionEventsThisFrame) | ||
1474 | { | ||
1475 | if (m_cureventsubscription < m_eventsubscription) | ||
1476 | return; | ||
1477 | |||
1478 | m_cureventsubscription = 0; | ||
1479 | |||
1480 | int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; | ||
1481 | |||
1482 | if (!SentEmptyCollisionsEvent || ncolisions > 0) | ||
1483 | { | ||
1484 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1485 | |||
1486 | if (ncolisions == 0) | ||
1487 | { | ||
1488 | SentEmptyCollisionsEvent = true; | ||
1489 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1490 | } | ||
1491 | else | ||
1492 | { | ||
1493 | SentEmptyCollisionsEvent = false; | ||
1494 | CollisionEventsThisFrame.Clear(); | ||
1495 | } | ||
1496 | } | ||
1497 | } | ||
1498 | } | ||
1499 | |||
1500 | internal void AddCollisionFrameTime(int t) | ||
1501 | { | ||
1502 | // protect it from overflow crashing | ||
1503 | if (m_cureventsubscription < 50000) | ||
1504 | m_cureventsubscription += t; | ||
1505 | } | ||
1506 | |||
1507 | public override bool SubscribedEvents() | ||
1508 | { | ||
1509 | if (m_eventsubscription > 0) | ||
1510 | return true; | ||
1511 | return false; | ||
1512 | } | ||
1513 | |||
1514 | private void changePhysicsStatus(bool NewStatus) | ||
1515 | { | ||
1516 | if (NewStatus != m_isPhysical) | ||
1517 | { | ||
1518 | if (NewStatus) | ||
1519 | { | ||
1520 | AvatarGeomAndBodyDestroy(); | ||
1521 | |||
1522 | AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); | ||
1523 | |||
1524 | _parent_scene.actor_name_map[collider] = (PhysicsActor)this; | ||
1525 | _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; | ||
1526 | _parent_scene.AddCharacter(this); | ||
1527 | } | ||
1528 | else | ||
1529 | { | ||
1530 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1531 | _parent_scene.RemoveCharacter(this); | ||
1532 | // destroy avatar capsule and related ODE data | ||
1533 | AvatarGeomAndBodyDestroy(); | ||
1534 | } | ||
1535 | m_freemove = false; | ||
1536 | m_isPhysical = NewStatus; | ||
1537 | } | ||
1538 | } | ||
1539 | |||
1540 | private void changeAdd() | ||
1541 | { | ||
1542 | changePhysicsStatus(true); | ||
1543 | } | ||
1544 | |||
1545 | private void changeRemove() | ||
1546 | { | ||
1547 | changePhysicsStatus(false); | ||
1548 | } | ||
1549 | |||
1550 | private void changeShape(PrimitiveBaseShape arg) | ||
1551 | { | ||
1552 | } | ||
1553 | |||
1554 | private void changeAvatarSize(strAvatarSize st) | ||
1555 | { | ||
1556 | m_feetOffset = st.offset; | ||
1557 | changeSize(st.size); | ||
1558 | } | ||
1559 | |||
1560 | private void changeSize(Vector3 pSize) | ||
1561 | { | ||
1562 | if (pSize.IsFinite()) | ||
1563 | { | ||
1564 | // for now only look to Z changes since viewers also don't change X and Y | ||
1565 | if (pSize.Z != m_size.Z) | ||
1566 | { | ||
1567 | AvatarGeomAndBodyDestroy(); | ||
1568 | |||
1569 | |||
1570 | float oldsz = m_size.Z; | ||
1571 | m_size = pSize; | ||
1572 | |||
1573 | |||
1574 | AvatarGeomAndBodyCreation(_position.X, _position.Y, | ||
1575 | _position.Z + (m_size.Z - oldsz) * 0.5f); | ||
1576 | |||
1577 | Velocity = Vector3.Zero; | ||
1578 | |||
1579 | |||
1580 | _parent_scene.actor_name_map[collider] = (PhysicsActor)this; | ||
1581 | _parent_scene.actor_name_map[capsule] = (PhysicsActor)this; | ||
1582 | } | ||
1583 | m_freemove = false; | ||
1584 | m_pidControllerActive = true; | ||
1585 | } | ||
1586 | else | ||
1587 | { | ||
1588 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
1589 | } | ||
1590 | } | ||
1591 | |||
1592 | private void changePosition( Vector3 newPos) | ||
1593 | { | ||
1594 | if (Body != IntPtr.Zero) | ||
1595 | d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); | ||
1596 | _position = newPos; | ||
1597 | m_freemove = false; | ||
1598 | m_pidControllerActive = true; | ||
1599 | } | ||
1600 | |||
1601 | private void changeOrientation(Quaternion newOri) | ||
1602 | { | ||
1603 | if (m_orientation != newOri) | ||
1604 | { | ||
1605 | m_orientation = newOri; // keep a copy for core use | ||
1606 | // but only use rotations around Z | ||
1607 | |||
1608 | m_orientation2D.W = newOri.W; | ||
1609 | m_orientation2D.Z = newOri.Z; | ||
1610 | |||
1611 | float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; | ||
1612 | if (t > 0) | ||
1613 | { | ||
1614 | t = 1.0f / (float)Math.Sqrt(t); | ||
1615 | m_orientation2D.W *= t; | ||
1616 | m_orientation2D.Z *= t; | ||
1617 | } | ||
1618 | else | ||
1619 | { | ||
1620 | m_orientation2D.W = 1.0f; | ||
1621 | m_orientation2D.Z = 0f; | ||
1622 | } | ||
1623 | m_orientation2D.Y = 0f; | ||
1624 | m_orientation2D.X = 0f; | ||
1625 | |||
1626 | d.Quaternion myrot = new d.Quaternion(); | ||
1627 | myrot.X = m_orientation2D.X; | ||
1628 | myrot.Y = m_orientation2D.Y; | ||
1629 | myrot.Z = m_orientation2D.Z; | ||
1630 | myrot.W = m_orientation2D.W; | ||
1631 | d.BodySetQuaternion(Body, ref myrot); | ||
1632 | } | ||
1633 | } | ||
1634 | |||
1635 | private void changeVelocity(Vector3 newVel) | ||
1636 | { | ||
1637 | m_pidControllerActive = true; | ||
1638 | m_freemove = false; | ||
1639 | _target_velocity = newVel; | ||
1640 | } | ||
1641 | |||
1642 | private void changeSetTorque(Vector3 newTorque) | ||
1643 | { | ||
1644 | } | ||
1645 | |||
1646 | private void changeAddForce(Vector3 newForce) | ||
1647 | { | ||
1648 | } | ||
1649 | |||
1650 | private void changeAddAngularForce(Vector3 arg) | ||
1651 | { | ||
1652 | } | ||
1653 | |||
1654 | private void changeAngularLock(Vector3 arg) | ||
1655 | { | ||
1656 | } | ||
1657 | |||
1658 | private void changeFloatOnWater(bool arg) | ||
1659 | { | ||
1660 | } | ||
1661 | |||
1662 | private void changeVolumedetetion(bool arg) | ||
1663 | { | ||
1664 | } | ||
1665 | |||
1666 | private void changeSelectedStatus(bool arg) | ||
1667 | { | ||
1668 | } | ||
1669 | |||
1670 | private void changeDisable(bool arg) | ||
1671 | { | ||
1672 | } | ||
1673 | |||
1674 | private void changeBuilding(bool arg) | ||
1675 | { | ||
1676 | } | ||
1677 | |||
1678 | private void setFreeMove() | ||
1679 | { | ||
1680 | m_pidControllerActive = true; | ||
1681 | _zeroFlag = false; | ||
1682 | _target_velocity = Vector3.Zero; | ||
1683 | m_freemove = true; | ||
1684 | m_colliderfilter = -1; | ||
1685 | m_colliderObjectfilter = -1; | ||
1686 | m_colliderGroundfilter = -1; | ||
1687 | |||
1688 | m_iscolliding = false; | ||
1689 | m_iscollidingGround = false; | ||
1690 | m_iscollidingObj = false; | ||
1691 | |||
1692 | CollisionEventsThisFrame.Clear(); | ||
1693 | } | ||
1694 | |||
1695 | private void changeForce(Vector3 newForce) | ||
1696 | { | ||
1697 | setFreeMove(); | ||
1698 | |||
1699 | if (Body != IntPtr.Zero) | ||
1700 | { | ||
1701 | if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) | ||
1702 | d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); | ||
1703 | } | ||
1704 | } | ||
1705 | |||
1706 | // for now momentum is actually velocity | ||
1707 | private void changeMomentum(Vector3 newmomentum) | ||
1708 | { | ||
1709 | _velocity = newmomentum; | ||
1710 | setFreeMove(); | ||
1711 | |||
1712 | if (Body != IntPtr.Zero) | ||
1713 | d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); | ||
1714 | } | ||
1715 | |||
1716 | private void donullchange() | ||
1717 | { | ||
1718 | } | ||
1719 | |||
1720 | public bool DoAChange(changes what, object arg) | ||
1721 | { | ||
1722 | if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) | ||
1723 | { | ||
1724 | return false; | ||
1725 | } | ||
1726 | |||
1727 | // nasty switch | ||
1728 | switch (what) | ||
1729 | { | ||
1730 | case changes.Add: | ||
1731 | changeAdd(); | ||
1732 | break; | ||
1733 | case changes.Remove: | ||
1734 | changeRemove(); | ||
1735 | break; | ||
1736 | |||
1737 | case changes.Position: | ||
1738 | changePosition((Vector3)arg); | ||
1739 | break; | ||
1740 | |||
1741 | case changes.Orientation: | ||
1742 | changeOrientation((Quaternion)arg); | ||
1743 | break; | ||
1744 | |||
1745 | case changes.PosOffset: | ||
1746 | donullchange(); | ||
1747 | break; | ||
1748 | |||
1749 | case changes.OriOffset: | ||
1750 | donullchange(); | ||
1751 | break; | ||
1752 | |||
1753 | case changes.Velocity: | ||
1754 | changeVelocity((Vector3)arg); | ||
1755 | break; | ||
1756 | |||
1757 | // case changes.Acceleration: | ||
1758 | // changeacceleration((Vector3)arg); | ||
1759 | // break; | ||
1760 | // case changes.AngVelocity: | ||
1761 | // changeangvelocity((Vector3)arg); | ||
1762 | // break; | ||
1763 | |||
1764 | case changes.Force: | ||
1765 | changeForce((Vector3)arg); | ||
1766 | break; | ||
1767 | |||
1768 | case changes.Torque: | ||
1769 | changeSetTorque((Vector3)arg); | ||
1770 | break; | ||
1771 | |||
1772 | case changes.AddForce: | ||
1773 | changeAddForce((Vector3)arg); | ||
1774 | break; | ||
1775 | |||
1776 | case changes.AddAngForce: | ||
1777 | changeAddAngularForce((Vector3)arg); | ||
1778 | break; | ||
1779 | |||
1780 | case changes.AngLock: | ||
1781 | changeAngularLock((Vector3)arg); | ||
1782 | break; | ||
1783 | |||
1784 | case changes.Size: | ||
1785 | changeSize((Vector3)arg); | ||
1786 | break; | ||
1787 | |||
1788 | case changes.AvatarSize: | ||
1789 | changeAvatarSize((strAvatarSize)arg); | ||
1790 | break; | ||
1791 | |||
1792 | case changes.Momentum: | ||
1793 | changeMomentum((Vector3)arg); | ||
1794 | break; | ||
1795 | /* not in use for now | ||
1796 | case changes.Shape: | ||
1797 | changeShape((PrimitiveBaseShape)arg); | ||
1798 | break; | ||
1799 | |||
1800 | case changes.CollidesWater: | ||
1801 | changeFloatOnWater((bool)arg); | ||
1802 | break; | ||
1803 | |||
1804 | case changes.VolumeDtc: | ||
1805 | changeVolumedetetion((bool)arg); | ||
1806 | break; | ||
1807 | |||
1808 | case changes.Physical: | ||
1809 | changePhysicsStatus((bool)arg); | ||
1810 | break; | ||
1811 | |||
1812 | case changes.Selected: | ||
1813 | changeSelectedStatus((bool)arg); | ||
1814 | break; | ||
1815 | |||
1816 | case changes.disabled: | ||
1817 | changeDisable((bool)arg); | ||
1818 | break; | ||
1819 | |||
1820 | case changes.building: | ||
1821 | changeBuilding((bool)arg); | ||
1822 | break; | ||
1823 | */ | ||
1824 | case changes.Null: | ||
1825 | donullchange(); | ||
1826 | break; | ||
1827 | |||
1828 | default: | ||
1829 | donullchange(); | ||
1830 | break; | ||
1831 | } | ||
1832 | return false; | ||
1833 | } | ||
1834 | |||
1835 | public void AddChange(changes what, object arg) | ||
1836 | { | ||
1837 | _parent_scene.AddChange((PhysicsActor)this, what, arg); | ||
1838 | } | ||
1839 | |||
1840 | private struct strAvatarSize | ||
1841 | { | ||
1842 | public Vector3 size; | ||
1843 | public float offset; | ||
1844 | } | ||
1845 | |||
1846 | } | ||
1847 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs new file mode 100644 index 0000000..9f1ab4c --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODEDynamics.cs | |||
@@ -0,0 +1,1096 @@ | |||
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 | // Extensive change Ubit 2012 | ||
42 | |||
43 | using System; | ||
44 | using System.Collections.Generic; | ||
45 | using System.Reflection; | ||
46 | using System.Runtime.InteropServices; | ||
47 | using log4net; | ||
48 | using OpenMetaverse; | ||
49 | using OdeAPI; | ||
50 | using OpenSim.Framework; | ||
51 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
52 | |||
53 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
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 | // WARNING this are working copies for internel use | ||
67 | // their values may not be the corresponding parameter | ||
68 | |||
69 | private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier | ||
70 | private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? | ||
71 | |||
72 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind | ||
73 | |||
74 | private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: | ||
75 | // HOVER_TERRAIN_ONLY | ||
76 | // HOVER_GLOBAL_HEIGHT | ||
77 | // NO_DEFLECTION_UP | ||
78 | // HOVER_WATER_ONLY | ||
79 | // HOVER_UP_ONLY | ||
80 | // LIMIT_MOTOR_UP | ||
81 | // LIMIT_ROLL_ONLY | ||
82 | private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl | ||
83 | |||
84 | // Linear properties | ||
85 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time | ||
86 | private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
87 | private float m_linearMotorDecayTimescale = 120; | ||
88 | private float m_linearMotorTimescale = 1000; | ||
89 | private Vector3 m_linearMotorOffset = Vector3.Zero; | ||
90 | |||
91 | //Angular properties | ||
92 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
93 | private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate | ||
94 | private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate | ||
95 | private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate | ||
96 | |||
97 | //Deflection properties | ||
98 | private float m_angularDeflectionEfficiency = 0; | ||
99 | private float m_angularDeflectionTimescale = 1000; | ||
100 | private float m_linearDeflectionEfficiency = 0; | ||
101 | private float m_linearDeflectionTimescale = 1000; | ||
102 | |||
103 | //Banking properties | ||
104 | private float m_bankingEfficiency = 0; | ||
105 | private float m_bankingMix = 0; | ||
106 | private float m_bankingTimescale = 1000; | ||
107 | |||
108 | //Hover and Buoyancy properties | ||
109 | private float m_VhoverHeight = 0f; | ||
110 | private float m_VhoverEfficiency = 0f; | ||
111 | private float m_VhoverTimescale = 1000f; | ||
112 | private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. | ||
113 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | ||
114 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | ||
115 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
116 | |||
117 | //Attractor properties | ||
118 | private float m_verticalAttractionEfficiency = 1.0f; // damped | ||
119 | private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. | ||
120 | |||
121 | |||
122 | // auxiliar | ||
123 | private float m_lmEfect = 0f; // current linear motor eficiency | ||
124 | private float m_lmDecay = 0f; // current linear decay | ||
125 | |||
126 | private float m_amEfect = 0; // current angular motor eficiency | ||
127 | private float m_amDecay = 0f; // current linear decay | ||
128 | |||
129 | private float m_ffactor = 1.0f; | ||
130 | |||
131 | private float m_timestep = 0.02f; | ||
132 | private float m_invtimestep = 50; | ||
133 | |||
134 | |||
135 | float m_ampwr; | ||
136 | float m_amdampX; | ||
137 | float m_amdampY; | ||
138 | float m_amdampZ; | ||
139 | |||
140 | float m_gravmod; | ||
141 | |||
142 | public float FrictionFactor | ||
143 | { | ||
144 | get | ||
145 | { | ||
146 | return m_ffactor; | ||
147 | } | ||
148 | } | ||
149 | |||
150 | public float GravMod | ||
151 | { | ||
152 | set | ||
153 | { | ||
154 | m_gravmod = value; | ||
155 | } | ||
156 | } | ||
157 | |||
158 | |||
159 | public ODEDynamics(OdePrim rootp) | ||
160 | { | ||
161 | rootPrim = rootp; | ||
162 | _pParentScene = rootPrim._parent_scene; | ||
163 | m_timestep = _pParentScene.ODE_STEPSIZE; | ||
164 | m_invtimestep = 1.0f / m_timestep; | ||
165 | m_gravmod = rootPrim.GravModifier; | ||
166 | } | ||
167 | |||
168 | public void DoSetVehicle(VehicleData vd) | ||
169 | { | ||
170 | m_type = vd.m_type; | ||
171 | m_flags = vd.m_flags; | ||
172 | |||
173 | |||
174 | // Linear properties | ||
175 | m_linearMotorDirection = vd.m_linearMotorDirection; | ||
176 | |||
177 | m_linearFrictionTimescale = vd.m_linearFrictionTimescale; | ||
178 | if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; | ||
179 | if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; | ||
180 | if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; | ||
181 | |||
182 | m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; | ||
183 | if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; | ||
184 | m_linearMotorDecayTimescale += 0.2f; | ||
185 | m_linearMotorDecayTimescale *= m_invtimestep; | ||
186 | |||
187 | m_linearMotorTimescale = vd.m_linearMotorTimescale; | ||
188 | if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; | ||
189 | |||
190 | m_linearMotorOffset = vd.m_linearMotorOffset; | ||
191 | |||
192 | //Angular properties | ||
193 | m_angularMotorDirection = vd.m_angularMotorDirection; | ||
194 | m_angularMotorTimescale = vd.m_angularMotorTimescale; | ||
195 | if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; | ||
196 | |||
197 | m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; | ||
198 | if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; | ||
199 | m_angularMotorDecayTimescale *= m_invtimestep; | ||
200 | |||
201 | m_angularFrictionTimescale = vd.m_angularFrictionTimescale; | ||
202 | if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; | ||
203 | if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; | ||
204 | if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; | ||
205 | |||
206 | //Deflection properties | ||
207 | m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; | ||
208 | m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; | ||
209 | if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; | ||
210 | |||
211 | m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; | ||
212 | m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; | ||
213 | if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; | ||
214 | |||
215 | //Banking properties | ||
216 | m_bankingEfficiency = vd.m_bankingEfficiency; | ||
217 | m_bankingMix = vd.m_bankingMix; | ||
218 | m_bankingTimescale = vd.m_bankingTimescale; | ||
219 | if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; | ||
220 | |||
221 | //Hover and Buoyancy properties | ||
222 | m_VhoverHeight = vd.m_VhoverHeight; | ||
223 | m_VhoverEfficiency = vd.m_VhoverEfficiency; | ||
224 | m_VhoverTimescale = vd.m_VhoverTimescale; | ||
225 | if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; | ||
226 | |||
227 | m_VehicleBuoyancy = vd.m_VehicleBuoyancy; | ||
228 | |||
229 | //Attractor properties | ||
230 | m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; | ||
231 | m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; | ||
232 | if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; | ||
233 | |||
234 | // Axis | ||
235 | m_referenceFrame = vd.m_referenceFrame; | ||
236 | |||
237 | m_lmEfect = 0; | ||
238 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
239 | m_amEfect = 0; | ||
240 | m_ffactor = 1.0f; | ||
241 | } | ||
242 | |||
243 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
244 | { | ||
245 | float len; | ||
246 | |||
247 | switch (pParam) | ||
248 | { | ||
249 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
250 | if (pValue < 0f) pValue = 0f; | ||
251 | if (pValue > 1f) pValue = 1f; | ||
252 | m_angularDeflectionEfficiency = pValue; | ||
253 | break; | ||
254 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
255 | if (pValue < m_timestep) pValue = m_timestep; | ||
256 | m_angularDeflectionTimescale = pValue; | ||
257 | break; | ||
258 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
259 | if (pValue < m_timestep) pValue = m_timestep; | ||
260 | else if (pValue > 120) pValue = 120; | ||
261 | m_angularMotorDecayTimescale = pValue * m_invtimestep; | ||
262 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
263 | break; | ||
264 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
265 | if (pValue < m_timestep) pValue = m_timestep; | ||
266 | m_angularMotorTimescale = pValue; | ||
267 | break; | ||
268 | case Vehicle.BANKING_EFFICIENCY: | ||
269 | if (pValue < -1f) pValue = -1f; | ||
270 | if (pValue > 1f) pValue = 1f; | ||
271 | m_bankingEfficiency = pValue; | ||
272 | break; | ||
273 | case Vehicle.BANKING_MIX: | ||
274 | if (pValue < 0f) pValue = 0f; | ||
275 | if (pValue > 1f) pValue = 1f; | ||
276 | m_bankingMix = pValue; | ||
277 | break; | ||
278 | case Vehicle.BANKING_TIMESCALE: | ||
279 | if (pValue < m_timestep) pValue = m_timestep; | ||
280 | m_bankingTimescale = pValue; | ||
281 | break; | ||
282 | case Vehicle.BUOYANCY: | ||
283 | if (pValue < -1f) pValue = -1f; | ||
284 | if (pValue > 1f) pValue = 1f; | ||
285 | m_VehicleBuoyancy = pValue; | ||
286 | break; | ||
287 | case Vehicle.HOVER_EFFICIENCY: | ||
288 | if (pValue < 0f) pValue = 0f; | ||
289 | if (pValue > 1f) pValue = 1f; | ||
290 | m_VhoverEfficiency = pValue; | ||
291 | break; | ||
292 | case Vehicle.HOVER_HEIGHT: | ||
293 | m_VhoverHeight = pValue; | ||
294 | break; | ||
295 | case Vehicle.HOVER_TIMESCALE: | ||
296 | if (pValue < m_timestep) pValue = m_timestep; | ||
297 | m_VhoverTimescale = pValue; | ||
298 | break; | ||
299 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
300 | if (pValue < 0f) pValue = 0f; | ||
301 | if (pValue > 1f) pValue = 1f; | ||
302 | m_linearDeflectionEfficiency = pValue; | ||
303 | break; | ||
304 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
305 | if (pValue < m_timestep) pValue = m_timestep; | ||
306 | m_linearDeflectionTimescale = pValue; | ||
307 | break; | ||
308 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
309 | if (pValue < m_timestep) pValue = m_timestep; | ||
310 | else if (pValue > 120) pValue = 120; | ||
311 | m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; | ||
312 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
313 | break; | ||
314 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
315 | if (pValue < m_timestep) pValue = m_timestep; | ||
316 | m_linearMotorTimescale = pValue; | ||
317 | break; | ||
318 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
319 | if (pValue < 0f) pValue = 0f; | ||
320 | if (pValue > 1f) pValue = 1f; | ||
321 | m_verticalAttractionEfficiency = pValue; | ||
322 | break; | ||
323 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
324 | if (pValue < m_timestep) pValue = m_timestep; | ||
325 | m_verticalAttractionTimescale = pValue; | ||
326 | break; | ||
327 | |||
328 | // These are vector properties but the engine lets you use a single float value to | ||
329 | // set all of the components to the same value | ||
330 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
331 | if (pValue < m_timestep) pValue = m_timestep; | ||
332 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
333 | break; | ||
334 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
335 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
336 | len = m_angularMotorDirection.Length(); | ||
337 | if (len > 12.566f) | ||
338 | m_angularMotorDirection *= (12.566f / len); | ||
339 | |||
340 | m_amEfect = 1.0f ; // turn it on | ||
341 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
342 | |||
343 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
344 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
345 | d.BodyEnable(rootPrim.Body); | ||
346 | break; | ||
347 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
348 | if (pValue < m_timestep) pValue = m_timestep; | ||
349 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
350 | break; | ||
351 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
352 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
353 | len = m_linearMotorDirection.Length(); | ||
354 | if (len > 100.0f) | ||
355 | m_linearMotorDirection *= (100.0f / len); | ||
356 | |||
357 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
358 | m_lmEfect = 1.0f; // turn it on | ||
359 | |||
360 | m_ffactor = 0.0f; | ||
361 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
362 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
363 | d.BodyEnable(rootPrim.Body); | ||
364 | break; | ||
365 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
366 | m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
367 | len = m_linearMotorOffset.Length(); | ||
368 | if (len > 100.0f) | ||
369 | m_linearMotorOffset *= (100.0f / len); | ||
370 | break; | ||
371 | } | ||
372 | }//end ProcessFloatVehicleParam | ||
373 | |||
374 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
375 | { | ||
376 | float len; | ||
377 | |||
378 | switch (pParam) | ||
379 | { | ||
380 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
381 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
382 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
383 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
384 | |||
385 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
386 | break; | ||
387 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
388 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
389 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
390 | len = m_angularMotorDirection.Length(); | ||
391 | if (len > 12.566f) | ||
392 | m_angularMotorDirection *= (12.566f / len); | ||
393 | |||
394 | m_amEfect = 1.0f; // turn it on | ||
395 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
396 | |||
397 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
398 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
399 | d.BodyEnable(rootPrim.Body); | ||
400 | break; | ||
401 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
402 | if (pValue.X < m_timestep) pValue.X = m_timestep; | ||
403 | if (pValue.Y < m_timestep) pValue.Y = m_timestep; | ||
404 | if (pValue.Z < m_timestep) pValue.Z = m_timestep; | ||
405 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
406 | break; | ||
407 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
408 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
409 | len = m_linearMotorDirection.Length(); | ||
410 | if (len > 100.0f) | ||
411 | m_linearMotorDirection *= (100.0f / len); | ||
412 | |||
413 | m_lmEfect = 1.0f; // turn it on | ||
414 | m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; | ||
415 | |||
416 | m_ffactor = 0.0f; | ||
417 | if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) | ||
418 | && !rootPrim.m_isSelected && !rootPrim.m_disabled) | ||
419 | d.BodyEnable(rootPrim.Body); | ||
420 | break; | ||
421 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
422 | m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
423 | len = m_linearMotorOffset.Length(); | ||
424 | if (len > 100.0f) | ||
425 | m_linearMotorOffset *= (100.0f / len); | ||
426 | break; | ||
427 | case Vehicle.BLOCK_EXIT: | ||
428 | m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
429 | break; | ||
430 | } | ||
431 | }//end ProcessVectorVehicleParam | ||
432 | |||
433 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
434 | { | ||
435 | switch (pParam) | ||
436 | { | ||
437 | case Vehicle.REFERENCE_FRAME: | ||
438 | // m_referenceFrame = Quaternion.Inverse(pValue); | ||
439 | m_referenceFrame = pValue; | ||
440 | break; | ||
441 | case Vehicle.ROLL_FRAME: | ||
442 | m_RollreferenceFrame = pValue; | ||
443 | break; | ||
444 | } | ||
445 | }//end ProcessRotationVehicleParam | ||
446 | |||
447 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
448 | { | ||
449 | if (remove) | ||
450 | { | ||
451 | m_flags &= ~((VehicleFlag)pParam); | ||
452 | } | ||
453 | else | ||
454 | { | ||
455 | m_flags |= (VehicleFlag)pParam; | ||
456 | } | ||
457 | }//end ProcessVehicleFlags | ||
458 | |||
459 | internal void ProcessTypeChange(Vehicle pType) | ||
460 | { | ||
461 | m_lmEfect = 0; | ||
462 | |||
463 | m_amEfect = 0; | ||
464 | m_ffactor = 1f; | ||
465 | |||
466 | m_linearMotorDirection = Vector3.Zero; | ||
467 | m_angularMotorDirection = Vector3.Zero; | ||
468 | |||
469 | m_BlockingEndPoint = Vector3.Zero; | ||
470 | m_RollreferenceFrame = Quaternion.Identity; | ||
471 | m_linearMotorOffset = Vector3.Zero; | ||
472 | |||
473 | m_referenceFrame = Quaternion.Identity; | ||
474 | |||
475 | // Set Defaults For Type | ||
476 | m_type = pType; | ||
477 | switch (pType) | ||
478 | { | ||
479 | case Vehicle.TYPE_NONE: | ||
480 | m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
481 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
482 | m_linearMotorTimescale = 1000; | ||
483 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
484 | m_angularMotorTimescale = 1000; | ||
485 | m_angularMotorDecayTimescale = 1000 * m_invtimestep; | ||
486 | m_VhoverHeight = 0; | ||
487 | m_VhoverEfficiency = 1; | ||
488 | m_VhoverTimescale = 1000; | ||
489 | m_VehicleBuoyancy = 0; | ||
490 | m_linearDeflectionEfficiency = 0; | ||
491 | m_linearDeflectionTimescale = 1000; | ||
492 | m_angularDeflectionEfficiency = 0; | ||
493 | m_angularDeflectionTimescale = 1000; | ||
494 | m_bankingEfficiency = 0; | ||
495 | m_bankingMix = 1; | ||
496 | m_bankingTimescale = 1000; | ||
497 | m_verticalAttractionEfficiency = 0; | ||
498 | m_verticalAttractionTimescale = 1000; | ||
499 | |||
500 | m_flags = (VehicleFlag)0; | ||
501 | break; | ||
502 | |||
503 | case Vehicle.TYPE_SLED: | ||
504 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
505 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
506 | m_linearMotorTimescale = 1000; | ||
507 | m_linearMotorDecayTimescale = 120 * m_invtimestep; | ||
508 | m_angularMotorTimescale = 1000; | ||
509 | m_angularMotorDecayTimescale = 120 * m_invtimestep; | ||
510 | m_VhoverHeight = 0; | ||
511 | m_VhoverEfficiency = 1; | ||
512 | m_VhoverTimescale = 10; | ||
513 | m_VehicleBuoyancy = 0; | ||
514 | m_linearDeflectionEfficiency = 1; | ||
515 | m_linearDeflectionTimescale = 1; | ||
516 | m_angularDeflectionEfficiency = 0; | ||
517 | m_angularDeflectionTimescale = 10; | ||
518 | m_verticalAttractionEfficiency = 1; | ||
519 | m_verticalAttractionTimescale = 1000; | ||
520 | m_bankingEfficiency = 0; | ||
521 | m_bankingMix = 1; | ||
522 | m_bankingTimescale = 10; | ||
523 | m_flags &= | ||
524 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
525 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
526 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
527 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
528 | VehicleFlag.LIMIT_MOTOR_UP); | ||
529 | break; | ||
530 | |||
531 | case Vehicle.TYPE_CAR: | ||
532 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
533 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
534 | m_linearMotorTimescale = 1; | ||
535 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
536 | m_angularMotorTimescale = 1; | ||
537 | m_angularMotorDecayTimescale = 0.8f * m_invtimestep; | ||
538 | m_VhoverHeight = 0; | ||
539 | m_VhoverEfficiency = 0; | ||
540 | m_VhoverTimescale = 1000; | ||
541 | m_VehicleBuoyancy = 0; | ||
542 | m_linearDeflectionEfficiency = 1; | ||
543 | m_linearDeflectionTimescale = 2; | ||
544 | m_angularDeflectionEfficiency = 0; | ||
545 | m_angularDeflectionTimescale = 10; | ||
546 | m_verticalAttractionEfficiency = 1f; | ||
547 | m_verticalAttractionTimescale = 10f; | ||
548 | m_bankingEfficiency = -0.2f; | ||
549 | m_bankingMix = 1; | ||
550 | m_bankingTimescale = 1; | ||
551 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
552 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
553 | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
554 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
555 | VehicleFlag.LIMIT_ROLL_ONLY | | ||
556 | VehicleFlag.LIMIT_MOTOR_UP | | ||
557 | VehicleFlag.HOVER_UP_ONLY); | ||
558 | break; | ||
559 | case Vehicle.TYPE_BOAT: | ||
560 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
561 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
562 | m_linearMotorTimescale = 5; | ||
563 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
564 | m_angularMotorTimescale = 4; | ||
565 | m_angularMotorDecayTimescale = 4 * m_invtimestep; | ||
566 | m_VhoverHeight = 0; | ||
567 | m_VhoverEfficiency = 0.5f; | ||
568 | m_VhoverTimescale = 2; | ||
569 | m_VehicleBuoyancy = 1; | ||
570 | m_linearDeflectionEfficiency = 0.5f; | ||
571 | m_linearDeflectionTimescale = 3; | ||
572 | m_angularDeflectionEfficiency = 0.5f; | ||
573 | m_angularDeflectionTimescale = 5; | ||
574 | m_verticalAttractionEfficiency = 0.5f; | ||
575 | m_verticalAttractionTimescale = 5f; | ||
576 | m_bankingEfficiency = -0.3f; | ||
577 | m_bankingMix = 0.8f; | ||
578 | m_bankingTimescale = 1; | ||
579 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | | ||
580 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
581 | VehicleFlag.HOVER_UP_ONLY); // | | ||
582 | // VehicleFlag.LIMIT_ROLL_ONLY); | ||
583 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
584 | VehicleFlag.LIMIT_MOTOR_UP | | ||
585 | VehicleFlag.HOVER_UP_ONLY | // new sl | ||
586 | VehicleFlag.HOVER_WATER_ONLY); | ||
587 | break; | ||
588 | |||
589 | case Vehicle.TYPE_AIRPLANE: | ||
590 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
591 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
592 | m_linearMotorTimescale = 2; | ||
593 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
594 | m_angularMotorTimescale = 4; | ||
595 | m_angularMotorDecayTimescale = 8 * m_invtimestep; | ||
596 | m_VhoverHeight = 0; | ||
597 | m_VhoverEfficiency = 0.5f; | ||
598 | m_VhoverTimescale = 1000; | ||
599 | m_VehicleBuoyancy = 0; | ||
600 | m_linearDeflectionEfficiency = 0.5f; | ||
601 | m_linearDeflectionTimescale = 0.5f; | ||
602 | m_angularDeflectionEfficiency = 1; | ||
603 | m_angularDeflectionTimescale = 2; | ||
604 | m_verticalAttractionEfficiency = 0.9f; | ||
605 | m_verticalAttractionTimescale = 2f; | ||
606 | m_bankingEfficiency = 1; | ||
607 | m_bankingMix = 0.7f; | ||
608 | m_bankingTimescale = 2; | ||
609 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
610 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
611 | VehicleFlag.HOVER_GLOBAL_HEIGHT | | ||
612 | VehicleFlag.HOVER_UP_ONLY | | ||
613 | VehicleFlag.NO_DEFLECTION_UP | | ||
614 | VehicleFlag.LIMIT_MOTOR_UP); | ||
615 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
616 | break; | ||
617 | |||
618 | case Vehicle.TYPE_BALLOON: | ||
619 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
620 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
621 | m_linearMotorTimescale = 5; | ||
622 | m_linearMotorDecayTimescale = 60 * m_invtimestep; | ||
623 | m_angularMotorTimescale = 6; | ||
624 | m_angularMotorDecayTimescale = 10 * m_invtimestep; | ||
625 | m_VhoverHeight = 5; | ||
626 | m_VhoverEfficiency = 0.8f; | ||
627 | m_VhoverTimescale = 10; | ||
628 | m_VehicleBuoyancy = 1; | ||
629 | m_linearDeflectionEfficiency = 0; | ||
630 | m_linearDeflectionTimescale = 5 * m_invtimestep; | ||
631 | m_angularDeflectionEfficiency = 0; | ||
632 | m_angularDeflectionTimescale = 5; | ||
633 | m_verticalAttractionEfficiency = 1f; | ||
634 | m_verticalAttractionTimescale = 1000f; | ||
635 | m_bankingEfficiency = 0; | ||
636 | m_bankingMix = 0.7f; | ||
637 | m_bankingTimescale = 5; | ||
638 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | | ||
639 | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
640 | VehicleFlag.HOVER_UP_ONLY | | ||
641 | VehicleFlag.NO_DEFLECTION_UP | | ||
642 | VehicleFlag.LIMIT_MOTOR_UP | //); | ||
643 | VehicleFlag.LIMIT_ROLL_ONLY | // new sl | ||
644 | VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl | ||
645 | |||
646 | // m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | | ||
647 | // VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
648 | break; | ||
649 | |||
650 | } | ||
651 | |||
652 | m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); | ||
653 | m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; | ||
654 | |||
655 | }//end SetDefaultsForType | ||
656 | |||
657 | internal void Stop() | ||
658 | { | ||
659 | m_lmEfect = 0; | ||
660 | m_lmDecay = 0f; | ||
661 | m_amEfect = 0; | ||
662 | m_amDecay = 0; | ||
663 | m_ffactor = 1f; | ||
664 | } | ||
665 | |||
666 | public static Vector3 Xrot(Quaternion rot) | ||
667 | { | ||
668 | Vector3 vec; | ||
669 | rot.Normalize(); // just in case | ||
670 | vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; | ||
671 | vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); | ||
672 | vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); | ||
673 | return vec; | ||
674 | } | ||
675 | |||
676 | public static Vector3 Zrot(Quaternion rot) | ||
677 | { | ||
678 | Vector3 vec; | ||
679 | rot.Normalize(); // just in case | ||
680 | vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); | ||
681 | vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); | ||
682 | vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; | ||
683 | |||
684 | return vec; | ||
685 | } | ||
686 | |||
687 | private const float pi = (float)Math.PI; | ||
688 | private const float halfpi = 0.5f * (float)Math.PI; | ||
689 | private const float twopi = 2.0f * pi; | ||
690 | |||
691 | public static Vector3 ubitRot2Euler(Quaternion rot) | ||
692 | { | ||
693 | // returns roll in X | ||
694 | // pitch in Y | ||
695 | // yaw in Z | ||
696 | Vector3 vec; | ||
697 | |||
698 | // assuming rot is normalised | ||
699 | // rot.Normalize(); | ||
700 | |||
701 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
702 | |||
703 | if (zX < -0.49999f) | ||
704 | { | ||
705 | vec.X = 0; | ||
706 | vec.Y = -halfpi; | ||
707 | vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); | ||
708 | } | ||
709 | else if (zX > 0.49999f) | ||
710 | { | ||
711 | vec.X = 0; | ||
712 | vec.Y = halfpi; | ||
713 | vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); | ||
714 | } | ||
715 | else | ||
716 | { | ||
717 | vec.Y = (float)Math.Asin(2 * zX); | ||
718 | |||
719 | float sqw = rot.W * rot.W; | ||
720 | |||
721 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
722 | float zZ = rot.Z * rot.Z + sqw - 0.5f; | ||
723 | |||
724 | vec.X = (float)Math.Atan2(minuszY, zZ); | ||
725 | |||
726 | float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) | ||
727 | float yY = rot.X * rot.X + sqw - 0.5f; | ||
728 | vec.Z = (float)Math.Atan2(yX, yY); | ||
729 | } | ||
730 | return vec; | ||
731 | } | ||
732 | |||
733 | public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) | ||
734 | { | ||
735 | // assuming rot is normalised | ||
736 | // rot.Normalize(); | ||
737 | |||
738 | float zX = rot.X * rot.Z + rot.Y * rot.W; | ||
739 | |||
740 | if (zX < -0.49999f) | ||
741 | { | ||
742 | roll = 0; | ||
743 | pitch = -halfpi; | ||
744 | } | ||
745 | else if (zX > 0.49999f) | ||
746 | { | ||
747 | roll = 0; | ||
748 | pitch = halfpi; | ||
749 | } | ||
750 | else | ||
751 | { | ||
752 | pitch = (float)Math.Asin(2 * zX); | ||
753 | |||
754 | float minuszY = rot.X * rot.W - rot.Y * rot.Z; | ||
755 | float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; | ||
756 | |||
757 | roll = (float)Math.Atan2(minuszY, zZ); | ||
758 | } | ||
759 | return ; | ||
760 | } | ||
761 | |||
762 | internal void Step() | ||
763 | { | ||
764 | IntPtr Body = rootPrim.Body; | ||
765 | |||
766 | d.Mass dmass; | ||
767 | d.BodyGetMass(Body, out dmass); | ||
768 | |||
769 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
770 | Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
771 | Quaternion rotq = objrotq; // rotq = rotation of object | ||
772 | rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame | ||
773 | Quaternion irotq = Quaternion.Inverse(rotq); | ||
774 | |||
775 | d.Vector3 dvtmp; | ||
776 | Vector3 tmpV; | ||
777 | Vector3 curVel; // velocity in world | ||
778 | Vector3 curAngVel; // angular velocity in world | ||
779 | Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame | ||
780 | Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame | ||
781 | d.Vector3 dtorque = new d.Vector3(); | ||
782 | |||
783 | dvtmp = d.BodyGetLinearVel(Body); | ||
784 | curVel.X = dvtmp.X; | ||
785 | curVel.Y = dvtmp.Y; | ||
786 | curVel.Z = dvtmp.Z; | ||
787 | Vector3 curLocalVel = curVel * irotq; // current velocity in local | ||
788 | |||
789 | dvtmp = d.BodyGetAngularVel(Body); | ||
790 | curAngVel.X = dvtmp.X; | ||
791 | curAngVel.Y = dvtmp.Y; | ||
792 | curAngVel.Z = dvtmp.Z; | ||
793 | Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local | ||
794 | |||
795 | float ldampZ = 0; | ||
796 | |||
797 | // linear motor | ||
798 | if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) | ||
799 | { | ||
800 | tmpV = m_linearMotorDirection - curLocalVel; // velocity error | ||
801 | tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep | ||
802 | tmpV *= rotq; // to world | ||
803 | |||
804 | if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) | ||
805 | tmpV.Z = 0; | ||
806 | |||
807 | if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) | ||
808 | { | ||
809 | // have offset, do it now | ||
810 | tmpV *= dmass.mass; | ||
811 | d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); | ||
812 | } | ||
813 | else | ||
814 | { | ||
815 | force.X += tmpV.X; | ||
816 | force.Y += tmpV.Y; | ||
817 | force.Z += tmpV.Z; | ||
818 | } | ||
819 | |||
820 | m_lmEfect *= m_lmDecay; | ||
821 | // m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); | ||
822 | m_ffactor = 0.0f; | ||
823 | } | ||
824 | else | ||
825 | { | ||
826 | m_lmEfect = 0; | ||
827 | m_ffactor = 1f; | ||
828 | } | ||
829 | |||
830 | // hover | ||
831 | if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) | ||
832 | { | ||
833 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
834 | d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); | ||
835 | pos.Z -= 0.21f; // minor offset that seems to be always there in sl | ||
836 | |||
837 | float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
838 | float perr; | ||
839 | |||
840 | // default to global but don't go underground | ||
841 | perr = m_VhoverHeight - pos.Z; | ||
842 | |||
843 | if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) | ||
844 | { | ||
845 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) | ||
846 | { | ||
847 | perr += _pParentScene.GetWaterLevel(); | ||
848 | } | ||
849 | else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | ||
850 | { | ||
851 | perr += t; | ||
852 | } | ||
853 | else | ||
854 | { | ||
855 | float w = _pParentScene.GetWaterLevel(); | ||
856 | if (t > w) | ||
857 | perr += t; | ||
858 | else | ||
859 | perr += w; | ||
860 | } | ||
861 | } | ||
862 | else if (t > m_VhoverHeight) | ||
863 | perr = t - pos.Z; ; | ||
864 | |||
865 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) | ||
866 | { | ||
867 | ldampZ = m_VhoverEfficiency * m_invtimestep; | ||
868 | |||
869 | perr *= (1.0f + ldampZ) / m_VhoverTimescale; | ||
870 | |||
871 | // force.Z += perr - curVel.Z * tmp; | ||
872 | force.Z += perr; | ||
873 | ldampZ *= -curVel.Z; | ||
874 | |||
875 | force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); | ||
876 | } | ||
877 | else // no buoyancy | ||
878 | force.Z += _pParentScene.gravityz; | ||
879 | } | ||
880 | else | ||
881 | { | ||
882 | // default gravity and Buoyancy | ||
883 | force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); | ||
884 | } | ||
885 | |||
886 | // linear deflection | ||
887 | if (m_linearDeflectionEfficiency > 0) | ||
888 | { | ||
889 | float len = curVel.Length(); | ||
890 | if (len > 0.01) // if moving | ||
891 | { | ||
892 | Vector3 atAxis; | ||
893 | atAxis = Xrot(rotq); // where are we pointing to | ||
894 | atAxis *= len; // make it same size as world velocity vector | ||
895 | |||
896 | tmpV = -atAxis; // oposite direction | ||
897 | atAxis -= curVel; // error to one direction | ||
898 | len = atAxis.LengthSquared(); | ||
899 | |||
900 | tmpV -= curVel; // error to oposite | ||
901 | float lens = tmpV.LengthSquared(); | ||
902 | |||
903 | if (len > 0.01 || lens > 0.01) // do nothing if close enougth | ||
904 | { | ||
905 | if (len < lens) | ||
906 | tmpV = atAxis; | ||
907 | |||
908 | tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep | ||
909 | force.X += tmpV.X; | ||
910 | force.Y += tmpV.Y; | ||
911 | if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) | ||
912 | force.Z += tmpV.Z; | ||
913 | } | ||
914 | } | ||
915 | } | ||
916 | |||
917 | // linear friction/damping | ||
918 | if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) | ||
919 | { | ||
920 | tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; | ||
921 | tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; | ||
922 | tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; | ||
923 | tmpV *= rotq; // to world | ||
924 | |||
925 | if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) | ||
926 | tmpV.Z = ldampZ; | ||
927 | force.X += tmpV.X; | ||
928 | force.Y += tmpV.Y; | ||
929 | force.Z += tmpV.Z; | ||
930 | } | ||
931 | |||
932 | // vertical atractor | ||
933 | if (m_verticalAttractionTimescale < 300) | ||
934 | { | ||
935 | float roll; | ||
936 | float pitch; | ||
937 | |||
938 | |||
939 | |||
940 | float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale; | ||
941 | |||
942 | float ftmp2; | ||
943 | ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; | ||
944 | m_amdampX = ftmp2; | ||
945 | |||
946 | m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; | ||
947 | |||
948 | GetRollPitch(irotq, out roll, out pitch); | ||
949 | |||
950 | if (roll > halfpi) | ||
951 | roll = pi - roll; | ||
952 | else if (roll < -halfpi) | ||
953 | roll = -pi - roll; | ||
954 | |||
955 | float effroll = pitch / halfpi; | ||
956 | effroll *= effroll; | ||
957 | effroll = 1 - effroll; | ||
958 | effroll *= roll; | ||
959 | |||
960 | |||
961 | torque.X += effroll * ftmp; | ||
962 | |||
963 | if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) | ||
964 | { | ||
965 | float effpitch = roll / halfpi; | ||
966 | effpitch *= effpitch; | ||
967 | effpitch = 1 - effpitch; | ||
968 | effpitch *= pitch; | ||
969 | |||
970 | torque.Y += effpitch * ftmp; | ||
971 | } | ||
972 | |||
973 | if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) | ||
974 | { | ||
975 | |||
976 | float broll = effroll; | ||
977 | /* | ||
978 | if (broll > halfpi) | ||
979 | broll = pi - broll; | ||
980 | else if (broll < -halfpi) | ||
981 | broll = -pi - broll; | ||
982 | */ | ||
983 | broll *= m_bankingEfficiency; | ||
984 | if (m_bankingMix != 0) | ||
985 | { | ||
986 | float vfact = Math.Abs(curLocalVel.X) / 10.0f; | ||
987 | if (vfact > 1.0f) vfact = 1.0f; | ||
988 | |||
989 | if (curLocalVel.X >= 0) | ||
990 | broll *= (1 + (vfact - 1) * m_bankingMix); | ||
991 | else | ||
992 | broll *= -(1 + (vfact - 1) * m_bankingMix); | ||
993 | } | ||
994 | // make z rot be in world Z not local as seems to be in sl | ||
995 | |||
996 | broll = broll / m_bankingTimescale; | ||
997 | |||
998 | |||
999 | tmpV = Zrot(irotq); | ||
1000 | tmpV *= broll; | ||
1001 | |||
1002 | torque.X += tmpV.X; | ||
1003 | torque.Y += tmpV.Y; | ||
1004 | torque.Z += tmpV.Z; | ||
1005 | |||
1006 | m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; | ||
1007 | m_amdampY = m_amdampZ; | ||
1008 | |||
1009 | } | ||
1010 | else | ||
1011 | { | ||
1012 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1013 | m_amdampY = m_amdampX; | ||
1014 | } | ||
1015 | } | ||
1016 | else | ||
1017 | { | ||
1018 | m_ampwr = 1.0f; | ||
1019 | m_amdampX = 1 / m_angularFrictionTimescale.X; | ||
1020 | m_amdampY = 1 / m_angularFrictionTimescale.Y; | ||
1021 | m_amdampZ = 1 / m_angularFrictionTimescale.Z; | ||
1022 | } | ||
1023 | |||
1024 | // angular motor | ||
1025 | if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) | ||
1026 | { | ||
1027 | tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error | ||
1028 | tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep | ||
1029 | torque.X += tmpV.X * m_ampwr; | ||
1030 | torque.Y += tmpV.Y * m_ampwr; | ||
1031 | torque.Z += tmpV.Z; | ||
1032 | |||
1033 | m_amEfect *= m_amDecay; | ||
1034 | } | ||
1035 | else | ||
1036 | m_amEfect = 0; | ||
1037 | |||
1038 | // angular deflection | ||
1039 | if (m_angularDeflectionEfficiency > 0) | ||
1040 | { | ||
1041 | Vector3 dirv; | ||
1042 | |||
1043 | if (curLocalVel.X > 0.01f) | ||
1044 | dirv = curLocalVel; | ||
1045 | else if (curLocalVel.X < -0.01f) | ||
1046 | // use oposite | ||
1047 | dirv = -curLocalVel; | ||
1048 | else | ||
1049 | { | ||
1050 | // make it fall into small positive x case | ||
1051 | dirv.X = 0.01f; | ||
1052 | dirv.Y = curLocalVel.Y; | ||
1053 | dirv.Z = curLocalVel.Z; | ||
1054 | } | ||
1055 | |||
1056 | float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; | ||
1057 | |||
1058 | if (Math.Abs(dirv.Z) > 0.01) | ||
1059 | { | ||
1060 | torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; | ||
1061 | } | ||
1062 | |||
1063 | if (Math.Abs(dirv.Y) > 0.01) | ||
1064 | { | ||
1065 | torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; | ||
1066 | } | ||
1067 | } | ||
1068 | |||
1069 | // angular friction | ||
1070 | if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) | ||
1071 | { | ||
1072 | torque.X -= curLocalAngVel.X * m_amdampX; | ||
1073 | torque.Y -= curLocalAngVel.Y * m_amdampY; | ||
1074 | torque.Z -= curLocalAngVel.Z * m_amdampZ; | ||
1075 | } | ||
1076 | |||
1077 | |||
1078 | if (force.X != 0 || force.Y != 0 || force.Z != 0) | ||
1079 | { | ||
1080 | force *= dmass.mass; | ||
1081 | d.BodyAddForce(Body, force.X, force.Y, force.Z); | ||
1082 | } | ||
1083 | |||
1084 | if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) | ||
1085 | { | ||
1086 | torque *= m_referenceFrame; // to object frame | ||
1087 | dtorque.X = torque.X ; | ||
1088 | dtorque.Y = torque.Y; | ||
1089 | dtorque.Z = torque.Z; | ||
1090 | |||
1091 | d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); | ||
1092 | d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame | ||
1093 | } | ||
1094 | } | ||
1095 | } | ||
1096 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs new file mode 100644 index 0000000..d4ba8a3 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODEMeshWorker.cs | |||
@@ -0,0 +1,933 @@ | |||
1 | /* | ||
2 | * AJLDuarte 2012 | ||
3 | */ | ||
4 | |||
5 | using System; | ||
6 | using System.Threading; | ||
7 | using System.Collections.Generic; | ||
8 | using System.IO; | ||
9 | using System.Reflection; | ||
10 | using System.Runtime.InteropServices; | ||
11 | using System.Text; | ||
12 | using OpenSim.Framework; | ||
13 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
14 | using OdeAPI; | ||
15 | using log4net; | ||
16 | using Nini.Config; | ||
17 | using OpenMetaverse; | ||
18 | |||
19 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
20 | { | ||
21 | public enum MeshState : byte | ||
22 | { | ||
23 | noNeed = 0, | ||
24 | |||
25 | loadingAsset = 1, | ||
26 | |||
27 | AssetOK = 0x0f, // 00001111 | ||
28 | |||
29 | NeedMask = 0x30, // 00110000 | ||
30 | needMesh = 0x10, // 00010000 | ||
31 | needAsset = 0x20, // 00100000 | ||
32 | |||
33 | FailMask = 0xC0, // 11000000 | ||
34 | AssetFailed = 0x40, // 01000000 | ||
35 | MeshFailed = 0x80, // 10000000 | ||
36 | |||
37 | MeshNoColide = FailMask | needAsset | ||
38 | } | ||
39 | |||
40 | public enum meshWorkerCmnds : byte | ||
41 | { | ||
42 | nop = 0, | ||
43 | addnew, | ||
44 | changefull, | ||
45 | changesize, | ||
46 | changeshapetype, | ||
47 | getmesh, | ||
48 | } | ||
49 | |||
50 | public class ODEPhysRepData | ||
51 | { | ||
52 | public PhysicsActor actor; | ||
53 | public PrimitiveBaseShape pbs; | ||
54 | public IMesh mesh; | ||
55 | |||
56 | public Vector3 size; | ||
57 | public Vector3 OBB; | ||
58 | public Vector3 OBBOffset; | ||
59 | |||
60 | public float volume; | ||
61 | |||
62 | public byte shapetype; | ||
63 | public bool hasOBB; | ||
64 | public bool hasMeshVolume; | ||
65 | public MeshState meshState; | ||
66 | public UUID? assetID; | ||
67 | public meshWorkerCmnds comand; | ||
68 | } | ||
69 | |||
70 | public class ODEMeshWorker | ||
71 | { | ||
72 | |||
73 | private ILog m_log; | ||
74 | private OdeScene m_scene; | ||
75 | private IMesher m_mesher; | ||
76 | |||
77 | public bool meshSculptedPrim = true; | ||
78 | public bool forceSimplePrimMeshing = false; | ||
79 | public float meshSculptLOD = 32; | ||
80 | public float MeshSculptphysicalLOD = 32; | ||
81 | |||
82 | |||
83 | private OpenSim.Framework.BlockingQueue<ODEPhysRepData> createqueue = new OpenSim.Framework.BlockingQueue<ODEPhysRepData>(); | ||
84 | private bool m_running; | ||
85 | |||
86 | private Thread m_thread; | ||
87 | |||
88 | public ODEMeshWorker(OdeScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) | ||
89 | { | ||
90 | m_scene = pScene; | ||
91 | m_log = pLog; | ||
92 | m_mesher = pMesher; | ||
93 | |||
94 | if (pConfig != null) | ||
95 | { | ||
96 | forceSimplePrimMeshing = pConfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); | ||
97 | meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); | ||
98 | meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); | ||
99 | MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); | ||
100 | } | ||
101 | m_running = true; | ||
102 | m_thread = new Thread(DoWork); | ||
103 | m_thread.Name = "OdeMeshWorker"; | ||
104 | m_thread.Start(); | ||
105 | } | ||
106 | |||
107 | private void DoWork() | ||
108 | { | ||
109 | m_mesher.ExpireFileCache(); | ||
110 | |||
111 | while(m_running) | ||
112 | { | ||
113 | ODEPhysRepData nextRep = createqueue.Dequeue(); | ||
114 | if(!m_running) | ||
115 | return; | ||
116 | if (nextRep == null) | ||
117 | continue; | ||
118 | if (m_scene.haveActor(nextRep.actor)) | ||
119 | { | ||
120 | switch (nextRep.comand) | ||
121 | { | ||
122 | case meshWorkerCmnds.changefull: | ||
123 | case meshWorkerCmnds.changeshapetype: | ||
124 | case meshWorkerCmnds.changesize: | ||
125 | GetMesh(nextRep); | ||
126 | if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) | ||
127 | m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); | ||
128 | break; | ||
129 | case meshWorkerCmnds.getmesh: | ||
130 | DoRepDataGetMesh(nextRep); | ||
131 | break; | ||
132 | } | ||
133 | } | ||
134 | } | ||
135 | } | ||
136 | |||
137 | public void Stop() | ||
138 | { | ||
139 | try | ||
140 | { | ||
141 | m_thread.Abort(); | ||
142 | createqueue.Clear(); | ||
143 | } | ||
144 | catch | ||
145 | { | ||
146 | } | ||
147 | } | ||
148 | |||
149 | public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, | ||
150 | Vector3 size, byte shapetype) | ||
151 | { | ||
152 | ODEPhysRepData repData = new ODEPhysRepData(); | ||
153 | repData.actor = actor; | ||
154 | repData.pbs = pbs; | ||
155 | repData.size = size; | ||
156 | repData.shapetype = shapetype; | ||
157 | |||
158 | CheckMesh(repData); | ||
159 | CalcVolumeData(repData); | ||
160 | m_scene.AddChange(actor, changes.PhysRepData, repData); | ||
161 | return; | ||
162 | } | ||
163 | |||
164 | public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, | ||
165 | Vector3 size, byte shapetype) | ||
166 | { | ||
167 | ODEPhysRepData repData = new ODEPhysRepData(); | ||
168 | repData.actor = actor; | ||
169 | repData.pbs = pbs; | ||
170 | repData.size = size; | ||
171 | repData.shapetype = shapetype; | ||
172 | |||
173 | CheckMesh(repData); | ||
174 | CalcVolumeData(repData); | ||
175 | m_scene.AddChange(actor, changes.AddPhysRep, repData); | ||
176 | return repData; | ||
177 | } | ||
178 | |||
179 | public void RequestMesh(ODEPhysRepData repData) | ||
180 | { | ||
181 | repData.mesh = null; | ||
182 | |||
183 | if (repData.meshState == MeshState.needAsset) | ||
184 | { | ||
185 | PrimitiveBaseShape pbs = repData.pbs; | ||
186 | |||
187 | // check if we got outdated | ||
188 | |||
189 | if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero) | ||
190 | { | ||
191 | repData.meshState = MeshState.noNeed; | ||
192 | return; | ||
193 | } | ||
194 | |||
195 | repData.assetID = pbs.SculptTexture; | ||
196 | repData.meshState = MeshState.loadingAsset; | ||
197 | |||
198 | repData.comand = meshWorkerCmnds.getmesh; | ||
199 | createqueue.Enqueue(repData); | ||
200 | } | ||
201 | } | ||
202 | |||
203 | // creates and prepares a mesh to use and calls parameters estimation | ||
204 | public bool CreateActorPhysRep(ODEPhysRepData repData) | ||
205 | { | ||
206 | IMesh mesh = repData.mesh; | ||
207 | |||
208 | if (mesh != null) | ||
209 | { | ||
210 | IntPtr vertices, indices; | ||
211 | int vertexCount, indexCount; | ||
212 | int vertexStride, triStride; | ||
213 | |||
214 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); | ||
215 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); | ||
216 | |||
217 | if (vertexCount == 0 || indexCount == 0) | ||
218 | { | ||
219 | m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", | ||
220 | repData.actor.Name, repData.pbs.SculptTexture.ToString()); | ||
221 | repData.meshState = MeshState.MeshFailed; | ||
222 | repData.hasOBB = false; | ||
223 | repData.mesh = null; | ||
224 | m_scene.mesher.ReleaseMesh(mesh); | ||
225 | } | ||
226 | else | ||
227 | { | ||
228 | repData.OBBOffset = mesh.GetCentroid(); | ||
229 | repData.OBB = mesh.GetOBB(); | ||
230 | repData.hasOBB = true; | ||
231 | mesh.releaseSourceMeshData(); | ||
232 | } | ||
233 | } | ||
234 | CalcVolumeData(repData); | ||
235 | return true; | ||
236 | } | ||
237 | |||
238 | public void AssetLoaded(ODEPhysRepData repData) | ||
239 | { | ||
240 | if (m_scene.haveActor(repData.actor)) | ||
241 | { | ||
242 | if (needsMeshing(repData.pbs)) // no need for pbs now? | ||
243 | { | ||
244 | repData.comand = meshWorkerCmnds.changefull; | ||
245 | createqueue.Enqueue(repData); | ||
246 | } | ||
247 | } | ||
248 | else | ||
249 | repData.pbs.SculptData = Utils.EmptyBytes; | ||
250 | } | ||
251 | |||
252 | public void DoRepDataGetMesh(ODEPhysRepData repData) | ||
253 | { | ||
254 | if (!repData.pbs.SculptEntry) | ||
255 | return; | ||
256 | |||
257 | if (repData.meshState != MeshState.loadingAsset) | ||
258 | return; | ||
259 | |||
260 | if (repData.assetID == null || repData.assetID == UUID.Zero) | ||
261 | return; | ||
262 | |||
263 | if (repData.assetID != repData.pbs.SculptTexture) | ||
264 | return; | ||
265 | |||
266 | // check if it is in cache | ||
267 | GetMesh(repData); | ||
268 | if (repData.meshState != MeshState.needAsset) | ||
269 | { | ||
270 | CreateActorPhysRep(repData); | ||
271 | m_scene.AddChange(repData.actor, changes.PhysRepData, repData); | ||
272 | return; | ||
273 | } | ||
274 | |||
275 | RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; | ||
276 | if (assetProvider == null) | ||
277 | return; | ||
278 | ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log); | ||
279 | } | ||
280 | |||
281 | |||
282 | /// <summary> | ||
283 | /// Routine to figure out if we need to mesh this prim with our mesher | ||
284 | /// </summary> | ||
285 | /// <param name="pbs"></param> | ||
286 | /// <returns></returns> | ||
287 | public bool needsMeshing(PrimitiveBaseShape pbs) | ||
288 | { | ||
289 | // check sculpts or meshs | ||
290 | if (pbs.SculptEntry) | ||
291 | { | ||
292 | if (meshSculptedPrim) | ||
293 | return true; | ||
294 | |||
295 | if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs | ||
296 | return true; | ||
297 | |||
298 | return false; | ||
299 | } | ||
300 | |||
301 | if (forceSimplePrimMeshing) | ||
302 | return true; | ||
303 | |||
304 | // 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 | ||
305 | |||
306 | if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) | ||
307 | || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 | ||
308 | && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) | ||
309 | { | ||
310 | |||
311 | if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 | ||
312 | && pbs.ProfileHollow == 0 | ||
313 | && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 | ||
314 | && pbs.PathBegin == 0 && pbs.PathEnd == 0 | ||
315 | && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 | ||
316 | && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 | ||
317 | && pbs.PathShearX == 0 && pbs.PathShearY == 0) | ||
318 | { | ||
319 | return false; | ||
320 | } | ||
321 | } | ||
322 | |||
323 | // following code doesn't give meshs to boxes and spheres ever | ||
324 | // and it's odd.. so for now just return true if asked to force meshs | ||
325 | // hopefully mesher will fail if doesn't suport so things still get basic boxes | ||
326 | |||
327 | int iPropertiesNotSupportedDefault = 0; | ||
328 | |||
329 | if (pbs.ProfileHollow != 0) | ||
330 | iPropertiesNotSupportedDefault++; | ||
331 | |||
332 | if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) | ||
333 | iPropertiesNotSupportedDefault++; | ||
334 | |||
335 | if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) | ||
336 | iPropertiesNotSupportedDefault++; | ||
337 | |||
338 | if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) | ||
339 | iPropertiesNotSupportedDefault++; | ||
340 | |||
341 | if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) | ||
342 | iPropertiesNotSupportedDefault++; | ||
343 | |||
344 | if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) | ||
345 | iPropertiesNotSupportedDefault++; | ||
346 | |||
347 | if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) | ||
348 | iPropertiesNotSupportedDefault++; | ||
349 | |||
350 | 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)) | ||
351 | iPropertiesNotSupportedDefault++; | ||
352 | |||
353 | if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1) | ||
354 | iPropertiesNotSupportedDefault++; | ||
355 | |||
356 | // test for torus | ||
357 | if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) | ||
358 | { | ||
359 | if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
360 | { | ||
361 | iPropertiesNotSupportedDefault++; | ||
362 | } | ||
363 | } | ||
364 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) | ||
365 | { | ||
366 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
367 | { | ||
368 | iPropertiesNotSupportedDefault++; | ||
369 | } | ||
370 | |||
371 | // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits | ||
372 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
373 | { | ||
374 | iPropertiesNotSupportedDefault++; | ||
375 | } | ||
376 | } | ||
377 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) | ||
378 | { | ||
379 | if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) | ||
380 | { | ||
381 | iPropertiesNotSupportedDefault++; | ||
382 | } | ||
383 | } | ||
384 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) | ||
385 | { | ||
386 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
387 | { | ||
388 | iPropertiesNotSupportedDefault++; | ||
389 | } | ||
390 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
391 | { | ||
392 | iPropertiesNotSupportedDefault++; | ||
393 | } | ||
394 | } | ||
395 | |||
396 | if (iPropertiesNotSupportedDefault == 0) | ||
397 | { | ||
398 | return false; | ||
399 | } | ||
400 | return true; | ||
401 | } | ||
402 | |||
403 | // see if we need a mesh and if so if we have a cached one | ||
404 | // called with a new repData | ||
405 | public void CheckMesh(ODEPhysRepData repData) | ||
406 | { | ||
407 | PhysicsActor actor = repData.actor; | ||
408 | PrimitiveBaseShape pbs = repData.pbs; | ||
409 | |||
410 | if (!needsMeshing(pbs)) | ||
411 | { | ||
412 | repData.meshState = MeshState.noNeed; | ||
413 | return; | ||
414 | } | ||
415 | |||
416 | IMesh mesh = null; | ||
417 | |||
418 | Vector3 size = repData.size; | ||
419 | byte shapetype = repData.shapetype; | ||
420 | |||
421 | bool convex; | ||
422 | |||
423 | int clod = (int)LevelOfDetail.High; | ||
424 | if (shapetype == 0) | ||
425 | convex = false; | ||
426 | else | ||
427 | { | ||
428 | convex = true; | ||
429 | if (pbs.SculptType != (byte)SculptType.Mesh) | ||
430 | clod = (int)LevelOfDetail.Low; | ||
431 | } | ||
432 | |||
433 | mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); | ||
434 | |||
435 | if (mesh == null) | ||
436 | { | ||
437 | if (pbs.SculptEntry) | ||
438 | { | ||
439 | if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) | ||
440 | { | ||
441 | repData.assetID = pbs.SculptTexture; | ||
442 | repData.meshState = MeshState.needAsset; | ||
443 | } | ||
444 | else | ||
445 | repData.meshState = MeshState.MeshFailed; | ||
446 | |||
447 | return; | ||
448 | } | ||
449 | else | ||
450 | { | ||
451 | repData.meshState = MeshState.needMesh; | ||
452 | mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); | ||
453 | if (mesh == null) | ||
454 | { | ||
455 | repData.meshState = MeshState.MeshFailed; | ||
456 | return; | ||
457 | } | ||
458 | } | ||
459 | } | ||
460 | |||
461 | repData.meshState = MeshState.AssetOK; | ||
462 | repData.mesh = mesh; | ||
463 | |||
464 | if (pbs.SculptEntry) | ||
465 | { | ||
466 | repData.assetID = pbs.SculptTexture; | ||
467 | } | ||
468 | |||
469 | pbs.SculptData = Utils.EmptyBytes; | ||
470 | return ; | ||
471 | } | ||
472 | |||
473 | public void GetMesh(ODEPhysRepData repData) | ||
474 | { | ||
475 | PhysicsActor actor = repData.actor; | ||
476 | |||
477 | PrimitiveBaseShape pbs = repData.pbs; | ||
478 | |||
479 | repData.mesh = null; | ||
480 | repData.hasOBB = false; | ||
481 | |||
482 | if (!needsMeshing(pbs)) | ||
483 | { | ||
484 | repData.meshState = MeshState.noNeed; | ||
485 | return; | ||
486 | } | ||
487 | |||
488 | if (repData.meshState == MeshState.MeshFailed) | ||
489 | return; | ||
490 | |||
491 | if (pbs.SculptEntry) | ||
492 | { | ||
493 | if (repData.meshState == MeshState.AssetFailed) | ||
494 | { | ||
495 | if (pbs.SculptTexture == repData.assetID) | ||
496 | return; | ||
497 | } | ||
498 | } | ||
499 | |||
500 | repData.meshState = MeshState.noNeed; | ||
501 | |||
502 | IMesh mesh = null; | ||
503 | Vector3 size = repData.size; | ||
504 | byte shapetype = repData.shapetype; | ||
505 | |||
506 | bool convex; | ||
507 | int clod = (int)LevelOfDetail.High; | ||
508 | if (shapetype == 0) | ||
509 | convex = false; | ||
510 | else | ||
511 | { | ||
512 | convex = true; | ||
513 | if (pbs.SculptType != (byte)SculptType.Mesh) | ||
514 | clod = (int)LevelOfDetail.Low; | ||
515 | } | ||
516 | |||
517 | mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); | ||
518 | |||
519 | if (mesh == null) | ||
520 | { | ||
521 | if (pbs.SculptEntry) | ||
522 | { | ||
523 | if (pbs.SculptTexture == UUID.Zero) | ||
524 | return; | ||
525 | |||
526 | repData.assetID = pbs.SculptTexture; | ||
527 | |||
528 | if (pbs.SculptData == null || pbs.SculptData.Length == 0) | ||
529 | { | ||
530 | repData.meshState = MeshState.needAsset; | ||
531 | return; | ||
532 | } | ||
533 | } | ||
534 | } | ||
535 | |||
536 | repData.mesh = mesh; | ||
537 | repData.pbs.SculptData = Utils.EmptyBytes; | ||
538 | |||
539 | if (mesh == null) | ||
540 | { | ||
541 | if (pbs.SculptEntry) | ||
542 | repData.meshState = MeshState.AssetFailed; | ||
543 | else | ||
544 | repData.meshState = MeshState.MeshFailed; | ||
545 | |||
546 | return; | ||
547 | } | ||
548 | |||
549 | repData.meshState = MeshState.AssetOK; | ||
550 | |||
551 | return; | ||
552 | } | ||
553 | |||
554 | private void CalculateBasicPrimVolume(ODEPhysRepData repData) | ||
555 | { | ||
556 | PrimitiveBaseShape _pbs = repData.pbs; | ||
557 | Vector3 _size = repData.size; | ||
558 | |||
559 | float volume = _size.X * _size.Y * _size.Z; // default | ||
560 | float tmp; | ||
561 | |||
562 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | ||
563 | float hollowVolume = hollowAmount * hollowAmount; | ||
564 | |||
565 | switch (_pbs.ProfileShape) | ||
566 | { | ||
567 | case ProfileShape.Square: | ||
568 | // default box | ||
569 | |||
570 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
571 | { | ||
572 | if (hollowAmount > 0.0) | ||
573 | { | ||
574 | switch (_pbs.HollowShape) | ||
575 | { | ||
576 | case HollowShape.Square: | ||
577 | case HollowShape.Same: | ||
578 | break; | ||
579 | |||
580 | case HollowShape.Circle: | ||
581 | |||
582 | hollowVolume *= 0.78539816339f; | ||
583 | break; | ||
584 | |||
585 | case HollowShape.Triangle: | ||
586 | |||
587 | hollowVolume *= (0.5f * .5f); | ||
588 | break; | ||
589 | |||
590 | default: | ||
591 | hollowVolume = 0; | ||
592 | break; | ||
593 | } | ||
594 | volume *= (1.0f - hollowVolume); | ||
595 | } | ||
596 | } | ||
597 | |||
598 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
599 | { | ||
600 | //a tube | ||
601 | |||
602 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
603 | tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
604 | volume -= volume * tmp * tmp; | ||
605 | |||
606 | if (hollowAmount > 0.0) | ||
607 | { | ||
608 | hollowVolume *= hollowAmount; | ||
609 | |||
610 | switch (_pbs.HollowShape) | ||
611 | { | ||
612 | case HollowShape.Square: | ||
613 | case HollowShape.Same: | ||
614 | break; | ||
615 | |||
616 | case HollowShape.Circle: | ||
617 | hollowVolume *= 0.78539816339f; | ||
618 | break; | ||
619 | |||
620 | case HollowShape.Triangle: | ||
621 | hollowVolume *= 0.5f * 0.5f; | ||
622 | break; | ||
623 | default: | ||
624 | hollowVolume = 0; | ||
625 | break; | ||
626 | } | ||
627 | volume *= (1.0f - hollowVolume); | ||
628 | } | ||
629 | } | ||
630 | |||
631 | break; | ||
632 | |||
633 | case ProfileShape.Circle: | ||
634 | |||
635 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
636 | { | ||
637 | volume *= 0.78539816339f; // elipse base | ||
638 | |||
639 | if (hollowAmount > 0.0) | ||
640 | { | ||
641 | switch (_pbs.HollowShape) | ||
642 | { | ||
643 | case HollowShape.Same: | ||
644 | case HollowShape.Circle: | ||
645 | break; | ||
646 | |||
647 | case HollowShape.Square: | ||
648 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
649 | break; | ||
650 | |||
651 | case HollowShape.Triangle: | ||
652 | hollowVolume *= .5f * 1.27323954473516f; | ||
653 | break; | ||
654 | |||
655 | default: | ||
656 | hollowVolume = 0; | ||
657 | break; | ||
658 | } | ||
659 | volume *= (1.0f - hollowVolume); | ||
660 | } | ||
661 | } | ||
662 | |||
663 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
664 | { | ||
665 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
666 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
667 | volume *= (1.0f - tmp * tmp); | ||
668 | |||
669 | if (hollowAmount > 0.0) | ||
670 | { | ||
671 | |||
672 | // calculate the hollow volume by it's shape compared to the prim shape | ||
673 | hollowVolume *= hollowAmount; | ||
674 | |||
675 | switch (_pbs.HollowShape) | ||
676 | { | ||
677 | case HollowShape.Same: | ||
678 | case HollowShape.Circle: | ||
679 | break; | ||
680 | |||
681 | case HollowShape.Square: | ||
682 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
683 | break; | ||
684 | |||
685 | case HollowShape.Triangle: | ||
686 | hollowVolume *= .5f * 1.27323954473516f; | ||
687 | break; | ||
688 | |||
689 | default: | ||
690 | hollowVolume = 0; | ||
691 | break; | ||
692 | } | ||
693 | volume *= (1.0f - hollowVolume); | ||
694 | } | ||
695 | } | ||
696 | break; | ||
697 | |||
698 | case ProfileShape.HalfCircle: | ||
699 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
700 | { | ||
701 | volume *= 0.5236f; | ||
702 | |||
703 | if (hollowAmount > 0.0) | ||
704 | { | ||
705 | hollowVolume *= hollowAmount; | ||
706 | |||
707 | switch (_pbs.HollowShape) | ||
708 | { | ||
709 | case HollowShape.Circle: | ||
710 | case HollowShape.Triangle: // diference in sl is minor and odd | ||
711 | case HollowShape.Same: | ||
712 | break; | ||
713 | |||
714 | case HollowShape.Square: | ||
715 | hollowVolume *= 0.909f; | ||
716 | break; | ||
717 | |||
718 | // case HollowShape.Triangle: | ||
719 | // hollowVolume *= .827f; | ||
720 | // break; | ||
721 | default: | ||
722 | hollowVolume = 0; | ||
723 | break; | ||
724 | } | ||
725 | volume *= (1.0f - hollowVolume); | ||
726 | } | ||
727 | |||
728 | } | ||
729 | break; | ||
730 | |||
731 | case ProfileShape.EquilateralTriangle: | ||
732 | |||
733 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
734 | { | ||
735 | volume *= 0.32475953f; | ||
736 | |||
737 | if (hollowAmount > 0.0) | ||
738 | { | ||
739 | |||
740 | // calculate the hollow volume by it's shape compared to the prim shape | ||
741 | switch (_pbs.HollowShape) | ||
742 | { | ||
743 | case HollowShape.Same: | ||
744 | case HollowShape.Triangle: | ||
745 | hollowVolume *= .25f; | ||
746 | break; | ||
747 | |||
748 | case HollowShape.Square: | ||
749 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
750 | break; | ||
751 | |||
752 | case HollowShape.Circle: | ||
753 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
754 | // Cyllinder hollow volume calculation | ||
755 | |||
756 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
757 | break; | ||
758 | |||
759 | default: | ||
760 | hollowVolume = 0; | ||
761 | break; | ||
762 | } | ||
763 | volume *= (1.0f - hollowVolume); | ||
764 | } | ||
765 | } | ||
766 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
767 | { | ||
768 | volume *= 0.32475953f; | ||
769 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
770 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
771 | volume *= (1.0f - tmp * tmp); | ||
772 | |||
773 | if (hollowAmount > 0.0) | ||
774 | { | ||
775 | |||
776 | hollowVolume *= hollowAmount; | ||
777 | |||
778 | switch (_pbs.HollowShape) | ||
779 | { | ||
780 | case HollowShape.Same: | ||
781 | case HollowShape.Triangle: | ||
782 | hollowVolume *= .25f; | ||
783 | break; | ||
784 | |||
785 | case HollowShape.Square: | ||
786 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
787 | break; | ||
788 | |||
789 | case HollowShape.Circle: | ||
790 | |||
791 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
792 | break; | ||
793 | |||
794 | default: | ||
795 | hollowVolume = 0; | ||
796 | break; | ||
797 | } | ||
798 | volume *= (1.0f - hollowVolume); | ||
799 | } | ||
800 | } | ||
801 | break; | ||
802 | |||
803 | default: | ||
804 | break; | ||
805 | } | ||
806 | |||
807 | float taperX1; | ||
808 | float taperY1; | ||
809 | float taperX; | ||
810 | float taperY; | ||
811 | float pathBegin; | ||
812 | float pathEnd; | ||
813 | float profileBegin; | ||
814 | float profileEnd; | ||
815 | |||
816 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | ||
817 | { | ||
818 | taperX1 = _pbs.PathScaleX * 0.01f; | ||
819 | if (taperX1 > 1.0f) | ||
820 | taperX1 = 2.0f - taperX1; | ||
821 | taperX = 1.0f - taperX1; | ||
822 | |||
823 | taperY1 = _pbs.PathScaleY * 0.01f; | ||
824 | if (taperY1 > 1.0f) | ||
825 | taperY1 = 2.0f - taperY1; | ||
826 | taperY = 1.0f - taperY1; | ||
827 | } | ||
828 | else | ||
829 | { | ||
830 | taperX = _pbs.PathTaperX * 0.01f; | ||
831 | if (taperX < 0.0f) | ||
832 | taperX = -taperX; | ||
833 | taperX1 = 1.0f - taperX; | ||
834 | |||
835 | taperY = _pbs.PathTaperY * 0.01f; | ||
836 | if (taperY < 0.0f) | ||
837 | taperY = -taperY; | ||
838 | taperY1 = 1.0f - taperY; | ||
839 | } | ||
840 | |||
841 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | ||
842 | |||
843 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | ||
844 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | ||
845 | volume *= (pathEnd - pathBegin); | ||
846 | |||
847 | // this is crude aproximation | ||
848 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | ||
849 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | ||
850 | volume *= (profileEnd - profileBegin); | ||
851 | |||
852 | repData.volume = volume; | ||
853 | } | ||
854 | |||
855 | private void CalcVolumeData(ODEPhysRepData repData) | ||
856 | { | ||
857 | if (repData.hasOBB) | ||
858 | { | ||
859 | Vector3 OBB = repData.OBB; | ||
860 | } | ||
861 | else | ||
862 | { | ||
863 | Vector3 OBB = repData.size; | ||
864 | OBB.X *= 0.5f; | ||
865 | OBB.Y *= 0.5f; | ||
866 | OBB.Z *= 0.5f; | ||
867 | |||
868 | repData.OBB = OBB; | ||
869 | repData.OBBOffset = Vector3.Zero; | ||
870 | } | ||
871 | |||
872 | CalculateBasicPrimVolume(repData); | ||
873 | } | ||
874 | } | ||
875 | |||
876 | public class ODEAssetRequest | ||
877 | { | ||
878 | ODEMeshWorker m_worker; | ||
879 | private ILog m_log; | ||
880 | ODEPhysRepData repData; | ||
881 | |||
882 | public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, | ||
883 | ODEPhysRepData pRepData, ILog plog) | ||
884 | { | ||
885 | m_worker = pWorker; | ||
886 | m_log = plog; | ||
887 | repData = pRepData; | ||
888 | |||
889 | repData.meshState = MeshState.AssetFailed; | ||
890 | if (provider == null) | ||
891 | return; | ||
892 | |||
893 | if (repData.assetID == null) | ||
894 | return; | ||
895 | |||
896 | UUID assetID = (UUID) repData.assetID; | ||
897 | if (assetID == UUID.Zero) | ||
898 | return; | ||
899 | |||
900 | repData.meshState = MeshState.loadingAsset; | ||
901 | provider(assetID, ODEassetReceived); | ||
902 | } | ||
903 | |||
904 | void ODEassetReceived(AssetBase asset) | ||
905 | { | ||
906 | repData.meshState = MeshState.AssetFailed; | ||
907 | if (asset != null) | ||
908 | { | ||
909 | if (asset.Data != null && asset.Data.Length > 0) | ||
910 | { | ||
911 | repData.meshState = MeshState.noNeed; | ||
912 | |||
913 | if (!repData.pbs.SculptEntry) | ||
914 | return; | ||
915 | if (repData.pbs.SculptTexture != repData.assetID) | ||
916 | return; | ||
917 | |||
918 | // repData.pbs.SculptData = new byte[asset.Data.Length]; | ||
919 | // asset.Data.CopyTo(repData.pbs.SculptData,0); | ||
920 | repData.pbs.SculptData = asset.Data; | ||
921 | repData.meshState = MeshState.AssetOK; | ||
922 | m_worker.AssetLoaded(repData); | ||
923 | } | ||
924 | else | ||
925 | m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", | ||
926 | repData.actor.Name, asset.ID.ToString()); | ||
927 | } | ||
928 | else | ||
929 | m_log.WarnFormat("[PHYSICS]: asset provider returned null asset fo mesh of prim {0}.", | ||
930 | repData.actor.Name); | ||
931 | } | ||
932 | } | ||
933 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs new file mode 100644 index 0000000..2e75bc9 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODEPrim.cs | |||
@@ -0,0 +1,3901 @@ | |||
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/13 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 | |||
49 | using System; | ||
50 | using System.Collections.Generic; | ||
51 | using System.Reflection; | ||
52 | using System.Runtime.InteropServices; | ||
53 | using System.Threading; | ||
54 | using log4net; | ||
55 | using OpenMetaverse; | ||
56 | using OdeAPI; | ||
57 | using OpenSim.Framework; | ||
58 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
59 | |||
60 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
61 | { | ||
62 | public class OdePrim : PhysicsActor | ||
63 | { | ||
64 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
65 | |||
66 | private bool m_isphysical; | ||
67 | private bool m_fakeisphysical; | ||
68 | private bool m_isphantom; | ||
69 | private bool m_fakeisphantom; | ||
70 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
71 | private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
72 | |||
73 | protected bool m_building; | ||
74 | protected bool m_forcePosOrRotation; | ||
75 | private bool m_iscolliding; | ||
76 | |||
77 | internal bool m_isSelected; | ||
78 | private bool m_delaySelect; | ||
79 | private bool m_lastdoneSelected; | ||
80 | internal bool m_outbounds; | ||
81 | |||
82 | private Quaternion m_lastorientation; | ||
83 | private Quaternion _orientation; | ||
84 | |||
85 | private Vector3 _position; | ||
86 | private Vector3 _velocity; | ||
87 | private Vector3 m_torque; | ||
88 | private Vector3 m_lastVelocity; | ||
89 | private Vector3 m_lastposition; | ||
90 | private Vector3 m_rotationalVelocity; | ||
91 | private Vector3 _size; | ||
92 | private Vector3 _acceleration; | ||
93 | private Vector3 m_angularlock = Vector3.One; | ||
94 | private IntPtr Amotor; | ||
95 | |||
96 | private Vector3 m_force; | ||
97 | private Vector3 m_forceacc; | ||
98 | private Vector3 m_angularForceacc; | ||
99 | |||
100 | private float m_invTimeStep; | ||
101 | private float m_timeStep; | ||
102 | |||
103 | private Vector3 m_PIDTarget; | ||
104 | private float m_PIDTau; | ||
105 | private bool m_usePID; | ||
106 | |||
107 | private float m_PIDHoverHeight; | ||
108 | private float m_PIDHoverTau; | ||
109 | private bool m_useHoverPID; | ||
110 | private PIDHoverType m_PIDHoverType; | ||
111 | private float m_targetHoverHeight; | ||
112 | private float m_groundHeight; | ||
113 | private float m_waterHeight; | ||
114 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | ||
115 | |||
116 | private int body_autodisable_frames; | ||
117 | public int bodydisablecontrol; | ||
118 | private float m_gravmod = 1.0f; | ||
119 | |||
120 | // Default we're a Geometry | ||
121 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | ||
122 | // Default colide nonphysical don't try to colide with anything | ||
123 | private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; | ||
124 | |||
125 | private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | | ||
126 | CollisionCategories.Character | | ||
127 | CollisionCategories.Land | | ||
128 | CollisionCategories.VolumeDtc); | ||
129 | |||
130 | // private bool m_collidesLand = true; | ||
131 | private bool m_collidesWater; | ||
132 | // public bool m_returnCollisions; | ||
133 | |||
134 | private bool m_NoColide; // for now only for internal use for bad meshs | ||
135 | |||
136 | |||
137 | // Default, Collide with Other Geometries, spaces and Bodies | ||
138 | private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; | ||
139 | |||
140 | public bool m_disabled; | ||
141 | |||
142 | private uint m_localID; | ||
143 | |||
144 | private IMesh m_mesh; | ||
145 | private object m_meshlock = new object(); | ||
146 | private PrimitiveBaseShape _pbs; | ||
147 | |||
148 | private UUID? m_assetID; | ||
149 | private MeshState m_meshState; | ||
150 | |||
151 | public OdeScene _parent_scene; | ||
152 | |||
153 | /// <summary> | ||
154 | /// The physics space which contains prim geometry | ||
155 | /// </summary> | ||
156 | public IntPtr m_targetSpace; | ||
157 | |||
158 | public IntPtr prim_geom; | ||
159 | public IntPtr _triMeshData; | ||
160 | |||
161 | private PhysicsActor _parent; | ||
162 | |||
163 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | ||
164 | |||
165 | public float m_collisionscore; | ||
166 | private int m_colliderfilter = 0; | ||
167 | |||
168 | public IntPtr collide_geom; // for objects: geom if single prim space it linkset | ||
169 | |||
170 | private float m_density; | ||
171 | private byte m_shapetype; | ||
172 | public bool _zeroFlag; | ||
173 | private bool m_lastUpdateSent; | ||
174 | |||
175 | public IntPtr Body; | ||
176 | |||
177 | private Vector3 _target_velocity; | ||
178 | |||
179 | public Vector3 m_OBBOffset; | ||
180 | public Vector3 m_OBB; | ||
181 | public float primOOBradiusSQ; | ||
182 | |||
183 | private bool m_hasOBB = true; | ||
184 | |||
185 | private float m_physCost; | ||
186 | private float m_streamCost; | ||
187 | |||
188 | public d.Mass primdMass; // prim inertia information on it's own referencial | ||
189 | float primMass; // prim own mass | ||
190 | float primVolume; // prim own volume; | ||
191 | float _mass; // object mass acording to case | ||
192 | |||
193 | public int givefakepos; | ||
194 | private Vector3 fakepos; | ||
195 | public int givefakeori; | ||
196 | private Quaternion fakeori; | ||
197 | |||
198 | private int m_eventsubscription; | ||
199 | private int m_cureventsubscription; | ||
200 | private CollisionEventUpdate CollisionEventsThisFrame = null; | ||
201 | private bool SentEmptyCollisionsEvent; | ||
202 | |||
203 | public volatile bool childPrim; | ||
204 | |||
205 | public ODEDynamics m_vehicle; | ||
206 | |||
207 | internal int m_material = (int)Material.Wood; | ||
208 | private float mu; | ||
209 | private float bounce; | ||
210 | |||
211 | /// <summary> | ||
212 | /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. | ||
213 | /// </summary> | ||
214 | public override bool IsPhysical // this is not reliable for internal use | ||
215 | { | ||
216 | get { return m_fakeisphysical; } | ||
217 | set | ||
218 | { | ||
219 | m_fakeisphysical = value; // we show imediatly to outside that we changed physical | ||
220 | // and also to stop imediatly some updates | ||
221 | // but real change will only happen in taintprocessing | ||
222 | |||
223 | if (!value) // Zero the remembered last velocity | ||
224 | m_lastVelocity = Vector3.Zero; | ||
225 | AddChange(changes.Physical, value); | ||
226 | } | ||
227 | } | ||
228 | |||
229 | public override bool IsVolumeDtc | ||
230 | { | ||
231 | get { return m_fakeisVolumeDetect; } | ||
232 | set | ||
233 | { | ||
234 | m_fakeisVolumeDetect = value; | ||
235 | AddChange(changes.VolumeDtc, value); | ||
236 | } | ||
237 | } | ||
238 | |||
239 | public override bool Phantom // this is not reliable for internal use | ||
240 | { | ||
241 | get { return m_fakeisphantom; } | ||
242 | set | ||
243 | { | ||
244 | m_fakeisphantom = value; | ||
245 | AddChange(changes.Phantom, value); | ||
246 | } | ||
247 | } | ||
248 | |||
249 | public override bool Building // this is not reliable for internal use | ||
250 | { | ||
251 | get { return m_building; } | ||
252 | set | ||
253 | { | ||
254 | // if (value) | ||
255 | // m_building = true; | ||
256 | AddChange(changes.building, value); | ||
257 | } | ||
258 | } | ||
259 | |||
260 | public override void getContactData(ref ContactData cdata) | ||
261 | { | ||
262 | cdata.mu = mu; | ||
263 | cdata.bounce = bounce; | ||
264 | |||
265 | // cdata.softcolide = m_softcolide; | ||
266 | cdata.softcolide = false; | ||
267 | |||
268 | if (m_isphysical) | ||
269 | { | ||
270 | ODEDynamics veh; | ||
271 | if (_parent != null) | ||
272 | veh = ((OdePrim)_parent).m_vehicle; | ||
273 | else | ||
274 | veh = m_vehicle; | ||
275 | |||
276 | if (veh != null && veh.Type != Vehicle.TYPE_NONE) | ||
277 | cdata.mu *= veh.FrictionFactor; | ||
278 | // cdata.mu *= 0; | ||
279 | } | ||
280 | } | ||
281 | |||
282 | public override float PhysicsCost | ||
283 | { | ||
284 | get | ||
285 | { | ||
286 | return m_physCost; | ||
287 | } | ||
288 | } | ||
289 | |||
290 | public override float StreamCost | ||
291 | { | ||
292 | get | ||
293 | { | ||
294 | return m_streamCost; | ||
295 | } | ||
296 | } | ||
297 | |||
298 | public override int PhysicsActorType | ||
299 | { | ||
300 | get { return (int)ActorTypes.Prim; } | ||
301 | set { return; } | ||
302 | } | ||
303 | |||
304 | public override bool SetAlwaysRun | ||
305 | { | ||
306 | get { return false; } | ||
307 | set { return; } | ||
308 | } | ||
309 | |||
310 | public override uint LocalID | ||
311 | { | ||
312 | get { return m_localID; } | ||
313 | set { m_localID = value; } | ||
314 | } | ||
315 | |||
316 | public override PhysicsActor ParentActor | ||
317 | { | ||
318 | get | ||
319 | { | ||
320 | if (childPrim) | ||
321 | return _parent; | ||
322 | else | ||
323 | return (PhysicsActor)this; | ||
324 | } | ||
325 | } | ||
326 | |||
327 | public override bool Grabbed | ||
328 | { | ||
329 | set { return; } | ||
330 | } | ||
331 | |||
332 | public override bool Selected | ||
333 | { | ||
334 | set | ||
335 | { | ||
336 | if (value) | ||
337 | m_isSelected = value; // if true set imediatly to stop moves etc | ||
338 | AddChange(changes.Selected, value); | ||
339 | } | ||
340 | } | ||
341 | |||
342 | public override bool Flying | ||
343 | { | ||
344 | // no flying prims for you | ||
345 | get { return false; } | ||
346 | set { } | ||
347 | } | ||
348 | |||
349 | public override bool IsColliding | ||
350 | { | ||
351 | get { return m_iscolliding; } | ||
352 | set | ||
353 | { | ||
354 | if (value) | ||
355 | { | ||
356 | m_colliderfilter += 2; | ||
357 | if (m_colliderfilter > 2) | ||
358 | m_colliderfilter = 2; | ||
359 | } | ||
360 | else | ||
361 | { | ||
362 | m_colliderfilter--; | ||
363 | if (m_colliderfilter < 0) | ||
364 | m_colliderfilter = 0; | ||
365 | } | ||
366 | |||
367 | if (m_colliderfilter == 0) | ||
368 | m_iscolliding = false; | ||
369 | else | ||
370 | m_iscolliding = true; | ||
371 | } | ||
372 | } | ||
373 | |||
374 | public override bool CollidingGround | ||
375 | { | ||
376 | get { return false; } | ||
377 | set { return; } | ||
378 | } | ||
379 | |||
380 | public override bool CollidingObj | ||
381 | { | ||
382 | get { return false; } | ||
383 | set { return; } | ||
384 | } | ||
385 | |||
386 | |||
387 | public override bool ThrottleUpdates {get;set;} | ||
388 | |||
389 | public override bool Stopped | ||
390 | { | ||
391 | get { return _zeroFlag; } | ||
392 | } | ||
393 | |||
394 | public override Vector3 Position | ||
395 | { | ||
396 | get | ||
397 | { | ||
398 | if (givefakepos > 0) | ||
399 | return fakepos; | ||
400 | else | ||
401 | return _position; | ||
402 | } | ||
403 | |||
404 | set | ||
405 | { | ||
406 | fakepos = value; | ||
407 | givefakepos++; | ||
408 | AddChange(changes.Position, value); | ||
409 | } | ||
410 | } | ||
411 | |||
412 | public override Vector3 Size | ||
413 | { | ||
414 | get { return _size; } | ||
415 | set | ||
416 | { | ||
417 | if (value.IsFinite()) | ||
418 | { | ||
419 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); | ||
420 | } | ||
421 | else | ||
422 | { | ||
423 | m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); | ||
424 | } | ||
425 | } | ||
426 | } | ||
427 | |||
428 | public override float Mass | ||
429 | { | ||
430 | get { return primMass; } | ||
431 | } | ||
432 | |||
433 | public override Vector3 Force | ||
434 | { | ||
435 | get { return m_force; } | ||
436 | set | ||
437 | { | ||
438 | if (value.IsFinite()) | ||
439 | { | ||
440 | AddChange(changes.Force, value); | ||
441 | } | ||
442 | else | ||
443 | { | ||
444 | m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); | ||
445 | } | ||
446 | } | ||
447 | } | ||
448 | |||
449 | public override void SetVolumeDetect(int param) | ||
450 | { | ||
451 | m_fakeisVolumeDetect = (param != 0); | ||
452 | AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); | ||
453 | } | ||
454 | |||
455 | public override Vector3 GeometricCenter | ||
456 | { | ||
457 | // this is not real geometric center but a average of positions relative to root prim acording to | ||
458 | // http://wiki.secondlife.com/wiki/llGetGeometricCenter | ||
459 | // ignoring tortured prims details since sl also seems to ignore | ||
460 | // so no real use in doing it on physics | ||
461 | get | ||
462 | { | ||
463 | return Vector3.Zero; | ||
464 | } | ||
465 | } | ||
466 | |||
467 | public override Vector3 CenterOfMass | ||
468 | { | ||
469 | get | ||
470 | { | ||
471 | lock (_parent_scene.OdeLock) | ||
472 | { | ||
473 | d.Vector3 dtmp; | ||
474 | if (!childPrim && Body != IntPtr.Zero) | ||
475 | { | ||
476 | dtmp = d.BodyGetPosition(Body); | ||
477 | return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
478 | } | ||
479 | else if (prim_geom != IntPtr.Zero) | ||
480 | { | ||
481 | d.Quaternion dq; | ||
482 | d.GeomCopyQuaternion(prim_geom, out dq); | ||
483 | Quaternion q; | ||
484 | q.X = dq.X; | ||
485 | q.Y = dq.Y; | ||
486 | q.Z = dq.Z; | ||
487 | q.W = dq.W; | ||
488 | |||
489 | Vector3 Ptot = m_OBBOffset * q; | ||
490 | dtmp = d.GeomGetPosition(prim_geom); | ||
491 | Ptot.X += dtmp.X; | ||
492 | Ptot.Y += dtmp.Y; | ||
493 | Ptot.Z += dtmp.Z; | ||
494 | |||
495 | // if(childPrim) we only know about physical linksets | ||
496 | return Ptot; | ||
497 | /* | ||
498 | float tmass = _mass; | ||
499 | Ptot *= tmass; | ||
500 | |||
501 | float m; | ||
502 | |||
503 | foreach (OdePrim prm in childrenPrim) | ||
504 | { | ||
505 | m = prm._mass; | ||
506 | Ptot += prm.CenterOfMass * m; | ||
507 | tmass += m; | ||
508 | } | ||
509 | |||
510 | if (tmass == 0) | ||
511 | tmass = 0; | ||
512 | else | ||
513 | tmass = 1.0f / tmass; | ||
514 | |||
515 | Ptot *= tmass; | ||
516 | return Ptot; | ||
517 | */ | ||
518 | } | ||
519 | else | ||
520 | return _position; | ||
521 | } | ||
522 | } | ||
523 | } | ||
524 | |||
525 | public override Vector3 OOBsize | ||
526 | { | ||
527 | get | ||
528 | { | ||
529 | return m_OBB; | ||
530 | } | ||
531 | } | ||
532 | |||
533 | public override Vector3 OOBoffset | ||
534 | { | ||
535 | get | ||
536 | { | ||
537 | return m_OBBOffset; | ||
538 | } | ||
539 | } | ||
540 | |||
541 | public override float OOBRadiusSQ | ||
542 | { | ||
543 | get | ||
544 | { | ||
545 | return primOOBradiusSQ; | ||
546 | } | ||
547 | } | ||
548 | |||
549 | public override PrimitiveBaseShape Shape | ||
550 | { | ||
551 | set | ||
552 | { | ||
553 | // AddChange(changes.Shape, value); | ||
554 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); | ||
555 | } | ||
556 | } | ||
557 | |||
558 | public override byte PhysicsShapeType | ||
559 | { | ||
560 | get | ||
561 | { | ||
562 | return m_shapetype; | ||
563 | } | ||
564 | set | ||
565 | { | ||
566 | m_shapetype = value; | ||
567 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); | ||
568 | } | ||
569 | } | ||
570 | |||
571 | public override Vector3 Velocity | ||
572 | { | ||
573 | get | ||
574 | { | ||
575 | if (_zeroFlag) | ||
576 | return Vector3.Zero; | ||
577 | return _velocity; | ||
578 | } | ||
579 | set | ||
580 | { | ||
581 | if (value.IsFinite()) | ||
582 | { | ||
583 | AddChange(changes.Velocity, value); | ||
584 | } | ||
585 | else | ||
586 | { | ||
587 | m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); | ||
588 | } | ||
589 | |||
590 | } | ||
591 | } | ||
592 | |||
593 | public override Vector3 Torque | ||
594 | { | ||
595 | get | ||
596 | { | ||
597 | if (!IsPhysical || Body == IntPtr.Zero) | ||
598 | return Vector3.Zero; | ||
599 | |||
600 | return m_torque; | ||
601 | } | ||
602 | |||
603 | set | ||
604 | { | ||
605 | if (value.IsFinite()) | ||
606 | { | ||
607 | AddChange(changes.Torque, value); | ||
608 | } | ||
609 | else | ||
610 | { | ||
611 | m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); | ||
612 | } | ||
613 | } | ||
614 | } | ||
615 | |||
616 | public override float CollisionScore | ||
617 | { | ||
618 | get { return m_collisionscore; } | ||
619 | set { m_collisionscore = value; } | ||
620 | } | ||
621 | |||
622 | public override bool Kinematic | ||
623 | { | ||
624 | get { return false; } | ||
625 | set { } | ||
626 | } | ||
627 | |||
628 | public override Quaternion Orientation | ||
629 | { | ||
630 | get | ||
631 | { | ||
632 | if (givefakeori > 0) | ||
633 | return fakeori; | ||
634 | else | ||
635 | |||
636 | return _orientation; | ||
637 | } | ||
638 | set | ||
639 | { | ||
640 | if (QuaternionIsFinite(value)) | ||
641 | { | ||
642 | fakeori = value; | ||
643 | givefakeori++; | ||
644 | |||
645 | value.Normalize(); | ||
646 | |||
647 | AddChange(changes.Orientation, value); | ||
648 | } | ||
649 | else | ||
650 | m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); | ||
651 | |||
652 | } | ||
653 | } | ||
654 | |||
655 | public override Vector3 Acceleration | ||
656 | { | ||
657 | get { return _acceleration; } | ||
658 | set { } | ||
659 | } | ||
660 | |||
661 | public override Vector3 RotationalVelocity | ||
662 | { | ||
663 | get | ||
664 | { | ||
665 | Vector3 pv = Vector3.Zero; | ||
666 | if (_zeroFlag) | ||
667 | return pv; | ||
668 | |||
669 | if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) | ||
670 | return pv; | ||
671 | |||
672 | return m_rotationalVelocity; | ||
673 | } | ||
674 | set | ||
675 | { | ||
676 | if (value.IsFinite()) | ||
677 | { | ||
678 | AddChange(changes.AngVelocity, value); | ||
679 | } | ||
680 | else | ||
681 | { | ||
682 | m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); | ||
683 | } | ||
684 | } | ||
685 | } | ||
686 | |||
687 | public override float Buoyancy | ||
688 | { | ||
689 | get { return m_buoyancy; } | ||
690 | set | ||
691 | { | ||
692 | AddChange(changes.Buoyancy,value); | ||
693 | } | ||
694 | } | ||
695 | |||
696 | public override bool FloatOnWater | ||
697 | { | ||
698 | set | ||
699 | { | ||
700 | AddChange(changes.CollidesWater, value); | ||
701 | } | ||
702 | } | ||
703 | |||
704 | public override Vector3 PIDTarget | ||
705 | { | ||
706 | set | ||
707 | { | ||
708 | if (value.IsFinite()) | ||
709 | { | ||
710 | AddChange(changes.PIDTarget,value); | ||
711 | } | ||
712 | else | ||
713 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | ||
714 | } | ||
715 | } | ||
716 | |||
717 | public override bool PIDActive | ||
718 | { | ||
719 | get | ||
720 | { | ||
721 | return m_usePID; | ||
722 | } | ||
723 | set | ||
724 | { | ||
725 | AddChange(changes.PIDActive,value); | ||
726 | } | ||
727 | } | ||
728 | |||
729 | public override float PIDTau | ||
730 | { | ||
731 | set | ||
732 | { | ||
733 | float tmp = 0; | ||
734 | if (value > 0) | ||
735 | { | ||
736 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); | ||
737 | if (value < mint) | ||
738 | tmp = mint; | ||
739 | else | ||
740 | tmp = value; | ||
741 | } | ||
742 | AddChange(changes.PIDTau,tmp); | ||
743 | } | ||
744 | } | ||
745 | |||
746 | public override float PIDHoverHeight | ||
747 | { | ||
748 | set | ||
749 | { | ||
750 | AddChange(changes.PIDHoverHeight,value); | ||
751 | } | ||
752 | } | ||
753 | public override bool PIDHoverActive | ||
754 | { | ||
755 | set | ||
756 | { | ||
757 | AddChange(changes.PIDHoverActive, value); | ||
758 | } | ||
759 | } | ||
760 | |||
761 | public override PIDHoverType PIDHoverType | ||
762 | { | ||
763 | set | ||
764 | { | ||
765 | AddChange(changes.PIDHoverType,value); | ||
766 | } | ||
767 | } | ||
768 | |||
769 | public override float PIDHoverTau | ||
770 | { | ||
771 | set | ||
772 | { | ||
773 | float tmp =0; | ||
774 | if (value > 0) | ||
775 | { | ||
776 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); | ||
777 | if (value < mint) | ||
778 | tmp = mint; | ||
779 | else | ||
780 | tmp = value; | ||
781 | } | ||
782 | AddChange(changes.PIDHoverTau, tmp); | ||
783 | } | ||
784 | } | ||
785 | |||
786 | public override Quaternion APIDTarget { set { return; } } | ||
787 | |||
788 | public override bool APIDActive { set { return; } } | ||
789 | |||
790 | public override float APIDStrength { set { return; } } | ||
791 | |||
792 | public override float APIDDamping { set { return; } } | ||
793 | |||
794 | public override int VehicleType | ||
795 | { | ||
796 | // we may need to put a fake on this | ||
797 | get | ||
798 | { | ||
799 | if (m_vehicle == null) | ||
800 | return (int)Vehicle.TYPE_NONE; | ||
801 | else | ||
802 | return (int)m_vehicle.Type; | ||
803 | } | ||
804 | set | ||
805 | { | ||
806 | AddChange(changes.VehicleType, value); | ||
807 | } | ||
808 | } | ||
809 | |||
810 | public override void VehicleFloatParam(int param, float value) | ||
811 | { | ||
812 | strVehicleFloatParam fp = new strVehicleFloatParam(); | ||
813 | fp.param = param; | ||
814 | fp.value = value; | ||
815 | AddChange(changes.VehicleFloatParam, fp); | ||
816 | } | ||
817 | |||
818 | public override void VehicleVectorParam(int param, Vector3 value) | ||
819 | { | ||
820 | strVehicleVectorParam fp = new strVehicleVectorParam(); | ||
821 | fp.param = param; | ||
822 | fp.value = value; | ||
823 | AddChange(changes.VehicleVectorParam, fp); | ||
824 | } | ||
825 | |||
826 | public override void VehicleRotationParam(int param, Quaternion value) | ||
827 | { | ||
828 | strVehicleQuatParam fp = new strVehicleQuatParam(); | ||
829 | fp.param = param; | ||
830 | fp.value = value; | ||
831 | AddChange(changes.VehicleRotationParam, fp); | ||
832 | } | ||
833 | |||
834 | public override void VehicleFlags(int param, bool value) | ||
835 | { | ||
836 | strVehicleBoolParam bp = new strVehicleBoolParam(); | ||
837 | bp.param = param; | ||
838 | bp.value = value; | ||
839 | AddChange(changes.VehicleFlags, bp); | ||
840 | } | ||
841 | |||
842 | public override void SetVehicle(object vdata) | ||
843 | { | ||
844 | AddChange(changes.SetVehicle, vdata); | ||
845 | } | ||
846 | public void SetAcceleration(Vector3 accel) | ||
847 | { | ||
848 | _acceleration = accel; | ||
849 | } | ||
850 | |||
851 | public override void AddForce(Vector3 force, bool pushforce) | ||
852 | { | ||
853 | if (force.IsFinite()) | ||
854 | { | ||
855 | if(pushforce) | ||
856 | AddChange(changes.AddForce, force); | ||
857 | else // a impulse | ||
858 | AddChange(changes.AddForce, force * m_invTimeStep); | ||
859 | } | ||
860 | else | ||
861 | { | ||
862 | m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); | ||
863 | } | ||
864 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
865 | } | ||
866 | |||
867 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
868 | { | ||
869 | if (force.IsFinite()) | ||
870 | { | ||
871 | // if(pushforce) for now applyrotationimpulse seems more happy applied as a force | ||
872 | AddChange(changes.AddAngForce, force); | ||
873 | // else // a impulse | ||
874 | // AddChange(changes.AddAngForce, force * m_invTimeStep); | ||
875 | } | ||
876 | else | ||
877 | { | ||
878 | m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); | ||
879 | } | ||
880 | } | ||
881 | |||
882 | public override void CrossingFailure() | ||
883 | { | ||
884 | if (m_outbounds) | ||
885 | { | ||
886 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
887 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
888 | _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); | ||
889 | |||
890 | m_lastposition = _position; | ||
891 | _velocity.X = 0; | ||
892 | _velocity.Y = 0; | ||
893 | _velocity.Z = 0; | ||
894 | |||
895 | m_lastVelocity = _velocity; | ||
896 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
897 | m_vehicle.Stop(); | ||
898 | |||
899 | if(Body != IntPtr.Zero) | ||
900 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
901 | if (prim_geom != IntPtr.Zero) | ||
902 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
903 | |||
904 | m_outbounds = false; | ||
905 | changeDisable(false); | ||
906 | base.RequestPhysicsterseUpdate(); | ||
907 | } | ||
908 | } | ||
909 | |||
910 | public override void SetMomentum(Vector3 momentum) | ||
911 | { | ||
912 | } | ||
913 | |||
914 | public override void SetMaterial(int pMaterial) | ||
915 | { | ||
916 | m_material = pMaterial; | ||
917 | mu = _parent_scene.m_materialContactsData[pMaterial].mu; | ||
918 | bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; | ||
919 | } | ||
920 | |||
921 | public override float Density | ||
922 | { | ||
923 | get | ||
924 | { | ||
925 | return m_density * 100f; | ||
926 | } | ||
927 | set | ||
928 | { | ||
929 | m_density = value / 100f; | ||
930 | // for not prim mass is not updated since this implies full rebuild of body inertia TODO | ||
931 | } | ||
932 | } | ||
933 | public override float GravModifier | ||
934 | { | ||
935 | get | ||
936 | { | ||
937 | return m_gravmod; | ||
938 | } | ||
939 | set | ||
940 | { | ||
941 | m_gravmod = value; | ||
942 | if (m_vehicle != null) | ||
943 | m_vehicle.GravMod = m_gravmod; | ||
944 | } | ||
945 | } | ||
946 | public override float Friction | ||
947 | { | ||
948 | get | ||
949 | { | ||
950 | return mu; | ||
951 | } | ||
952 | set | ||
953 | { | ||
954 | mu = value; | ||
955 | } | ||
956 | } | ||
957 | |||
958 | public override float Restitution | ||
959 | { | ||
960 | get | ||
961 | { | ||
962 | return bounce; | ||
963 | } | ||
964 | set | ||
965 | { | ||
966 | bounce = value; | ||
967 | } | ||
968 | } | ||
969 | |||
970 | public void setPrimForRemoval() | ||
971 | { | ||
972 | AddChange(changes.Remove, null); | ||
973 | } | ||
974 | |||
975 | public override void link(PhysicsActor obj) | ||
976 | { | ||
977 | AddChange(changes.Link, obj); | ||
978 | } | ||
979 | |||
980 | public override void delink() | ||
981 | { | ||
982 | AddChange(changes.DeLink, null); | ||
983 | } | ||
984 | |||
985 | public override void LockAngularMotion(Vector3 axis) | ||
986 | { | ||
987 | // reverse the zero/non zero values for ODE. | ||
988 | if (axis.IsFinite()) | ||
989 | { | ||
990 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
991 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
992 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
993 | // m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
994 | AddChange(changes.AngLock, axis); | ||
995 | } | ||
996 | else | ||
997 | { | ||
998 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | ||
999 | } | ||
1000 | } | ||
1001 | |||
1002 | public override void SubscribeEvents(int ms) | ||
1003 | { | ||
1004 | m_eventsubscription = ms; | ||
1005 | m_cureventsubscription = 0; | ||
1006 | if (CollisionEventsThisFrame == null) | ||
1007 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1008 | SentEmptyCollisionsEvent = false; | ||
1009 | } | ||
1010 | |||
1011 | public override void UnSubscribeEvents() | ||
1012 | { | ||
1013 | if (CollisionEventsThisFrame != null) | ||
1014 | { | ||
1015 | CollisionEventsThisFrame.Clear(); | ||
1016 | CollisionEventsThisFrame = null; | ||
1017 | } | ||
1018 | m_eventsubscription = 0; | ||
1019 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1020 | } | ||
1021 | |||
1022 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1023 | { | ||
1024 | if (CollisionEventsThisFrame == null) | ||
1025 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1026 | // if(CollisionEventsThisFrame.Count < 32) | ||
1027 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1028 | } | ||
1029 | |||
1030 | public void SendCollisions() | ||
1031 | { | ||
1032 | if (CollisionEventsThisFrame == null) | ||
1033 | return; | ||
1034 | |||
1035 | if (m_cureventsubscription < m_eventsubscription) | ||
1036 | return; | ||
1037 | |||
1038 | m_cureventsubscription = 0; | ||
1039 | |||
1040 | int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; | ||
1041 | |||
1042 | if (!SentEmptyCollisionsEvent || ncolisions > 0) | ||
1043 | { | ||
1044 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1045 | |||
1046 | if (ncolisions == 0) | ||
1047 | { | ||
1048 | SentEmptyCollisionsEvent = true; | ||
1049 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1050 | } | ||
1051 | else | ||
1052 | { | ||
1053 | SentEmptyCollisionsEvent = false; | ||
1054 | CollisionEventsThisFrame.Clear(); | ||
1055 | } | ||
1056 | } | ||
1057 | } | ||
1058 | |||
1059 | internal void AddCollisionFrameTime(int t) | ||
1060 | { | ||
1061 | if (m_cureventsubscription < 50000) | ||
1062 | m_cureventsubscription += t; | ||
1063 | } | ||
1064 | |||
1065 | public override bool SubscribedEvents() | ||
1066 | { | ||
1067 | if (m_eventsubscription > 0) | ||
1068 | return true; | ||
1069 | return false; | ||
1070 | } | ||
1071 | |||
1072 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
1073 | Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) | ||
1074 | { | ||
1075 | Name = primName; | ||
1076 | LocalID = plocalID; | ||
1077 | |||
1078 | m_vehicle = null; | ||
1079 | |||
1080 | if (!pos.IsFinite()) | ||
1081 | { | ||
1082 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | ||
1083 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | ||
1084 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); | ||
1085 | } | ||
1086 | _position = pos; | ||
1087 | givefakepos = 0; | ||
1088 | |||
1089 | m_timeStep = parent_scene.ODE_STEPSIZE; | ||
1090 | m_invTimeStep = 1f / m_timeStep; | ||
1091 | |||
1092 | m_density = parent_scene.geomDefaultDensity; | ||
1093 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
1094 | |||
1095 | prim_geom = IntPtr.Zero; | ||
1096 | collide_geom = IntPtr.Zero; | ||
1097 | Body = IntPtr.Zero; | ||
1098 | |||
1099 | if (!size.IsFinite()) | ||
1100 | { | ||
1101 | size = new Vector3(0.5f, 0.5f, 0.5f); | ||
1102 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); | ||
1103 | } | ||
1104 | |||
1105 | if (size.X <= 0) size.X = 0.01f; | ||
1106 | if (size.Y <= 0) size.Y = 0.01f; | ||
1107 | if (size.Z <= 0) size.Z = 0.01f; | ||
1108 | |||
1109 | _size = size; | ||
1110 | |||
1111 | if (!QuaternionIsFinite(rotation)) | ||
1112 | { | ||
1113 | rotation = Quaternion.Identity; | ||
1114 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); | ||
1115 | } | ||
1116 | |||
1117 | _orientation = rotation; | ||
1118 | givefakeori = 0; | ||
1119 | |||
1120 | _pbs = pbs; | ||
1121 | |||
1122 | _parent_scene = parent_scene; | ||
1123 | m_targetSpace = IntPtr.Zero; | ||
1124 | |||
1125 | if (pos.Z < 0) | ||
1126 | { | ||
1127 | m_isphysical = false; | ||
1128 | } | ||
1129 | else | ||
1130 | { | ||
1131 | m_isphysical = pisPhysical; | ||
1132 | } | ||
1133 | m_fakeisphysical = m_isphysical; | ||
1134 | |||
1135 | m_isVolumeDetect = false; | ||
1136 | m_fakeisVolumeDetect = false; | ||
1137 | |||
1138 | m_force = Vector3.Zero; | ||
1139 | |||
1140 | m_iscolliding = false; | ||
1141 | m_colliderfilter = 0; | ||
1142 | m_NoColide = false; | ||
1143 | |||
1144 | _triMeshData = IntPtr.Zero; | ||
1145 | |||
1146 | m_shapetype = _shapeType; | ||
1147 | |||
1148 | m_lastdoneSelected = false; | ||
1149 | m_isSelected = false; | ||
1150 | m_delaySelect = false; | ||
1151 | |||
1152 | m_isphantom = pisPhantom; | ||
1153 | m_fakeisphantom = pisPhantom; | ||
1154 | |||
1155 | mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; | ||
1156 | bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; | ||
1157 | |||
1158 | m_building = true; // control must set this to false when done | ||
1159 | |||
1160 | // get basic mass parameters | ||
1161 | ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); | ||
1162 | |||
1163 | primVolume = repData.volume; | ||
1164 | m_OBB = repData.OBB; | ||
1165 | m_OBBOffset = repData.OBBOffset; | ||
1166 | |||
1167 | UpdatePrimBodyData(); | ||
1168 | } | ||
1169 | |||
1170 | private void resetCollisionAccounting() | ||
1171 | { | ||
1172 | m_collisionscore = 0; | ||
1173 | } | ||
1174 | |||
1175 | private void UpdateCollisionCatFlags() | ||
1176 | { | ||
1177 | if(m_isphysical && m_disabled) | ||
1178 | { | ||
1179 | m_collisionCategories = 0; | ||
1180 | m_collisionFlags = 0; | ||
1181 | } | ||
1182 | |||
1183 | else if (m_isSelected) | ||
1184 | { | ||
1185 | m_collisionCategories = CollisionCategories.Selected; | ||
1186 | m_collisionFlags = 0; | ||
1187 | } | ||
1188 | |||
1189 | else if (m_isVolumeDetect) | ||
1190 | { | ||
1191 | m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1192 | if (m_isphysical) | ||
1193 | m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; | ||
1194 | else | ||
1195 | m_collisionFlags = 0; | ||
1196 | } | ||
1197 | else if (m_isphantom) | ||
1198 | { | ||
1199 | m_collisionCategories = CollisionCategories.Phantom; | ||
1200 | if (m_isphysical) | ||
1201 | m_collisionFlags = CollisionCategories.Land; | ||
1202 | else | ||
1203 | m_collisionFlags = 0; | ||
1204 | } | ||
1205 | else | ||
1206 | { | ||
1207 | m_collisionCategories = CollisionCategories.Geom; | ||
1208 | if (m_isphysical) | ||
1209 | m_collisionFlags = m_default_collisionFlagsPhysical; | ||
1210 | else | ||
1211 | m_collisionFlags = m_default_collisionFlagsNotPhysical; | ||
1212 | } | ||
1213 | } | ||
1214 | |||
1215 | private void ApplyCollisionCatFlags() | ||
1216 | { | ||
1217 | if (prim_geom != IntPtr.Zero) | ||
1218 | { | ||
1219 | if (!childPrim && childrenPrim.Count > 0) | ||
1220 | { | ||
1221 | foreach (OdePrim prm in childrenPrim) | ||
1222 | { | ||
1223 | if (m_isphysical && m_disabled) | ||
1224 | { | ||
1225 | prm.m_collisionCategories = 0; | ||
1226 | prm.m_collisionFlags = 0; | ||
1227 | } | ||
1228 | else | ||
1229 | { | ||
1230 | // preserve some | ||
1231 | if (prm.m_isSelected) | ||
1232 | { | ||
1233 | prm.m_collisionCategories = CollisionCategories.Selected; | ||
1234 | prm.m_collisionFlags = 0; | ||
1235 | } | ||
1236 | else if (prm.m_isVolumeDetect) | ||
1237 | { | ||
1238 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1239 | if (m_isphysical) | ||
1240 | prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; | ||
1241 | else | ||
1242 | prm.m_collisionFlags = 0; | ||
1243 | } | ||
1244 | else if (prm.m_isphantom) | ||
1245 | { | ||
1246 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1247 | if (m_isphysical) | ||
1248 | prm.m_collisionFlags = CollisionCategories.Land; | ||
1249 | else | ||
1250 | prm.m_collisionFlags = 0; | ||
1251 | } | ||
1252 | else | ||
1253 | { | ||
1254 | prm.m_collisionCategories = m_collisionCategories; | ||
1255 | prm.m_collisionFlags = m_collisionFlags; | ||
1256 | } | ||
1257 | } | ||
1258 | |||
1259 | if (prm.prim_geom != IntPtr.Zero) | ||
1260 | { | ||
1261 | if (prm.m_NoColide) | ||
1262 | { | ||
1263 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1264 | if (m_isphysical) | ||
1265 | d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); | ||
1266 | else | ||
1267 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1268 | } | ||
1269 | else | ||
1270 | { | ||
1271 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); | ||
1272 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1273 | } | ||
1274 | } | ||
1275 | } | ||
1276 | } | ||
1277 | |||
1278 | if (m_NoColide) | ||
1279 | { | ||
1280 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1281 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); | ||
1282 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1283 | { | ||
1284 | d.GeomSetCategoryBits(collide_geom, 0); | ||
1285 | d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); | ||
1286 | } | ||
1287 | } | ||
1288 | else | ||
1289 | { | ||
1290 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1291 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1292 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1293 | { | ||
1294 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); | ||
1295 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
1296 | } | ||
1297 | } | ||
1298 | } | ||
1299 | } | ||
1300 | |||
1301 | private void createAMotor(Vector3 axis) | ||
1302 | { | ||
1303 | if (Body == IntPtr.Zero) | ||
1304 | return; | ||
1305 | |||
1306 | if (Amotor != IntPtr.Zero) | ||
1307 | { | ||
1308 | d.JointDestroy(Amotor); | ||
1309 | Amotor = IntPtr.Zero; | ||
1310 | } | ||
1311 | |||
1312 | int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); | ||
1313 | |||
1314 | if (axisnum <= 0) | ||
1315 | return; | ||
1316 | |||
1317 | // stop it | ||
1318 | d.BodySetTorque(Body, 0, 0, 0); | ||
1319 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
1320 | |||
1321 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
1322 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
1323 | |||
1324 | d.JointSetAMotorMode(Amotor, 0); | ||
1325 | |||
1326 | d.JointSetAMotorNumAxes(Amotor, axisnum); | ||
1327 | |||
1328 | // get current orientation to lock | ||
1329 | |||
1330 | d.Quaternion dcur = d.BodyGetQuaternion(Body); | ||
1331 | Quaternion curr; // crap convertion between identical things | ||
1332 | curr.X = dcur.X; | ||
1333 | curr.Y = dcur.Y; | ||
1334 | curr.Z = dcur.Z; | ||
1335 | curr.W = dcur.W; | ||
1336 | Vector3 ax; | ||
1337 | |||
1338 | int i = 0; | ||
1339 | int j = 0; | ||
1340 | if (axis.X == 0) | ||
1341 | { | ||
1342 | ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X | ||
1343 | // ODE should do this with axis relative to body 1 but seems to fail | ||
1344 | d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); | ||
1345 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
1346 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); | ||
1347 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); | ||
1348 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
1349 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1350 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); | ||
1351 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); | ||
1352 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); | ||
1353 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); | ||
1354 | i++; | ||
1355 | j = 256; // move to next axis set | ||
1356 | } | ||
1357 | |||
1358 | if (axis.Y == 0) | ||
1359 | { | ||
1360 | ax = (new Vector3(0, 1, 0)) * curr; | ||
1361 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
1362 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
1363 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
1364 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
1365 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1366 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1367 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1368 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1369 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1370 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1371 | i++; | ||
1372 | j += 256; | ||
1373 | } | ||
1374 | |||
1375 | if (axis.Z == 0) | ||
1376 | { | ||
1377 | ax = (new Vector3(0, 0, 1)) * curr; | ||
1378 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
1379 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
1380 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
1381 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
1382 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1383 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1384 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1385 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1386 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1387 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1388 | } | ||
1389 | } | ||
1390 | |||
1391 | |||
1392 | private void SetGeom(IntPtr geom) | ||
1393 | { | ||
1394 | prim_geom = geom; | ||
1395 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); | ||
1396 | if (prim_geom != IntPtr.Zero) | ||
1397 | { | ||
1398 | |||
1399 | if (m_NoColide) | ||
1400 | { | ||
1401 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1402 | if (m_isphysical) | ||
1403 | { | ||
1404 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); | ||
1405 | } | ||
1406 | else | ||
1407 | { | ||
1408 | d.GeomSetCollideBits(prim_geom, 0); | ||
1409 | d.GeomDisable(prim_geom); | ||
1410 | } | ||
1411 | } | ||
1412 | else | ||
1413 | { | ||
1414 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1415 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1416 | } | ||
1417 | |||
1418 | UpdatePrimBodyData(); | ||
1419 | _parent_scene.actor_name_map[prim_geom] = this; | ||
1420 | |||
1421 | /* | ||
1422 | // debug | ||
1423 | d.AABB aabb; | ||
1424 | d.GeomGetAABB(prim_geom, out aabb); | ||
1425 | float x = aabb.MaxX - aabb.MinX; | ||
1426 | float y = aabb.MaxY - aabb.MinY; | ||
1427 | float z = aabb.MaxZ - aabb.MinZ; | ||
1428 | if( x > 60.0f || y > 60.0f || z > 60.0f) | ||
1429 | m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1430 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1431 | else if (x < 0.001f || y < 0.001f || z < 0.001f) | ||
1432 | m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1433 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1434 | |||
1435 | // | ||
1436 | */ | ||
1437 | |||
1438 | } | ||
1439 | else | ||
1440 | m_log.Warn("Setting bad Geom"); | ||
1441 | } | ||
1442 | |||
1443 | private bool GetMeshGeom() | ||
1444 | { | ||
1445 | IntPtr vertices, indices; | ||
1446 | int vertexCount, indexCount; | ||
1447 | int vertexStride, triStride; | ||
1448 | |||
1449 | IMesh mesh = m_mesh; | ||
1450 | |||
1451 | if (mesh == null) | ||
1452 | return false; | ||
1453 | |||
1454 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); | ||
1455 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); | ||
1456 | |||
1457 | if (vertexCount == 0 || indexCount == 0) | ||
1458 | { | ||
1459 | m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", | ||
1460 | Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); | ||
1461 | |||
1462 | m_hasOBB = false; | ||
1463 | m_OBBOffset = Vector3.Zero; | ||
1464 | m_OBB = _size * 0.5f; | ||
1465 | |||
1466 | m_physCost = 0.1f; | ||
1467 | m_streamCost = 1.0f; | ||
1468 | |||
1469 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1470 | m_meshState = MeshState.MeshFailed; | ||
1471 | m_mesh = null; | ||
1472 | return false; | ||
1473 | } | ||
1474 | |||
1475 | IntPtr geo = IntPtr.Zero; | ||
1476 | |||
1477 | try | ||
1478 | { | ||
1479 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
1480 | |||
1481 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | ||
1482 | d.GeomTriMeshDataPreprocess(_triMeshData); | ||
1483 | |||
1484 | geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); | ||
1485 | } | ||
1486 | |||
1487 | catch (Exception e) | ||
1488 | { | ||
1489 | m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); | ||
1490 | if (_triMeshData != IntPtr.Zero) | ||
1491 | { | ||
1492 | try | ||
1493 | { | ||
1494 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1495 | } | ||
1496 | catch | ||
1497 | { | ||
1498 | } | ||
1499 | } | ||
1500 | _triMeshData = IntPtr.Zero; | ||
1501 | |||
1502 | m_hasOBB = false; | ||
1503 | m_OBBOffset = Vector3.Zero; | ||
1504 | m_OBB = _size * 0.5f; | ||
1505 | m_physCost = 0.1f; | ||
1506 | m_streamCost = 1.0f; | ||
1507 | |||
1508 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1509 | m_meshState = MeshState.MeshFailed; | ||
1510 | m_mesh = null; | ||
1511 | return false; | ||
1512 | } | ||
1513 | |||
1514 | m_physCost = 0.0013f * (float)indexCount; | ||
1515 | // todo | ||
1516 | m_streamCost = 1.0f; | ||
1517 | |||
1518 | SetGeom(geo); | ||
1519 | |||
1520 | return true; | ||
1521 | } | ||
1522 | |||
1523 | private void CreateGeom() | ||
1524 | { | ||
1525 | bool hasMesh = false; | ||
1526 | |||
1527 | m_NoColide = false; | ||
1528 | |||
1529 | if ((m_meshState & MeshState.MeshNoColide) != 0) | ||
1530 | m_NoColide = true; | ||
1531 | |||
1532 | else if(m_mesh != null) | ||
1533 | { | ||
1534 | if (GetMeshGeom()) | ||
1535 | hasMesh = true; | ||
1536 | else | ||
1537 | m_NoColide = true; | ||
1538 | } | ||
1539 | |||
1540 | |||
1541 | if (!hasMesh) | ||
1542 | { | ||
1543 | IntPtr geo = IntPtr.Zero; | ||
1544 | |||
1545 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 | ||
1546 | && _size.X == _size.Y && _size.Y == _size.Z) | ||
1547 | { // it's a sphere | ||
1548 | try | ||
1549 | { | ||
1550 | geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); | ||
1551 | } | ||
1552 | catch (Exception e) | ||
1553 | { | ||
1554 | m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); | ||
1555 | return; | ||
1556 | } | ||
1557 | } | ||
1558 | else | ||
1559 | {// do it as a box | ||
1560 | try | ||
1561 | { | ||
1562 | geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); | ||
1563 | } | ||
1564 | catch (Exception e) | ||
1565 | { | ||
1566 | m_log.Warn("[PHYSICS]: Create box failed: {0}", e); | ||
1567 | return; | ||
1568 | } | ||
1569 | } | ||
1570 | m_physCost = 0.1f; | ||
1571 | m_streamCost = 1.0f; | ||
1572 | SetGeom(geo); | ||
1573 | } | ||
1574 | } | ||
1575 | |||
1576 | private void RemoveGeom() | ||
1577 | { | ||
1578 | if (prim_geom != IntPtr.Zero) | ||
1579 | { | ||
1580 | _parent_scene.actor_name_map.Remove(prim_geom); | ||
1581 | |||
1582 | try | ||
1583 | { | ||
1584 | d.GeomDestroy(prim_geom); | ||
1585 | if (_triMeshData != IntPtr.Zero) | ||
1586 | { | ||
1587 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1588 | _triMeshData = IntPtr.Zero; | ||
1589 | } | ||
1590 | } | ||
1591 | catch (Exception e) | ||
1592 | { | ||
1593 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); | ||
1594 | } | ||
1595 | |||
1596 | prim_geom = IntPtr.Zero; | ||
1597 | collide_geom = IntPtr.Zero; | ||
1598 | m_targetSpace = IntPtr.Zero; | ||
1599 | } | ||
1600 | else | ||
1601 | { | ||
1602 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); | ||
1603 | } | ||
1604 | |||
1605 | lock (m_meshlock) | ||
1606 | { | ||
1607 | if (m_mesh != null) | ||
1608 | { | ||
1609 | _parent_scene.mesher.ReleaseMesh(m_mesh); | ||
1610 | m_mesh = null; | ||
1611 | } | ||
1612 | } | ||
1613 | |||
1614 | Body = IntPtr.Zero; | ||
1615 | m_hasOBB = false; | ||
1616 | } | ||
1617 | |||
1618 | //sets non physical prim m_targetSpace to right space in spaces grid for static prims | ||
1619 | // should only be called for non physical prims unless they are becoming non physical | ||
1620 | private void SetInStaticSpace(OdePrim prim) | ||
1621 | { | ||
1622 | IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); | ||
1623 | prim.m_targetSpace = targetSpace; | ||
1624 | collide_geom = IntPtr.Zero; | ||
1625 | } | ||
1626 | |||
1627 | public void enableBodySoft() | ||
1628 | { | ||
1629 | m_disabled = false; | ||
1630 | if (!childPrim && !m_isSelected) | ||
1631 | { | ||
1632 | if (m_isphysical && Body != IntPtr.Zero) | ||
1633 | { | ||
1634 | UpdateCollisionCatFlags(); | ||
1635 | ApplyCollisionCatFlags(); | ||
1636 | |||
1637 | d.BodyEnable(Body); | ||
1638 | } | ||
1639 | } | ||
1640 | resetCollisionAccounting(); | ||
1641 | } | ||
1642 | |||
1643 | private void disableBodySoft() | ||
1644 | { | ||
1645 | m_disabled = true; | ||
1646 | if (!childPrim) | ||
1647 | { | ||
1648 | if (m_isphysical && Body != IntPtr.Zero) | ||
1649 | { | ||
1650 | if (m_isSelected) | ||
1651 | m_collisionFlags = CollisionCategories.Selected; | ||
1652 | else | ||
1653 | m_collisionCategories = 0; | ||
1654 | m_collisionFlags = 0; | ||
1655 | ApplyCollisionCatFlags(); | ||
1656 | d.BodyDisable(Body); | ||
1657 | } | ||
1658 | } | ||
1659 | } | ||
1660 | |||
1661 | private void MakeBody() | ||
1662 | { | ||
1663 | if (!m_isphysical) // only physical get bodies | ||
1664 | return; | ||
1665 | |||
1666 | if (childPrim) // child prims don't get bodies; | ||
1667 | return; | ||
1668 | |||
1669 | if (m_building) | ||
1670 | return; | ||
1671 | |||
1672 | if (prim_geom == IntPtr.Zero) | ||
1673 | { | ||
1674 | m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); | ||
1675 | return; | ||
1676 | } | ||
1677 | |||
1678 | if (Body != IntPtr.Zero) | ||
1679 | { | ||
1680 | DestroyBody(); | ||
1681 | m_log.Warn("[PHYSICS]: MakeBody called having a body"); | ||
1682 | } | ||
1683 | |||
1684 | if (d.GeomGetBody(prim_geom) != IntPtr.Zero) | ||
1685 | { | ||
1686 | d.GeomSetBody(prim_geom, IntPtr.Zero); | ||
1687 | m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); | ||
1688 | } | ||
1689 | |||
1690 | d.Matrix3 mymat = new d.Matrix3(); | ||
1691 | d.Quaternion myrot = new d.Quaternion(); | ||
1692 | d.Mass objdmass = new d.Mass { }; | ||
1693 | |||
1694 | Body = d.BodyCreate(_parent_scene.world); | ||
1695 | |||
1696 | objdmass = primdMass; | ||
1697 | |||
1698 | // rotate inertia | ||
1699 | myrot.X = _orientation.X; | ||
1700 | myrot.Y = _orientation.Y; | ||
1701 | myrot.Z = _orientation.Z; | ||
1702 | myrot.W = _orientation.W; | ||
1703 | |||
1704 | d.RfromQ(out mymat, ref myrot); | ||
1705 | d.MassRotate(ref objdmass, ref mymat); | ||
1706 | |||
1707 | // set the body rotation | ||
1708 | d.BodySetRotation(Body, ref mymat); | ||
1709 | |||
1710 | // recompute full object inertia if needed | ||
1711 | if (childrenPrim.Count > 0) | ||
1712 | { | ||
1713 | d.Matrix3 mat = new d.Matrix3(); | ||
1714 | d.Quaternion quat = new d.Quaternion(); | ||
1715 | d.Mass tmpdmass = new d.Mass { }; | ||
1716 | Vector3 rcm; | ||
1717 | |||
1718 | rcm.X = _position.X; | ||
1719 | rcm.Y = _position.Y; | ||
1720 | rcm.Z = _position.Z; | ||
1721 | |||
1722 | lock (childrenPrim) | ||
1723 | { | ||
1724 | foreach (OdePrim prm in childrenPrim) | ||
1725 | { | ||
1726 | if (prm.prim_geom == IntPtr.Zero) | ||
1727 | { | ||
1728 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); | ||
1729 | continue; | ||
1730 | } | ||
1731 | |||
1732 | tmpdmass = prm.primdMass; | ||
1733 | |||
1734 | // apply prim current rotation to inertia | ||
1735 | quat.X = prm._orientation.X; | ||
1736 | quat.Y = prm._orientation.Y; | ||
1737 | quat.Z = prm._orientation.Z; | ||
1738 | quat.W = prm._orientation.W; | ||
1739 | d.RfromQ(out mat, ref quat); | ||
1740 | d.MassRotate(ref tmpdmass, ref mat); | ||
1741 | |||
1742 | Vector3 ppos = prm._position; | ||
1743 | ppos.X -= rcm.X; | ||
1744 | ppos.Y -= rcm.Y; | ||
1745 | ppos.Z -= rcm.Z; | ||
1746 | // refer inertia to root prim center of mass position | ||
1747 | d.MassTranslate(ref tmpdmass, | ||
1748 | ppos.X, | ||
1749 | ppos.Y, | ||
1750 | ppos.Z); | ||
1751 | |||
1752 | d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia | ||
1753 | // fix prim colision cats | ||
1754 | |||
1755 | if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) | ||
1756 | { | ||
1757 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); | ||
1758 | m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); | ||
1759 | } | ||
1760 | |||
1761 | d.GeomClearOffset(prm.prim_geom); | ||
1762 | d.GeomSetBody(prm.prim_geom, Body); | ||
1763 | prm.Body = Body; | ||
1764 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation | ||
1765 | } | ||
1766 | } | ||
1767 | } | ||
1768 | |||
1769 | d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset | ||
1770 | // associate root geom with body | ||
1771 | d.GeomSetBody(prim_geom, Body); | ||
1772 | |||
1773 | d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); | ||
1774 | d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1775 | |||
1776 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
1777 | myrot.X = -myrot.X; | ||
1778 | myrot.Y = -myrot.Y; | ||
1779 | myrot.Z = -myrot.Z; | ||
1780 | |||
1781 | d.RfromQ(out mymat, ref myrot); | ||
1782 | d.MassRotate(ref objdmass, ref mymat); | ||
1783 | |||
1784 | d.BodySetMass(Body, ref objdmass); | ||
1785 | _mass = objdmass.mass; | ||
1786 | |||
1787 | // disconnect from world gravity so we can apply buoyancy | ||
1788 | d.BodySetGravityMode(Body, false); | ||
1789 | |||
1790 | d.BodySetAutoDisableFlag(Body, true); | ||
1791 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1792 | d.BodySetAutoDisableAngularThreshold(Body, 0.01f); | ||
1793 | d.BodySetAutoDisableLinearThreshold(Body, 0.01f); | ||
1794 | d.BodySetDamping(Body, .005f, .001f); | ||
1795 | |||
1796 | if (m_targetSpace != IntPtr.Zero) | ||
1797 | { | ||
1798 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1799 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
1800 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
1801 | } | ||
1802 | |||
1803 | if (childrenPrim.Count == 0) | ||
1804 | { | ||
1805 | collide_geom = prim_geom; | ||
1806 | m_targetSpace = _parent_scene.ActiveSpace; | ||
1807 | } | ||
1808 | else | ||
1809 | { | ||
1810 | m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); | ||
1811 | d.HashSpaceSetLevels(m_targetSpace, -2, 8); | ||
1812 | d.SpaceSetSublevel(m_targetSpace, 3); | ||
1813 | d.SpaceSetCleanup(m_targetSpace, false); | ||
1814 | |||
1815 | d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | | ||
1816 | CollisionCategories.Geom | | ||
1817 | CollisionCategories.Phantom | | ||
1818 | CollisionCategories.VolumeDtc | ||
1819 | )); | ||
1820 | d.GeomSetCollideBits(m_targetSpace, 0); | ||
1821 | collide_geom = m_targetSpace; | ||
1822 | } | ||
1823 | |||
1824 | d.SpaceAdd(m_targetSpace, prim_geom); | ||
1825 | |||
1826 | if (m_delaySelect) | ||
1827 | { | ||
1828 | m_isSelected = true; | ||
1829 | m_delaySelect = false; | ||
1830 | } | ||
1831 | |||
1832 | m_collisionscore = 0; | ||
1833 | |||
1834 | UpdateCollisionCatFlags(); | ||
1835 | ApplyCollisionCatFlags(); | ||
1836 | |||
1837 | _parent_scene.addActivePrim(this); | ||
1838 | |||
1839 | lock (childrenPrim) | ||
1840 | { | ||
1841 | foreach (OdePrim prm in childrenPrim) | ||
1842 | { | ||
1843 | if (prm.prim_geom == IntPtr.Zero) | ||
1844 | continue; | ||
1845 | |||
1846 | Vector3 ppos = prm._position; | ||
1847 | d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position | ||
1848 | |||
1849 | if (prm.m_targetSpace != m_targetSpace) | ||
1850 | { | ||
1851 | if (prm.m_targetSpace != IntPtr.Zero) | ||
1852 | { | ||
1853 | _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); | ||
1854 | if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) | ||
1855 | d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); | ||
1856 | } | ||
1857 | prm.m_targetSpace = m_targetSpace; | ||
1858 | d.SpaceAdd(m_targetSpace, prm.prim_geom); | ||
1859 | } | ||
1860 | |||
1861 | prm.m_collisionscore = 0; | ||
1862 | |||
1863 | if(!m_disabled) | ||
1864 | prm.m_disabled = false; | ||
1865 | |||
1866 | _parent_scene.addActivePrim(prm); | ||
1867 | } | ||
1868 | } | ||
1869 | |||
1870 | // The body doesn't already have a finite rotation mode set here | ||
1871 | if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) | ||
1872 | { | ||
1873 | createAMotor(m_angularlock); | ||
1874 | } | ||
1875 | |||
1876 | |||
1877 | if (m_isSelected || m_disabled) | ||
1878 | { | ||
1879 | d.BodyDisable(Body); | ||
1880 | } | ||
1881 | else | ||
1882 | { | ||
1883 | d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); | ||
1884 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | ||
1885 | } | ||
1886 | _parent_scene.addActiveGroups(this); | ||
1887 | } | ||
1888 | |||
1889 | private void DestroyBody() | ||
1890 | { | ||
1891 | if (Body != IntPtr.Zero) | ||
1892 | { | ||
1893 | _parent_scene.remActivePrim(this); | ||
1894 | |||
1895 | collide_geom = IntPtr.Zero; | ||
1896 | |||
1897 | if (m_disabled) | ||
1898 | m_collisionCategories = 0; | ||
1899 | else if (m_isSelected) | ||
1900 | m_collisionCategories = CollisionCategories.Selected; | ||
1901 | else if (m_isVolumeDetect) | ||
1902 | m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1903 | else if (m_isphantom) | ||
1904 | m_collisionCategories = CollisionCategories.Phantom; | ||
1905 | else | ||
1906 | m_collisionCategories = CollisionCategories.Geom; | ||
1907 | |||
1908 | m_collisionFlags = 0; | ||
1909 | |||
1910 | if (prim_geom != IntPtr.Zero) | ||
1911 | { | ||
1912 | if (m_NoColide) | ||
1913 | { | ||
1914 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1915 | d.GeomSetCollideBits(prim_geom, 0); | ||
1916 | } | ||
1917 | else | ||
1918 | { | ||
1919 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1920 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1921 | } | ||
1922 | UpdateDataFromGeom(); | ||
1923 | d.GeomSetBody(prim_geom, IntPtr.Zero); | ||
1924 | SetInStaticSpace(this); | ||
1925 | } | ||
1926 | |||
1927 | if (!childPrim) | ||
1928 | { | ||
1929 | lock (childrenPrim) | ||
1930 | { | ||
1931 | foreach (OdePrim prm in childrenPrim) | ||
1932 | { | ||
1933 | _parent_scene.remActivePrim(prm); | ||
1934 | |||
1935 | if (prm.m_isSelected) | ||
1936 | prm.m_collisionCategories = CollisionCategories.Selected; | ||
1937 | else if (prm.m_isVolumeDetect) | ||
1938 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1939 | else if (prm.m_isphantom) | ||
1940 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1941 | else | ||
1942 | prm.m_collisionCategories = CollisionCategories.Geom; | ||
1943 | |||
1944 | prm.m_collisionFlags = 0; | ||
1945 | |||
1946 | if (prm.prim_geom != IntPtr.Zero) | ||
1947 | { | ||
1948 | if (prm.m_NoColide) | ||
1949 | { | ||
1950 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1951 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1952 | } | ||
1953 | else | ||
1954 | { | ||
1955 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); | ||
1956 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1957 | } | ||
1958 | prm.UpdateDataFromGeom(); | ||
1959 | SetInStaticSpace(prm); | ||
1960 | } | ||
1961 | prm.Body = IntPtr.Zero; | ||
1962 | prm._mass = prm.primMass; | ||
1963 | prm.m_collisionscore = 0; | ||
1964 | } | ||
1965 | } | ||
1966 | if (Amotor != IntPtr.Zero) | ||
1967 | { | ||
1968 | d.JointDestroy(Amotor); | ||
1969 | Amotor = IntPtr.Zero; | ||
1970 | } | ||
1971 | _parent_scene.remActiveGroup(this); | ||
1972 | d.BodyDestroy(Body); | ||
1973 | } | ||
1974 | Body = IntPtr.Zero; | ||
1975 | } | ||
1976 | _mass = primMass; | ||
1977 | m_collisionscore = 0; | ||
1978 | } | ||
1979 | |||
1980 | private void FixInertia(Vector3 NewPos,Quaternion newrot) | ||
1981 | { | ||
1982 | d.Matrix3 mat = new d.Matrix3(); | ||
1983 | d.Quaternion quat = new d.Quaternion(); | ||
1984 | |||
1985 | d.Mass tmpdmass = new d.Mass { }; | ||
1986 | d.Mass objdmass = new d.Mass { }; | ||
1987 | |||
1988 | d.BodyGetMass(Body, out tmpdmass); | ||
1989 | objdmass = tmpdmass; | ||
1990 | |||
1991 | d.Vector3 dobjpos; | ||
1992 | d.Vector3 thispos; | ||
1993 | |||
1994 | // get current object position and rotation | ||
1995 | dobjpos = d.BodyGetPosition(Body); | ||
1996 | |||
1997 | // get prim own inertia in its local frame | ||
1998 | tmpdmass = primdMass; | ||
1999 | |||
2000 | // transform to object frame | ||
2001 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2002 | d.MassRotate(ref tmpdmass, ref mat); | ||
2003 | |||
2004 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2005 | d.MassTranslate(ref tmpdmass, | ||
2006 | thispos.X, | ||
2007 | thispos.Y, | ||
2008 | thispos.Z); | ||
2009 | |||
2010 | // subtract current prim inertia from object | ||
2011 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2012 | |||
2013 | // back prim own inertia | ||
2014 | tmpdmass = primdMass; | ||
2015 | |||
2016 | // update to new position and orientation | ||
2017 | _position = NewPos; | ||
2018 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); | ||
2019 | _orientation = newrot; | ||
2020 | quat.X = newrot.X; | ||
2021 | quat.Y = newrot.Y; | ||
2022 | quat.Z = newrot.Z; | ||
2023 | quat.W = newrot.W; | ||
2024 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); | ||
2025 | |||
2026 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2027 | d.MassRotate(ref tmpdmass, ref mat); | ||
2028 | |||
2029 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2030 | d.MassTranslate(ref tmpdmass, | ||
2031 | thispos.X, | ||
2032 | thispos.Y, | ||
2033 | thispos.Z); | ||
2034 | |||
2035 | d.MassAdd(ref objdmass, ref tmpdmass); | ||
2036 | |||
2037 | // fix all positions | ||
2038 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2039 | while (g != IntPtr.Zero) | ||
2040 | { | ||
2041 | thispos = d.GeomGetOffsetPosition(g); | ||
2042 | thispos.X -= objdmass.c.X; | ||
2043 | thispos.Y -= objdmass.c.Y; | ||
2044 | thispos.Z -= objdmass.c.Z; | ||
2045 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2046 | g = d.dBodyGetNextGeom(g); | ||
2047 | } | ||
2048 | d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); | ||
2049 | |||
2050 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2051 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2052 | d.BodySetMass(Body, ref objdmass); | ||
2053 | _mass = objdmass.mass; | ||
2054 | } | ||
2055 | |||
2056 | |||
2057 | |||
2058 | private void FixInertia(Vector3 NewPos) | ||
2059 | { | ||
2060 | d.Matrix3 primmat = new d.Matrix3(); | ||
2061 | d.Mass tmpdmass = new d.Mass { }; | ||
2062 | d.Mass objdmass = new d.Mass { }; | ||
2063 | d.Mass primmass = new d.Mass { }; | ||
2064 | |||
2065 | d.Vector3 dobjpos; | ||
2066 | d.Vector3 thispos; | ||
2067 | |||
2068 | d.BodyGetMass(Body, out objdmass); | ||
2069 | |||
2070 | // get prim own inertia in its local frame | ||
2071 | primmass = primdMass; | ||
2072 | // transform to object frame | ||
2073 | primmat = d.GeomGetOffsetRotation(prim_geom); | ||
2074 | d.MassRotate(ref primmass, ref primmat); | ||
2075 | |||
2076 | tmpdmass = primmass; | ||
2077 | |||
2078 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2079 | d.MassTranslate(ref tmpdmass, | ||
2080 | thispos.X, | ||
2081 | thispos.Y, | ||
2082 | thispos.Z); | ||
2083 | |||
2084 | // subtract current prim inertia from object | ||
2085 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2086 | |||
2087 | // update to new position | ||
2088 | _position = NewPos; | ||
2089 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); | ||
2090 | |||
2091 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2092 | d.MassTranslate(ref primmass, | ||
2093 | thispos.X, | ||
2094 | thispos.Y, | ||
2095 | thispos.Z); | ||
2096 | |||
2097 | d.MassAdd(ref objdmass, ref primmass); | ||
2098 | |||
2099 | // fix all positions | ||
2100 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2101 | while (g != IntPtr.Zero) | ||
2102 | { | ||
2103 | thispos = d.GeomGetOffsetPosition(g); | ||
2104 | thispos.X -= objdmass.c.X; | ||
2105 | thispos.Y -= objdmass.c.Y; | ||
2106 | thispos.Z -= objdmass.c.Z; | ||
2107 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2108 | g = d.dBodyGetNextGeom(g); | ||
2109 | } | ||
2110 | |||
2111 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); | ||
2112 | |||
2113 | // get current object position and rotation | ||
2114 | dobjpos = d.BodyGetPosition(Body); | ||
2115 | |||
2116 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2117 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2118 | d.BodySetMass(Body, ref objdmass); | ||
2119 | _mass = objdmass.mass; | ||
2120 | } | ||
2121 | |||
2122 | private void FixInertia(Quaternion newrot) | ||
2123 | { | ||
2124 | d.Matrix3 mat = new d.Matrix3(); | ||
2125 | d.Quaternion quat = new d.Quaternion(); | ||
2126 | |||
2127 | d.Mass tmpdmass = new d.Mass { }; | ||
2128 | d.Mass objdmass = new d.Mass { }; | ||
2129 | d.Vector3 dobjpos; | ||
2130 | d.Vector3 thispos; | ||
2131 | |||
2132 | d.BodyGetMass(Body, out objdmass); | ||
2133 | |||
2134 | // get prim own inertia in its local frame | ||
2135 | tmpdmass = primdMass; | ||
2136 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2137 | d.MassRotate(ref tmpdmass, ref mat); | ||
2138 | // transform to object frame | ||
2139 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2140 | d.MassTranslate(ref tmpdmass, | ||
2141 | thispos.X, | ||
2142 | thispos.Y, | ||
2143 | thispos.Z); | ||
2144 | |||
2145 | // subtract current prim inertia from object | ||
2146 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2147 | |||
2148 | // update to new orientation | ||
2149 | _orientation = newrot; | ||
2150 | quat.X = newrot.X; | ||
2151 | quat.Y = newrot.Y; | ||
2152 | quat.Z = newrot.Z; | ||
2153 | quat.W = newrot.W; | ||
2154 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); | ||
2155 | |||
2156 | tmpdmass = primdMass; | ||
2157 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2158 | d.MassRotate(ref tmpdmass, ref mat); | ||
2159 | d.MassTranslate(ref tmpdmass, | ||
2160 | thispos.X, | ||
2161 | thispos.Y, | ||
2162 | thispos.Z); | ||
2163 | |||
2164 | d.MassAdd(ref objdmass, ref tmpdmass); | ||
2165 | |||
2166 | // fix all positions | ||
2167 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2168 | while (g != IntPtr.Zero) | ||
2169 | { | ||
2170 | thispos = d.GeomGetOffsetPosition(g); | ||
2171 | thispos.X -= objdmass.c.X; | ||
2172 | thispos.Y -= objdmass.c.Y; | ||
2173 | thispos.Z -= objdmass.c.Z; | ||
2174 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2175 | g = d.dBodyGetNextGeom(g); | ||
2176 | } | ||
2177 | |||
2178 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); | ||
2179 | // get current object position and rotation | ||
2180 | dobjpos = d.BodyGetPosition(Body); | ||
2181 | |||
2182 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2183 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2184 | d.BodySetMass(Body, ref objdmass); | ||
2185 | _mass = objdmass.mass; | ||
2186 | } | ||
2187 | |||
2188 | |||
2189 | #region Mass Calculation | ||
2190 | |||
2191 | private void UpdatePrimBodyData() | ||
2192 | { | ||
2193 | primMass = m_density * primVolume; | ||
2194 | |||
2195 | if (primMass <= 0) | ||
2196 | primMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
2197 | if (primMass > _parent_scene.maximumMassObject) | ||
2198 | primMass = _parent_scene.maximumMassObject; | ||
2199 | |||
2200 | _mass = primMass; // just in case | ||
2201 | |||
2202 | d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); | ||
2203 | |||
2204 | d.MassTranslate(ref primdMass, | ||
2205 | m_OBBOffset.X, | ||
2206 | m_OBBOffset.Y, | ||
2207 | m_OBBOffset.Z); | ||
2208 | |||
2209 | primOOBradiusSQ = m_OBB.LengthSquared(); | ||
2210 | |||
2211 | if (_triMeshData != IntPtr.Zero) | ||
2212 | { | ||
2213 | float pc = m_physCost; | ||
2214 | float psf = primOOBradiusSQ; | ||
2215 | psf *= 1.33f * .2f; | ||
2216 | pc *= psf; | ||
2217 | if (pc < 0.1f) | ||
2218 | pc = 0.1f; | ||
2219 | |||
2220 | m_physCost = pc; | ||
2221 | } | ||
2222 | else | ||
2223 | m_physCost = 0.1f; | ||
2224 | |||
2225 | m_streamCost = 1.0f; | ||
2226 | } | ||
2227 | |||
2228 | #endregion | ||
2229 | |||
2230 | |||
2231 | /// <summary> | ||
2232 | /// Add a child prim to this parent prim. | ||
2233 | /// </summary> | ||
2234 | /// <param name="prim">Child prim</param> | ||
2235 | // I'm the parent | ||
2236 | // prim is the child | ||
2237 | public void ParentPrim(OdePrim prim) | ||
2238 | { | ||
2239 | //Console.WriteLine("ParentPrim " + m_primName); | ||
2240 | if (this.m_localID != prim.m_localID) | ||
2241 | { | ||
2242 | DestroyBody(); // for now we need to rebuil entire object on link change | ||
2243 | |||
2244 | lock (childrenPrim) | ||
2245 | { | ||
2246 | // adopt the prim | ||
2247 | if (!childrenPrim.Contains(prim)) | ||
2248 | childrenPrim.Add(prim); | ||
2249 | |||
2250 | // see if this prim has kids and adopt them also | ||
2251 | // should not happen for now | ||
2252 | foreach (OdePrim prm in prim.childrenPrim) | ||
2253 | { | ||
2254 | if (!childrenPrim.Contains(prm)) | ||
2255 | { | ||
2256 | if (prm.Body != IntPtr.Zero) | ||
2257 | { | ||
2258 | if (prm.prim_geom != IntPtr.Zero) | ||
2259 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); | ||
2260 | if (prm.Body != prim.Body) | ||
2261 | prm.DestroyBody(); // don't loose bodies around | ||
2262 | prm.Body = IntPtr.Zero; | ||
2263 | } | ||
2264 | |||
2265 | childrenPrim.Add(prm); | ||
2266 | prm._parent = this; | ||
2267 | } | ||
2268 | } | ||
2269 | } | ||
2270 | //Remove old children from the prim | ||
2271 | prim.childrenPrim.Clear(); | ||
2272 | |||
2273 | if (prim.Body != IntPtr.Zero) | ||
2274 | { | ||
2275 | if (prim.prim_geom != IntPtr.Zero) | ||
2276 | d.GeomSetBody(prim.prim_geom, IntPtr.Zero); | ||
2277 | prim.DestroyBody(); // don't loose bodies around | ||
2278 | prim.Body = IntPtr.Zero; | ||
2279 | } | ||
2280 | |||
2281 | prim.childPrim = true; | ||
2282 | prim._parent = this; | ||
2283 | |||
2284 | MakeBody(); // full nasty reconstruction | ||
2285 | } | ||
2286 | } | ||
2287 | |||
2288 | private void UpdateChildsfromgeom() | ||
2289 | { | ||
2290 | if (childrenPrim.Count > 0) | ||
2291 | { | ||
2292 | foreach (OdePrim prm in childrenPrim) | ||
2293 | prm.UpdateDataFromGeom(); | ||
2294 | } | ||
2295 | } | ||
2296 | |||
2297 | private void UpdateDataFromGeom() | ||
2298 | { | ||
2299 | if (prim_geom != IntPtr.Zero) | ||
2300 | { | ||
2301 | d.Quaternion qtmp; | ||
2302 | d.GeomCopyQuaternion(prim_geom, out qtmp); | ||
2303 | _orientation.X = qtmp.X; | ||
2304 | _orientation.Y = qtmp.Y; | ||
2305 | _orientation.Z = qtmp.Z; | ||
2306 | _orientation.W = qtmp.W; | ||
2307 | /* | ||
2308 | // Debug | ||
2309 | float qlen = _orientation.Length(); | ||
2310 | if (qlen > 1.01f || qlen < 0.99) | ||
2311 | m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); | ||
2312 | // | ||
2313 | */ | ||
2314 | _orientation.Normalize(); | ||
2315 | |||
2316 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
2317 | _position.X = lpos.X; | ||
2318 | _position.Y = lpos.Y; | ||
2319 | _position.Z = lpos.Z; | ||
2320 | } | ||
2321 | } | ||
2322 | |||
2323 | private void ChildDelink(OdePrim odePrim, bool remakebodies) | ||
2324 | { | ||
2325 | // Okay, we have a delinked child.. destroy all body and remake | ||
2326 | if (odePrim != this && !childrenPrim.Contains(odePrim)) | ||
2327 | return; | ||
2328 | |||
2329 | DestroyBody(); | ||
2330 | |||
2331 | if (odePrim == this) // delinking the root prim | ||
2332 | { | ||
2333 | OdePrim newroot = null; | ||
2334 | lock (childrenPrim) | ||
2335 | { | ||
2336 | if (childrenPrim.Count > 0) | ||
2337 | { | ||
2338 | newroot = childrenPrim[0]; | ||
2339 | childrenPrim.RemoveAt(0); | ||
2340 | foreach (OdePrim prm in childrenPrim) | ||
2341 | { | ||
2342 | newroot.childrenPrim.Add(prm); | ||
2343 | } | ||
2344 | childrenPrim.Clear(); | ||
2345 | } | ||
2346 | if (newroot != null) | ||
2347 | { | ||
2348 | newroot.childPrim = false; | ||
2349 | newroot._parent = null; | ||
2350 | if (remakebodies) | ||
2351 | newroot.MakeBody(); | ||
2352 | } | ||
2353 | } | ||
2354 | } | ||
2355 | |||
2356 | else | ||
2357 | { | ||
2358 | lock (childrenPrim) | ||
2359 | { | ||
2360 | childrenPrim.Remove(odePrim); | ||
2361 | odePrim.childPrim = false; | ||
2362 | odePrim._parent = null; | ||
2363 | // odePrim.UpdateDataFromGeom(); | ||
2364 | if (remakebodies) | ||
2365 | odePrim.MakeBody(); | ||
2366 | } | ||
2367 | } | ||
2368 | if (remakebodies) | ||
2369 | MakeBody(); | ||
2370 | } | ||
2371 | |||
2372 | protected void ChildRemove(OdePrim odePrim, bool reMakeBody) | ||
2373 | { | ||
2374 | // Okay, we have a delinked child.. destroy all body and remake | ||
2375 | if (odePrim != this && !childrenPrim.Contains(odePrim)) | ||
2376 | return; | ||
2377 | |||
2378 | DestroyBody(); | ||
2379 | |||
2380 | if (odePrim == this) | ||
2381 | { | ||
2382 | OdePrim newroot = null; | ||
2383 | lock (childrenPrim) | ||
2384 | { | ||
2385 | if (childrenPrim.Count > 0) | ||
2386 | { | ||
2387 | newroot = childrenPrim[0]; | ||
2388 | childrenPrim.RemoveAt(0); | ||
2389 | foreach (OdePrim prm in childrenPrim) | ||
2390 | { | ||
2391 | newroot.childrenPrim.Add(prm); | ||
2392 | } | ||
2393 | childrenPrim.Clear(); | ||
2394 | } | ||
2395 | if (newroot != null) | ||
2396 | { | ||
2397 | newroot.childPrim = false; | ||
2398 | newroot._parent = null; | ||
2399 | newroot.MakeBody(); | ||
2400 | } | ||
2401 | } | ||
2402 | if (reMakeBody) | ||
2403 | MakeBody(); | ||
2404 | return; | ||
2405 | } | ||
2406 | else | ||
2407 | { | ||
2408 | lock (childrenPrim) | ||
2409 | { | ||
2410 | childrenPrim.Remove(odePrim); | ||
2411 | odePrim.childPrim = false; | ||
2412 | odePrim._parent = null; | ||
2413 | if (reMakeBody) | ||
2414 | odePrim.MakeBody(); | ||
2415 | } | ||
2416 | } | ||
2417 | MakeBody(); | ||
2418 | } | ||
2419 | |||
2420 | |||
2421 | #region changes | ||
2422 | |||
2423 | private void changeadd() | ||
2424 | { | ||
2425 | } | ||
2426 | |||
2427 | private void changeAngularLock(Vector3 newLock) | ||
2428 | { | ||
2429 | // do we have a Physical object? | ||
2430 | if (Body != IntPtr.Zero) | ||
2431 | { | ||
2432 | //Check that we have a Parent | ||
2433 | //If we have a parent then we're not authorative here | ||
2434 | if (_parent == null) | ||
2435 | { | ||
2436 | if (!newLock.ApproxEquals(Vector3.One, 0f)) | ||
2437 | { | ||
2438 | createAMotor(newLock); | ||
2439 | } | ||
2440 | else | ||
2441 | { | ||
2442 | if (Amotor != IntPtr.Zero) | ||
2443 | { | ||
2444 | d.JointDestroy(Amotor); | ||
2445 | Amotor = IntPtr.Zero; | ||
2446 | } | ||
2447 | } | ||
2448 | } | ||
2449 | } | ||
2450 | // Store this for later in case we get turned into a separate body | ||
2451 | m_angularlock = newLock; | ||
2452 | } | ||
2453 | |||
2454 | private void changeLink(OdePrim NewParent) | ||
2455 | { | ||
2456 | if (_parent == null && NewParent != null) | ||
2457 | { | ||
2458 | NewParent.ParentPrim(this); | ||
2459 | } | ||
2460 | else if (_parent != null) | ||
2461 | { | ||
2462 | if (_parent is OdePrim) | ||
2463 | { | ||
2464 | if (NewParent != _parent) | ||
2465 | { | ||
2466 | (_parent as OdePrim).ChildDelink(this, false); // for now... | ||
2467 | childPrim = false; | ||
2468 | |||
2469 | if (NewParent != null) | ||
2470 | { | ||
2471 | NewParent.ParentPrim(this); | ||
2472 | } | ||
2473 | } | ||
2474 | } | ||
2475 | } | ||
2476 | _parent = NewParent; | ||
2477 | } | ||
2478 | |||
2479 | |||
2480 | private void Stop() | ||
2481 | { | ||
2482 | if (!childPrim) | ||
2483 | { | ||
2484 | // m_force = Vector3.Zero; | ||
2485 | m_forceacc = Vector3.Zero; | ||
2486 | m_angularForceacc = Vector3.Zero; | ||
2487 | // m_torque = Vector3.Zero; | ||
2488 | _velocity = Vector3.Zero; | ||
2489 | _acceleration = Vector3.Zero; | ||
2490 | m_rotationalVelocity = Vector3.Zero; | ||
2491 | _target_velocity = Vector3.Zero; | ||
2492 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
2493 | m_vehicle.Stop(); | ||
2494 | |||
2495 | _zeroFlag = false; | ||
2496 | base.RequestPhysicsterseUpdate(); | ||
2497 | } | ||
2498 | |||
2499 | if (Body != IntPtr.Zero) | ||
2500 | { | ||
2501 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
2502 | d.BodySetTorque(Body, 0f, 0f, 0f); | ||
2503 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
2504 | d.BodySetAngularVel(Body, 0f, 0f, 0f); | ||
2505 | } | ||
2506 | } | ||
2507 | |||
2508 | private void changePhantomStatus(bool newval) | ||
2509 | { | ||
2510 | m_isphantom = newval; | ||
2511 | |||
2512 | UpdateCollisionCatFlags(); | ||
2513 | ApplyCollisionCatFlags(); | ||
2514 | } | ||
2515 | |||
2516 | /* not in use | ||
2517 | internal void ChildSelectedChange(bool childSelect) | ||
2518 | { | ||
2519 | if(childPrim) | ||
2520 | return; | ||
2521 | |||
2522 | if (childSelect == m_isSelected) | ||
2523 | return; | ||
2524 | |||
2525 | if (childSelect) | ||
2526 | { | ||
2527 | DoSelectedStatus(true); | ||
2528 | } | ||
2529 | |||
2530 | else | ||
2531 | { | ||
2532 | foreach (OdePrim prm in childrenPrim) | ||
2533 | { | ||
2534 | if (prm.m_isSelected) | ||
2535 | return; | ||
2536 | } | ||
2537 | DoSelectedStatus(false); | ||
2538 | } | ||
2539 | } | ||
2540 | */ | ||
2541 | private void changeSelectedStatus(bool newval) | ||
2542 | { | ||
2543 | if (m_lastdoneSelected == newval) | ||
2544 | return; | ||
2545 | |||
2546 | m_lastdoneSelected = newval; | ||
2547 | DoSelectedStatus(newval); | ||
2548 | } | ||
2549 | |||
2550 | private void CheckDelaySelect() | ||
2551 | { | ||
2552 | if (m_delaySelect) | ||
2553 | { | ||
2554 | DoSelectedStatus(m_isSelected); | ||
2555 | } | ||
2556 | } | ||
2557 | |||
2558 | private void DoSelectedStatus(bool newval) | ||
2559 | { | ||
2560 | m_isSelected = newval; | ||
2561 | Stop(); | ||
2562 | |||
2563 | if (newval) | ||
2564 | { | ||
2565 | if (!childPrim && Body != IntPtr.Zero) | ||
2566 | d.BodyDisable(Body); | ||
2567 | |||
2568 | if (m_delaySelect || m_isphysical) | ||
2569 | { | ||
2570 | m_collisionCategories = CollisionCategories.Selected; | ||
2571 | m_collisionFlags = 0; | ||
2572 | |||
2573 | if (!childPrim) | ||
2574 | { | ||
2575 | foreach (OdePrim prm in childrenPrim) | ||
2576 | { | ||
2577 | prm.m_collisionCategories = m_collisionCategories; | ||
2578 | prm.m_collisionFlags = m_collisionFlags; | ||
2579 | |||
2580 | if (prm.prim_geom != IntPtr.Zero) | ||
2581 | { | ||
2582 | |||
2583 | if (prm.m_NoColide) | ||
2584 | { | ||
2585 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
2586 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
2587 | } | ||
2588 | else | ||
2589 | { | ||
2590 | d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); | ||
2591 | d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); | ||
2592 | } | ||
2593 | } | ||
2594 | prm.m_delaySelect = false; | ||
2595 | } | ||
2596 | } | ||
2597 | // else if (_parent != null) | ||
2598 | // ((OdePrim)_parent).ChildSelectedChange(true); | ||
2599 | |||
2600 | |||
2601 | if (prim_geom != IntPtr.Zero) | ||
2602 | { | ||
2603 | if (m_NoColide) | ||
2604 | { | ||
2605 | d.GeomSetCategoryBits(prim_geom, 0); | ||
2606 | d.GeomSetCollideBits(prim_geom, 0); | ||
2607 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
2608 | { | ||
2609 | d.GeomSetCategoryBits(collide_geom, 0); | ||
2610 | d.GeomSetCollideBits(collide_geom, 0); | ||
2611 | } | ||
2612 | |||
2613 | } | ||
2614 | else | ||
2615 | { | ||
2616 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
2617 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
2618 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
2619 | { | ||
2620 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); | ||
2621 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
2622 | } | ||
2623 | } | ||
2624 | } | ||
2625 | |||
2626 | m_delaySelect = false; | ||
2627 | } | ||
2628 | else if(!m_isphysical) | ||
2629 | { | ||
2630 | m_delaySelect = true; | ||
2631 | } | ||
2632 | } | ||
2633 | else | ||
2634 | { | ||
2635 | if (!childPrim) | ||
2636 | { | ||
2637 | if (Body != IntPtr.Zero && !m_disabled) | ||
2638 | d.BodyEnable(Body); | ||
2639 | } | ||
2640 | // else if (_parent != null) | ||
2641 | // ((OdePrim)_parent).ChildSelectedChange(false); | ||
2642 | |||
2643 | UpdateCollisionCatFlags(); | ||
2644 | ApplyCollisionCatFlags(); | ||
2645 | |||
2646 | m_delaySelect = false; | ||
2647 | } | ||
2648 | |||
2649 | resetCollisionAccounting(); | ||
2650 | } | ||
2651 | |||
2652 | private void changePosition(Vector3 newPos) | ||
2653 | { | ||
2654 | CheckDelaySelect(); | ||
2655 | if (m_isphysical) | ||
2656 | { | ||
2657 | if (childPrim) // inertia is messed, must rebuild | ||
2658 | { | ||
2659 | if (m_building) | ||
2660 | { | ||
2661 | _position = newPos; | ||
2662 | } | ||
2663 | |||
2664 | else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) | ||
2665 | { | ||
2666 | FixInertia(newPos); | ||
2667 | if (!d.BodyIsEnabled(Body)) | ||
2668 | d.BodyEnable(Body); | ||
2669 | } | ||
2670 | } | ||
2671 | else | ||
2672 | { | ||
2673 | if (_position != newPos) | ||
2674 | { | ||
2675 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2676 | _position = newPos; | ||
2677 | } | ||
2678 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2679 | d.BodyEnable(Body); | ||
2680 | } | ||
2681 | } | ||
2682 | else | ||
2683 | { | ||
2684 | if (prim_geom != IntPtr.Zero) | ||
2685 | { | ||
2686 | if (newPos != _position) | ||
2687 | { | ||
2688 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2689 | _position = newPos; | ||
2690 | |||
2691 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); | ||
2692 | } | ||
2693 | } | ||
2694 | } | ||
2695 | givefakepos--; | ||
2696 | if (givefakepos < 0) | ||
2697 | givefakepos = 0; | ||
2698 | // changeSelectedStatus(); | ||
2699 | resetCollisionAccounting(); | ||
2700 | } | ||
2701 | |||
2702 | private void changeOrientation(Quaternion newOri) | ||
2703 | { | ||
2704 | CheckDelaySelect(); | ||
2705 | if (m_isphysical) | ||
2706 | { | ||
2707 | if (childPrim) // inertia is messed, must rebuild | ||
2708 | { | ||
2709 | if (m_building) | ||
2710 | { | ||
2711 | _orientation = newOri; | ||
2712 | } | ||
2713 | /* | ||
2714 | else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) | ||
2715 | { | ||
2716 | FixInertia(_position, newOri); | ||
2717 | if (!d.BodyIsEnabled(Body)) | ||
2718 | d.BodyEnable(Body); | ||
2719 | } | ||
2720 | */ | ||
2721 | } | ||
2722 | else | ||
2723 | { | ||
2724 | if (newOri != _orientation) | ||
2725 | { | ||
2726 | d.Quaternion myrot = new d.Quaternion(); | ||
2727 | myrot.X = newOri.X; | ||
2728 | myrot.Y = newOri.Y; | ||
2729 | myrot.Z = newOri.Z; | ||
2730 | myrot.W = newOri.W; | ||
2731 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2732 | _orientation = newOri; | ||
2733 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2734 | createAMotor(m_angularlock); | ||
2735 | } | ||
2736 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2737 | d.BodyEnable(Body); | ||
2738 | } | ||
2739 | } | ||
2740 | else | ||
2741 | { | ||
2742 | if (prim_geom != IntPtr.Zero) | ||
2743 | { | ||
2744 | if (newOri != _orientation) | ||
2745 | { | ||
2746 | d.Quaternion myrot = new d.Quaternion(); | ||
2747 | myrot.X = newOri.X; | ||
2748 | myrot.Y = newOri.Y; | ||
2749 | myrot.Z = newOri.Z; | ||
2750 | myrot.W = newOri.W; | ||
2751 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2752 | _orientation = newOri; | ||
2753 | } | ||
2754 | } | ||
2755 | } | ||
2756 | givefakeori--; | ||
2757 | if (givefakeori < 0) | ||
2758 | givefakeori = 0; | ||
2759 | resetCollisionAccounting(); | ||
2760 | } | ||
2761 | |||
2762 | private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) | ||
2763 | { | ||
2764 | CheckDelaySelect(); | ||
2765 | if (m_isphysical) | ||
2766 | { | ||
2767 | if (childPrim && m_building) // inertia is messed, must rebuild | ||
2768 | { | ||
2769 | _position = newPos; | ||
2770 | _orientation = newOri; | ||
2771 | } | ||
2772 | else | ||
2773 | { | ||
2774 | if (newOri != _orientation) | ||
2775 | { | ||
2776 | d.Quaternion myrot = new d.Quaternion(); | ||
2777 | myrot.X = newOri.X; | ||
2778 | myrot.Y = newOri.Y; | ||
2779 | myrot.Z = newOri.Z; | ||
2780 | myrot.W = newOri.W; | ||
2781 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2782 | _orientation = newOri; | ||
2783 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2784 | createAMotor(m_angularlock); | ||
2785 | } | ||
2786 | if (_position != newPos) | ||
2787 | { | ||
2788 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2789 | _position = newPos; | ||
2790 | } | ||
2791 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2792 | d.BodyEnable(Body); | ||
2793 | } | ||
2794 | } | ||
2795 | else | ||
2796 | { | ||
2797 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
2798 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
2799 | |||
2800 | if (prim_geom != IntPtr.Zero) | ||
2801 | { | ||
2802 | if (newOri != _orientation) | ||
2803 | { | ||
2804 | d.Quaternion myrot = new d.Quaternion(); | ||
2805 | myrot.X = newOri.X; | ||
2806 | myrot.Y = newOri.Y; | ||
2807 | myrot.Z = newOri.Z; | ||
2808 | myrot.W = newOri.W; | ||
2809 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2810 | _orientation = newOri; | ||
2811 | } | ||
2812 | |||
2813 | if (newPos != _position) | ||
2814 | { | ||
2815 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2816 | _position = newPos; | ||
2817 | |||
2818 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); | ||
2819 | } | ||
2820 | } | ||
2821 | } | ||
2822 | givefakepos--; | ||
2823 | if (givefakepos < 0) | ||
2824 | givefakepos = 0; | ||
2825 | givefakeori--; | ||
2826 | if (givefakeori < 0) | ||
2827 | givefakeori = 0; | ||
2828 | resetCollisionAccounting(); | ||
2829 | } | ||
2830 | |||
2831 | private void changeDisable(bool disable) | ||
2832 | { | ||
2833 | if (disable) | ||
2834 | { | ||
2835 | if (!m_disabled) | ||
2836 | disableBodySoft(); | ||
2837 | } | ||
2838 | else | ||
2839 | { | ||
2840 | if (m_disabled) | ||
2841 | enableBodySoft(); | ||
2842 | } | ||
2843 | } | ||
2844 | |||
2845 | private void changePhysicsStatus(bool NewStatus) | ||
2846 | { | ||
2847 | CheckDelaySelect(); | ||
2848 | |||
2849 | m_isphysical = NewStatus; | ||
2850 | |||
2851 | if (!childPrim) | ||
2852 | { | ||
2853 | if (NewStatus) | ||
2854 | { | ||
2855 | if (Body == IntPtr.Zero) | ||
2856 | MakeBody(); | ||
2857 | } | ||
2858 | else | ||
2859 | { | ||
2860 | if (Body != IntPtr.Zero) | ||
2861 | { | ||
2862 | DestroyBody(); | ||
2863 | } | ||
2864 | Stop(); | ||
2865 | } | ||
2866 | } | ||
2867 | |||
2868 | resetCollisionAccounting(); | ||
2869 | } | ||
2870 | |||
2871 | private void changeSize(Vector3 newSize) | ||
2872 | { | ||
2873 | } | ||
2874 | |||
2875 | private void changeShape(PrimitiveBaseShape newShape) | ||
2876 | { | ||
2877 | } | ||
2878 | |||
2879 | private void changeAddPhysRep(ODEPhysRepData repData) | ||
2880 | { | ||
2881 | _size = repData.size; //?? | ||
2882 | _pbs = repData.pbs; | ||
2883 | m_shapetype = repData.shapetype; | ||
2884 | |||
2885 | m_mesh = repData.mesh; | ||
2886 | |||
2887 | m_assetID = repData.assetID; | ||
2888 | m_meshState = repData.meshState; | ||
2889 | |||
2890 | m_hasOBB = repData.hasOBB; | ||
2891 | m_OBBOffset = repData.OBBOffset; | ||
2892 | m_OBB = repData.OBB; | ||
2893 | |||
2894 | primVolume = repData.volume; | ||
2895 | |||
2896 | CreateGeom(); | ||
2897 | |||
2898 | if (prim_geom != IntPtr.Zero) | ||
2899 | { | ||
2900 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2901 | d.Quaternion myrot = new d.Quaternion(); | ||
2902 | myrot.X = _orientation.X; | ||
2903 | myrot.Y = _orientation.Y; | ||
2904 | myrot.Z = _orientation.Z; | ||
2905 | myrot.W = _orientation.W; | ||
2906 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2907 | } | ||
2908 | |||
2909 | if (!m_isphysical) | ||
2910 | { | ||
2911 | SetInStaticSpace(this); | ||
2912 | UpdateCollisionCatFlags(); | ||
2913 | ApplyCollisionCatFlags(); | ||
2914 | } | ||
2915 | else | ||
2916 | MakeBody(); | ||
2917 | |||
2918 | if ((m_meshState & MeshState.NeedMask) != 0) | ||
2919 | { | ||
2920 | repData.size = _size; | ||
2921 | repData.pbs = _pbs; | ||
2922 | repData.shapetype = m_shapetype; | ||
2923 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
2924 | } | ||
2925 | } | ||
2926 | |||
2927 | private void changePhysRepData(ODEPhysRepData repData) | ||
2928 | { | ||
2929 | CheckDelaySelect(); | ||
2930 | |||
2931 | OdePrim parent = (OdePrim)_parent; | ||
2932 | |||
2933 | bool chp = childPrim; | ||
2934 | |||
2935 | if (chp) | ||
2936 | { | ||
2937 | if (parent != null) | ||
2938 | { | ||
2939 | parent.DestroyBody(); | ||
2940 | } | ||
2941 | } | ||
2942 | else | ||
2943 | { | ||
2944 | DestroyBody(); | ||
2945 | } | ||
2946 | |||
2947 | RemoveGeom(); | ||
2948 | |||
2949 | _size = repData.size; | ||
2950 | _pbs = repData.pbs; | ||
2951 | m_shapetype = repData.shapetype; | ||
2952 | |||
2953 | m_mesh = repData.mesh; | ||
2954 | |||
2955 | m_assetID = repData.assetID; | ||
2956 | m_meshState = repData.meshState; | ||
2957 | |||
2958 | m_hasOBB = repData.hasOBB; | ||
2959 | m_OBBOffset = repData.OBBOffset; | ||
2960 | m_OBB = repData.OBB; | ||
2961 | |||
2962 | primVolume = repData.volume; | ||
2963 | |||
2964 | CreateGeom(); | ||
2965 | |||
2966 | if (prim_geom != IntPtr.Zero) | ||
2967 | { | ||
2968 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2969 | d.Quaternion myrot = new d.Quaternion(); | ||
2970 | myrot.X = _orientation.X; | ||
2971 | myrot.Y = _orientation.Y; | ||
2972 | myrot.Z = _orientation.Z; | ||
2973 | myrot.W = _orientation.W; | ||
2974 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2975 | } | ||
2976 | |||
2977 | if (m_isphysical) | ||
2978 | { | ||
2979 | if (chp) | ||
2980 | { | ||
2981 | if (parent != null) | ||
2982 | { | ||
2983 | parent.MakeBody(); | ||
2984 | } | ||
2985 | } | ||
2986 | else | ||
2987 | MakeBody(); | ||
2988 | } | ||
2989 | else | ||
2990 | { | ||
2991 | SetInStaticSpace(this); | ||
2992 | UpdateCollisionCatFlags(); | ||
2993 | ApplyCollisionCatFlags(); | ||
2994 | } | ||
2995 | |||
2996 | resetCollisionAccounting(); | ||
2997 | |||
2998 | if ((m_meshState & MeshState.NeedMask) != 0) | ||
2999 | { | ||
3000 | repData.size = _size; | ||
3001 | repData.pbs = _pbs; | ||
3002 | repData.shapetype = m_shapetype; | ||
3003 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
3004 | } | ||
3005 | } | ||
3006 | |||
3007 | private void changeFloatOnWater(bool newval) | ||
3008 | { | ||
3009 | m_collidesWater = newval; | ||
3010 | |||
3011 | UpdateCollisionCatFlags(); | ||
3012 | ApplyCollisionCatFlags(); | ||
3013 | } | ||
3014 | |||
3015 | private void changeSetTorque(Vector3 newtorque) | ||
3016 | { | ||
3017 | if (!m_isSelected) | ||
3018 | { | ||
3019 | if (m_isphysical && Body != IntPtr.Zero) | ||
3020 | { | ||
3021 | if (m_disabled) | ||
3022 | enableBodySoft(); | ||
3023 | else if (!d.BodyIsEnabled(Body)) | ||
3024 | d.BodyEnable(Body); | ||
3025 | |||
3026 | } | ||
3027 | m_torque = newtorque; | ||
3028 | } | ||
3029 | } | ||
3030 | |||
3031 | private void changeForce(Vector3 force) | ||
3032 | { | ||
3033 | m_force = force; | ||
3034 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
3035 | d.BodyEnable(Body); | ||
3036 | } | ||
3037 | |||
3038 | private void changeAddForce(Vector3 theforce) | ||
3039 | { | ||
3040 | m_forceacc += theforce; | ||
3041 | if (!m_isSelected) | ||
3042 | { | ||
3043 | lock (this) | ||
3044 | { | ||
3045 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
3046 | if (m_isphysical && Body != IntPtr.Zero) | ||
3047 | { | ||
3048 | if (m_disabled) | ||
3049 | enableBodySoft(); | ||
3050 | else if (!d.BodyIsEnabled(Body)) | ||
3051 | d.BodyEnable(Body); | ||
3052 | } | ||
3053 | } | ||
3054 | m_collisionscore = 0; | ||
3055 | } | ||
3056 | } | ||
3057 | |||
3058 | // actually angular impulse | ||
3059 | private void changeAddAngularImpulse(Vector3 aimpulse) | ||
3060 | { | ||
3061 | m_angularForceacc += aimpulse * m_invTimeStep; | ||
3062 | if (!m_isSelected) | ||
3063 | { | ||
3064 | lock (this) | ||
3065 | { | ||
3066 | if (m_isphysical && Body != IntPtr.Zero) | ||
3067 | { | ||
3068 | if (m_disabled) | ||
3069 | enableBodySoft(); | ||
3070 | else if (!d.BodyIsEnabled(Body)) | ||
3071 | d.BodyEnable(Body); | ||
3072 | } | ||
3073 | } | ||
3074 | m_collisionscore = 0; | ||
3075 | } | ||
3076 | } | ||
3077 | |||
3078 | private void changevelocity(Vector3 newVel) | ||
3079 | { | ||
3080 | float len = newVel.LengthSquared(); | ||
3081 | if (len > 100000.0f) // limit to 100m/s | ||
3082 | { | ||
3083 | len = 100.0f / (float)Math.Sqrt(len); | ||
3084 | newVel *= len; | ||
3085 | } | ||
3086 | |||
3087 | if (!m_isSelected) | ||
3088 | { | ||
3089 | if (Body != IntPtr.Zero) | ||
3090 | { | ||
3091 | if (m_disabled) | ||
3092 | enableBodySoft(); | ||
3093 | else if (!d.BodyIsEnabled(Body)) | ||
3094 | d.BodyEnable(Body); | ||
3095 | |||
3096 | d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); | ||
3097 | } | ||
3098 | //resetCollisionAccounting(); | ||
3099 | } | ||
3100 | _velocity = newVel; | ||
3101 | } | ||
3102 | |||
3103 | private void changeangvelocity(Vector3 newAngVel) | ||
3104 | { | ||
3105 | float len = newAngVel.LengthSquared(); | ||
3106 | if (len > 144.0f) // limit to 12rad/s | ||
3107 | { | ||
3108 | len = 12.0f / (float)Math.Sqrt(len); | ||
3109 | newAngVel *= len; | ||
3110 | } | ||
3111 | |||
3112 | if (!m_isSelected) | ||
3113 | { | ||
3114 | if (Body != IntPtr.Zero) | ||
3115 | { | ||
3116 | if (m_disabled) | ||
3117 | enableBodySoft(); | ||
3118 | else if (!d.BodyIsEnabled(Body)) | ||
3119 | d.BodyEnable(Body); | ||
3120 | |||
3121 | |||
3122 | d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); | ||
3123 | } | ||
3124 | //resetCollisionAccounting(); | ||
3125 | } | ||
3126 | m_rotationalVelocity = newAngVel; | ||
3127 | } | ||
3128 | |||
3129 | private void changeVolumedetetion(bool newVolDtc) | ||
3130 | { | ||
3131 | m_isVolumeDetect = newVolDtc; | ||
3132 | m_fakeisVolumeDetect = newVolDtc; | ||
3133 | UpdateCollisionCatFlags(); | ||
3134 | ApplyCollisionCatFlags(); | ||
3135 | } | ||
3136 | |||
3137 | protected void changeBuilding(bool newbuilding) | ||
3138 | { | ||
3139 | // Check if we need to do anything | ||
3140 | if (newbuilding == m_building) | ||
3141 | return; | ||
3142 | |||
3143 | if ((bool)newbuilding) | ||
3144 | { | ||
3145 | m_building = true; | ||
3146 | if (!childPrim) | ||
3147 | DestroyBody(); | ||
3148 | } | ||
3149 | else | ||
3150 | { | ||
3151 | m_building = false; | ||
3152 | CheckDelaySelect(); | ||
3153 | if (!childPrim) | ||
3154 | MakeBody(); | ||
3155 | } | ||
3156 | if (!childPrim && childrenPrim.Count > 0) | ||
3157 | { | ||
3158 | foreach (OdePrim prm in childrenPrim) | ||
3159 | prm.changeBuilding(m_building); // call directly | ||
3160 | } | ||
3161 | } | ||
3162 | |||
3163 | public void changeSetVehicle(VehicleData vdata) | ||
3164 | { | ||
3165 | if (m_vehicle == null) | ||
3166 | m_vehicle = new ODEDynamics(this); | ||
3167 | m_vehicle.DoSetVehicle(vdata); | ||
3168 | } | ||
3169 | |||
3170 | private void changeVehicleType(int value) | ||
3171 | { | ||
3172 | if (value == (int)Vehicle.TYPE_NONE) | ||
3173 | { | ||
3174 | if (m_vehicle != null) | ||
3175 | m_vehicle = null; | ||
3176 | } | ||
3177 | else | ||
3178 | { | ||
3179 | if (m_vehicle == null) | ||
3180 | m_vehicle = new ODEDynamics(this); | ||
3181 | |||
3182 | m_vehicle.ProcessTypeChange((Vehicle)value); | ||
3183 | } | ||
3184 | } | ||
3185 | |||
3186 | private void changeVehicleFloatParam(strVehicleFloatParam fp) | ||
3187 | { | ||
3188 | if (m_vehicle == null) | ||
3189 | return; | ||
3190 | |||
3191 | m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); | ||
3192 | } | ||
3193 | |||
3194 | private void changeVehicleVectorParam(strVehicleVectorParam vp) | ||
3195 | { | ||
3196 | if (m_vehicle == null) | ||
3197 | return; | ||
3198 | m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); | ||
3199 | } | ||
3200 | |||
3201 | private void changeVehicleRotationParam(strVehicleQuatParam qp) | ||
3202 | { | ||
3203 | if (m_vehicle == null) | ||
3204 | return; | ||
3205 | m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); | ||
3206 | } | ||
3207 | |||
3208 | private void changeVehicleFlags(strVehicleBoolParam bp) | ||
3209 | { | ||
3210 | if (m_vehicle == null) | ||
3211 | return; | ||
3212 | m_vehicle.ProcessVehicleFlags(bp.param, bp.value); | ||
3213 | } | ||
3214 | |||
3215 | private void changeBuoyancy(float b) | ||
3216 | { | ||
3217 | m_buoyancy = b; | ||
3218 | } | ||
3219 | |||
3220 | private void changePIDTarget(Vector3 trg) | ||
3221 | { | ||
3222 | m_PIDTarget = trg; | ||
3223 | } | ||
3224 | |||
3225 | private void changePIDTau(float tau) | ||
3226 | { | ||
3227 | m_PIDTau = tau; | ||
3228 | } | ||
3229 | |||
3230 | private void changePIDActive(bool val) | ||
3231 | { | ||
3232 | m_usePID = val; | ||
3233 | } | ||
3234 | |||
3235 | private void changePIDHoverHeight(float val) | ||
3236 | { | ||
3237 | m_PIDHoverHeight = val; | ||
3238 | if (val == 0) | ||
3239 | m_useHoverPID = false; | ||
3240 | } | ||
3241 | |||
3242 | private void changePIDHoverType(PIDHoverType type) | ||
3243 | { | ||
3244 | m_PIDHoverType = type; | ||
3245 | } | ||
3246 | |||
3247 | private void changePIDHoverTau(float tau) | ||
3248 | { | ||
3249 | m_PIDHoverTau = tau; | ||
3250 | } | ||
3251 | |||
3252 | private void changePIDHoverActive(bool active) | ||
3253 | { | ||
3254 | m_useHoverPID = active; | ||
3255 | } | ||
3256 | |||
3257 | #endregion | ||
3258 | |||
3259 | public void Move() | ||
3260 | { | ||
3261 | if (!childPrim && m_isphysical && Body != IntPtr.Zero && | ||
3262 | !m_disabled && !m_isSelected && !m_building && !m_outbounds) | ||
3263 | { | ||
3264 | if (!d.BodyIsEnabled(Body)) | ||
3265 | { | ||
3266 | // let vehicles sleep | ||
3267 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
3268 | return; | ||
3269 | |||
3270 | if (++bodydisablecontrol < 20) | ||
3271 | return; | ||
3272 | |||
3273 | d.BodyEnable(Body); | ||
3274 | } | ||
3275 | |||
3276 | bodydisablecontrol = 0; | ||
3277 | |||
3278 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator | ||
3279 | |||
3280 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
3281 | { | ||
3282 | // 'VEHICLES' are dealt with in ODEDynamics.cs | ||
3283 | m_vehicle.Step(); | ||
3284 | return; | ||
3285 | } | ||
3286 | |||
3287 | float fx = 0; | ||
3288 | float fy = 0; | ||
3289 | float fz = 0; | ||
3290 | |||
3291 | float m_mass = _mass; | ||
3292 | |||
3293 | if (m_usePID && m_PIDTau > 0) | ||
3294 | { | ||
3295 | // for now position error | ||
3296 | _target_velocity = | ||
3297 | new Vector3( | ||
3298 | (m_PIDTarget.X - lpos.X), | ||
3299 | (m_PIDTarget.Y - lpos.Y), | ||
3300 | (m_PIDTarget.Z - lpos.Z) | ||
3301 | ); | ||
3302 | |||
3303 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) | ||
3304 | { | ||
3305 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
3306 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
3307 | return; | ||
3308 | } | ||
3309 | else | ||
3310 | { | ||
3311 | _zeroFlag = false; | ||
3312 | |||
3313 | float tmp = 1 / m_PIDTau; | ||
3314 | _target_velocity *= tmp; | ||
3315 | |||
3316 | // apply limits | ||
3317 | tmp = _target_velocity.Length(); | ||
3318 | if (tmp > 50.0f) | ||
3319 | { | ||
3320 | tmp = 50 / tmp; | ||
3321 | _target_velocity *= tmp; | ||
3322 | } | ||
3323 | else if (tmp < 0.05f) | ||
3324 | { | ||
3325 | tmp = 0.05f / tmp; | ||
3326 | _target_velocity *= tmp; | ||
3327 | } | ||
3328 | |||
3329 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3330 | fx = (_target_velocity.X - vel.X) * m_invTimeStep; | ||
3331 | fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; | ||
3332 | fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; | ||
3333 | // d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3334 | } | ||
3335 | } // end if (m_usePID) | ||
3336 | |||
3337 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | ||
3338 | else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) | ||
3339 | { | ||
3340 | |||
3341 | // Non-Vehicles have a limited set of Hover options. | ||
3342 | // determine what our target height really is based on HoverType | ||
3343 | |||
3344 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); | ||
3345 | |||
3346 | switch (m_PIDHoverType) | ||
3347 | { | ||
3348 | case PIDHoverType.Ground: | ||
3349 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3350 | break; | ||
3351 | |||
3352 | case PIDHoverType.GroundAndWater: | ||
3353 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
3354 | if (m_groundHeight > m_waterHeight) | ||
3355 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3356 | else | ||
3357 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
3358 | break; | ||
3359 | } // end switch (m_PIDHoverType) | ||
3360 | |||
3361 | // don't go underground unless volumedetector | ||
3362 | |||
3363 | if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) | ||
3364 | { | ||
3365 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3366 | |||
3367 | fz = (m_targetHoverHeight - lpos.Z); | ||
3368 | |||
3369 | // if error is zero, use position control; otherwise, velocity control | ||
3370 | if (Math.Abs(fz) < 0.01f) | ||
3371 | { | ||
3372 | d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); | ||
3373 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | ||
3374 | } | ||
3375 | else | ||
3376 | { | ||
3377 | _zeroFlag = false; | ||
3378 | fz /= m_PIDHoverTau; | ||
3379 | |||
3380 | float tmp = Math.Abs(fz); | ||
3381 | if (tmp > 50) | ||
3382 | fz = 50 * Math.Sign(fz); | ||
3383 | else if (tmp < 0.1) | ||
3384 | fz = 0.1f * Math.Sign(fz); | ||
3385 | |||
3386 | fz = ((fz - vel.Z) * m_invTimeStep); | ||
3387 | } | ||
3388 | } | ||
3389 | } | ||
3390 | else | ||
3391 | { | ||
3392 | float b = (1.0f - m_buoyancy) * m_gravmod; | ||
3393 | fx = _parent_scene.gravityx * b; | ||
3394 | fy = _parent_scene.gravityy * b; | ||
3395 | fz = _parent_scene.gravityz * b; | ||
3396 | } | ||
3397 | |||
3398 | fx *= m_mass; | ||
3399 | fy *= m_mass; | ||
3400 | fz *= m_mass; | ||
3401 | |||
3402 | // constant force | ||
3403 | fx += m_force.X; | ||
3404 | fy += m_force.Y; | ||
3405 | fz += m_force.Z; | ||
3406 | |||
3407 | fx += m_forceacc.X; | ||
3408 | fy += m_forceacc.Y; | ||
3409 | fz += m_forceacc.Z; | ||
3410 | |||
3411 | m_forceacc = Vector3.Zero; | ||
3412 | |||
3413 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
3414 | if (fx != 0 || fy != 0 || fz != 0) | ||
3415 | { | ||
3416 | d.BodyAddForce(Body, fx, fy, fz); | ||
3417 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | ||
3418 | } | ||
3419 | |||
3420 | Vector3 trq; | ||
3421 | |||
3422 | trq = m_torque; | ||
3423 | trq += m_angularForceacc; | ||
3424 | m_angularForceacc = Vector3.Zero; | ||
3425 | if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) | ||
3426 | { | ||
3427 | d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); | ||
3428 | } | ||
3429 | } | ||
3430 | else | ||
3431 | { // is not physical, or is not a body or is selected | ||
3432 | // _zeroPosition = d.BodyGetPosition(Body); | ||
3433 | return; | ||
3434 | //Console.WriteLine("Nothing " + Name); | ||
3435 | |||
3436 | } | ||
3437 | } | ||
3438 | |||
3439 | public void UpdatePositionAndVelocity(int frame) | ||
3440 | { | ||
3441 | if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) | ||
3442 | { | ||
3443 | bool bodyenabled = d.BodyIsEnabled(Body); | ||
3444 | if (bodyenabled || !_zeroFlag) | ||
3445 | { | ||
3446 | bool lastZeroFlag = _zeroFlag; | ||
3447 | |||
3448 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
3449 | |||
3450 | // check outside region | ||
3451 | if (lpos.Z < -100 || lpos.Z > 100000f) | ||
3452 | { | ||
3453 | m_outbounds = true; | ||
3454 | |||
3455 | lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); | ||
3456 | _acceleration.X = 0; | ||
3457 | _acceleration.Y = 0; | ||
3458 | _acceleration.Z = 0; | ||
3459 | |||
3460 | _velocity.X = 0; | ||
3461 | _velocity.Y = 0; | ||
3462 | _velocity.Z = 0; | ||
3463 | m_rotationalVelocity.X = 0; | ||
3464 | m_rotationalVelocity.Y = 0; | ||
3465 | m_rotationalVelocity.Z = 0; | ||
3466 | |||
3467 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
3468 | d.BodySetAngularVel(Body, 0, 0, 0); // stop it | ||
3469 | d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere | ||
3470 | m_lastposition = _position; | ||
3471 | m_lastorientation = _orientation; | ||
3472 | |||
3473 | base.RequestPhysicsterseUpdate(); | ||
3474 | |||
3475 | // throttleCounter = 0; | ||
3476 | _zeroFlag = true; | ||
3477 | |||
3478 | disableBodySoft(); // disable it and colisions | ||
3479 | base.RaiseOutOfBounds(_position); | ||
3480 | return; | ||
3481 | } | ||
3482 | |||
3483 | if (lpos.X < 0f) | ||
3484 | { | ||
3485 | _position.X = Util.Clip(lpos.X, -2f, -0.1f); | ||
3486 | m_outbounds = true; | ||
3487 | } | ||
3488 | else if (lpos.X > _parent_scene.WorldExtents.X) | ||
3489 | { | ||
3490 | _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); | ||
3491 | m_outbounds = true; | ||
3492 | } | ||
3493 | if (lpos.Y < 0f) | ||
3494 | { | ||
3495 | _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); | ||
3496 | m_outbounds = true; | ||
3497 | } | ||
3498 | else if (lpos.Y > _parent_scene.WorldExtents.Y) | ||
3499 | { | ||
3500 | _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); | ||
3501 | m_outbounds = true; | ||
3502 | } | ||
3503 | |||
3504 | if (m_outbounds) | ||
3505 | { | ||
3506 | m_lastposition = _position; | ||
3507 | m_lastorientation = _orientation; | ||
3508 | |||
3509 | d.Vector3 dtmp = d.BodyGetAngularVel(Body); | ||
3510 | m_rotationalVelocity.X = dtmp.X; | ||
3511 | m_rotationalVelocity.Y = dtmp.Y; | ||
3512 | m_rotationalVelocity.Z = dtmp.Z; | ||
3513 | |||
3514 | dtmp = d.BodyGetLinearVel(Body); | ||
3515 | _velocity.X = dtmp.X; | ||
3516 | _velocity.Y = dtmp.Y; | ||
3517 | _velocity.Z = dtmp.Z; | ||
3518 | |||
3519 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
3520 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
3521 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
3522 | disableBodySoft(); // stop collisions | ||
3523 | UnSubscribeEvents(); | ||
3524 | |||
3525 | base.RequestPhysicsterseUpdate(); | ||
3526 | return; | ||
3527 | } | ||
3528 | |||
3529 | d.Quaternion ori; | ||
3530 | d.GeomCopyQuaternion(prim_geom, out ori); | ||
3531 | |||
3532 | // decide if moving | ||
3533 | // use positions since this are integrated quantities | ||
3534 | // tolerance values depende a lot on simulation noise... | ||
3535 | // use simple math.abs since we dont need to be exact | ||
3536 | |||
3537 | if (!bodyenabled || | ||
3538 | (Math.Abs(_position.X - lpos.X) < 0.005f) | ||
3539 | && (Math.Abs(_position.Y - lpos.Y) < 0.005f) | ||
3540 | && (Math.Abs(_position.Z - lpos.Z) < 0.005f) | ||
3541 | && (Math.Abs(_orientation.X - ori.X) < 0.0005f) | ||
3542 | && (Math.Abs(_orientation.Y - ori.Y) < 0.0005f) | ||
3543 | && (Math.Abs(_orientation.Z - ori.Z) < 0.0005f) // ignore W | ||
3544 | ) | ||
3545 | { | ||
3546 | _zeroFlag = true; | ||
3547 | } | ||
3548 | else | ||
3549 | _zeroFlag = false; | ||
3550 | |||
3551 | // update velocities and aceleration | ||
3552 | if (!(_zeroFlag && lastZeroFlag)) | ||
3553 | { | ||
3554 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3555 | |||
3556 | _acceleration = _velocity; | ||
3557 | |||
3558 | if ((Math.Abs(vel.X) < 0.005f) && | ||
3559 | (Math.Abs(vel.Y) < 0.005f) && | ||
3560 | (Math.Abs(vel.Z) < 0.005f)) | ||
3561 | { | ||
3562 | _velocity = Vector3.Zero; | ||
3563 | float t = -m_invTimeStep; | ||
3564 | _acceleration = _acceleration * t; | ||
3565 | } | ||
3566 | else | ||
3567 | { | ||
3568 | _velocity.X = vel.X; | ||
3569 | _velocity.Y = vel.Y; | ||
3570 | _velocity.Z = vel.Z; | ||
3571 | _acceleration = (_velocity - _acceleration) * m_invTimeStep; | ||
3572 | } | ||
3573 | |||
3574 | if ((Math.Abs(_acceleration.X) < 0.01f) && | ||
3575 | (Math.Abs(_acceleration.Y) < 0.01f) && | ||
3576 | (Math.Abs(_acceleration.Z) < 0.01f)) | ||
3577 | { | ||
3578 | _acceleration = Vector3.Zero; | ||
3579 | } | ||
3580 | |||
3581 | if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && | ||
3582 | (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && | ||
3583 | (Math.Abs(_orientation.Z - ori.Z) < 0.0001) | ||
3584 | ) | ||
3585 | { | ||
3586 | m_rotationalVelocity = Vector3.Zero; | ||
3587 | } | ||
3588 | else | ||
3589 | { | ||
3590 | vel = d.BodyGetAngularVel(Body); | ||
3591 | m_rotationalVelocity.X = vel.X; | ||
3592 | m_rotationalVelocity.Y = vel.Y; | ||
3593 | m_rotationalVelocity.Z = vel.Z; | ||
3594 | } | ||
3595 | // } | ||
3596 | |||
3597 | _position.X = lpos.X; | ||
3598 | _position.Y = lpos.Y; | ||
3599 | _position.Z = lpos.Z; | ||
3600 | |||
3601 | _orientation.X = ori.X; | ||
3602 | _orientation.Y = ori.Y; | ||
3603 | _orientation.Z = ori.Z; | ||
3604 | _orientation.W = ori.W; | ||
3605 | } | ||
3606 | if (_zeroFlag) | ||
3607 | { | ||
3608 | if (lastZeroFlag) | ||
3609 | { | ||
3610 | _velocity = Vector3.Zero; | ||
3611 | _acceleration = Vector3.Zero; | ||
3612 | m_rotationalVelocity = Vector3.Zero; | ||
3613 | } | ||
3614 | |||
3615 | if (!m_lastUpdateSent) | ||
3616 | { | ||
3617 | base.RequestPhysicsterseUpdate(); | ||
3618 | if (lastZeroFlag) | ||
3619 | m_lastUpdateSent = true; | ||
3620 | } | ||
3621 | return; | ||
3622 | } | ||
3623 | |||
3624 | base.RequestPhysicsterseUpdate(); | ||
3625 | m_lastUpdateSent = false; | ||
3626 | } | ||
3627 | } | ||
3628 | } | ||
3629 | |||
3630 | internal static bool QuaternionIsFinite(Quaternion q) | ||
3631 | { | ||
3632 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
3633 | return false; | ||
3634 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
3635 | return false; | ||
3636 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
3637 | return false; | ||
3638 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
3639 | return false; | ||
3640 | return true; | ||
3641 | } | ||
3642 | |||
3643 | internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) | ||
3644 | { | ||
3645 | // assumes object center of mass is zero | ||
3646 | float smass = part.mass; | ||
3647 | theobj.mass -= smass; | ||
3648 | |||
3649 | smass *= 1.0f / (theobj.mass); ; | ||
3650 | |||
3651 | theobj.c.X -= part.c.X * smass; | ||
3652 | theobj.c.Y -= part.c.Y * smass; | ||
3653 | theobj.c.Z -= part.c.Z * smass; | ||
3654 | |||
3655 | theobj.I.M00 -= part.I.M00; | ||
3656 | theobj.I.M01 -= part.I.M01; | ||
3657 | theobj.I.M02 -= part.I.M02; | ||
3658 | theobj.I.M10 -= part.I.M10; | ||
3659 | theobj.I.M11 -= part.I.M11; | ||
3660 | theobj.I.M12 -= part.I.M12; | ||
3661 | theobj.I.M20 -= part.I.M20; | ||
3662 | theobj.I.M21 -= part.I.M21; | ||
3663 | theobj.I.M22 -= part.I.M22; | ||
3664 | } | ||
3665 | |||
3666 | private void donullchange() | ||
3667 | { | ||
3668 | } | ||
3669 | |||
3670 | public bool DoAChange(changes what, object arg) | ||
3671 | { | ||
3672 | if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) | ||
3673 | { | ||
3674 | return false; | ||
3675 | } | ||
3676 | |||
3677 | // nasty switch | ||
3678 | switch (what) | ||
3679 | { | ||
3680 | case changes.Add: | ||
3681 | changeadd(); | ||
3682 | break; | ||
3683 | |||
3684 | case changes.AddPhysRep: | ||
3685 | changeAddPhysRep((ODEPhysRepData)arg); | ||
3686 | break; | ||
3687 | |||
3688 | case changes.Remove: | ||
3689 | //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... | ||
3690 | //When we return true, it destroys all of the prims in the linkset anyway | ||
3691 | if (_parent != null) | ||
3692 | { | ||
3693 | OdePrim parent = (OdePrim)_parent; | ||
3694 | parent.ChildRemove(this, false); | ||
3695 | } | ||
3696 | else | ||
3697 | ChildRemove(this, false); | ||
3698 | |||
3699 | m_vehicle = null; | ||
3700 | RemoveGeom(); | ||
3701 | m_targetSpace = IntPtr.Zero; | ||
3702 | UnSubscribeEvents(); | ||
3703 | return true; | ||
3704 | |||
3705 | case changes.Link: | ||
3706 | OdePrim tmp = (OdePrim)arg; | ||
3707 | changeLink(tmp); | ||
3708 | break; | ||
3709 | |||
3710 | case changes.DeLink: | ||
3711 | changeLink(null); | ||
3712 | break; | ||
3713 | |||
3714 | case changes.Position: | ||
3715 | changePosition((Vector3)arg); | ||
3716 | break; | ||
3717 | |||
3718 | case changes.Orientation: | ||
3719 | changeOrientation((Quaternion)arg); | ||
3720 | break; | ||
3721 | |||
3722 | case changes.PosOffset: | ||
3723 | donullchange(); | ||
3724 | break; | ||
3725 | |||
3726 | case changes.OriOffset: | ||
3727 | donullchange(); | ||
3728 | break; | ||
3729 | |||
3730 | case changes.Velocity: | ||
3731 | changevelocity((Vector3)arg); | ||
3732 | break; | ||
3733 | |||
3734 | // case changes.Acceleration: | ||
3735 | // changeacceleration((Vector3)arg); | ||
3736 | // break; | ||
3737 | |||
3738 | case changes.AngVelocity: | ||
3739 | changeangvelocity((Vector3)arg); | ||
3740 | break; | ||
3741 | |||
3742 | case changes.Force: | ||
3743 | changeForce((Vector3)arg); | ||
3744 | break; | ||
3745 | |||
3746 | case changes.Torque: | ||
3747 | changeSetTorque((Vector3)arg); | ||
3748 | break; | ||
3749 | |||
3750 | case changes.AddForce: | ||
3751 | changeAddForce((Vector3)arg); | ||
3752 | break; | ||
3753 | |||
3754 | case changes.AddAngForce: | ||
3755 | changeAddAngularImpulse((Vector3)arg); | ||
3756 | break; | ||
3757 | |||
3758 | case changes.AngLock: | ||
3759 | changeAngularLock((Vector3)arg); | ||
3760 | break; | ||
3761 | |||
3762 | case changes.Size: | ||
3763 | changeSize((Vector3)arg); | ||
3764 | break; | ||
3765 | |||
3766 | case changes.Shape: | ||
3767 | changeShape((PrimitiveBaseShape)arg); | ||
3768 | break; | ||
3769 | |||
3770 | case changes.PhysRepData: | ||
3771 | changePhysRepData((ODEPhysRepData) arg); | ||
3772 | break; | ||
3773 | |||
3774 | case changes.CollidesWater: | ||
3775 | changeFloatOnWater((bool)arg); | ||
3776 | break; | ||
3777 | |||
3778 | case changes.VolumeDtc: | ||
3779 | changeVolumedetetion((bool)arg); | ||
3780 | break; | ||
3781 | |||
3782 | case changes.Phantom: | ||
3783 | changePhantomStatus((bool)arg); | ||
3784 | break; | ||
3785 | |||
3786 | case changes.Physical: | ||
3787 | changePhysicsStatus((bool)arg); | ||
3788 | break; | ||
3789 | |||
3790 | case changes.Selected: | ||
3791 | changeSelectedStatus((bool)arg); | ||
3792 | break; | ||
3793 | |||
3794 | case changes.disabled: | ||
3795 | changeDisable((bool)arg); | ||
3796 | break; | ||
3797 | |||
3798 | case changes.building: | ||
3799 | changeBuilding((bool)arg); | ||
3800 | break; | ||
3801 | |||
3802 | case changes.VehicleType: | ||
3803 | changeVehicleType((int)arg); | ||
3804 | break; | ||
3805 | |||
3806 | case changes.VehicleFlags: | ||
3807 | changeVehicleFlags((strVehicleBoolParam) arg); | ||
3808 | break; | ||
3809 | |||
3810 | case changes.VehicleFloatParam: | ||
3811 | changeVehicleFloatParam((strVehicleFloatParam) arg); | ||
3812 | break; | ||
3813 | |||
3814 | case changes.VehicleVectorParam: | ||
3815 | changeVehicleVectorParam((strVehicleVectorParam) arg); | ||
3816 | break; | ||
3817 | |||
3818 | case changes.VehicleRotationParam: | ||
3819 | changeVehicleRotationParam((strVehicleQuatParam) arg); | ||
3820 | break; | ||
3821 | |||
3822 | case changes.SetVehicle: | ||
3823 | changeSetVehicle((VehicleData) arg); | ||
3824 | break; | ||
3825 | |||
3826 | case changes.Buoyancy: | ||
3827 | changeBuoyancy((float)arg); | ||
3828 | break; | ||
3829 | |||
3830 | case changes.PIDTarget: | ||
3831 | changePIDTarget((Vector3)arg); | ||
3832 | break; | ||
3833 | |||
3834 | case changes.PIDTau: | ||
3835 | changePIDTau((float)arg); | ||
3836 | break; | ||
3837 | |||
3838 | case changes.PIDActive: | ||
3839 | changePIDActive((bool)arg); | ||
3840 | break; | ||
3841 | |||
3842 | case changes.PIDHoverHeight: | ||
3843 | changePIDHoverHeight((float)arg); | ||
3844 | break; | ||
3845 | |||
3846 | case changes.PIDHoverType: | ||
3847 | changePIDHoverType((PIDHoverType)arg); | ||
3848 | break; | ||
3849 | |||
3850 | case changes.PIDHoverTau: | ||
3851 | changePIDHoverTau((float)arg); | ||
3852 | break; | ||
3853 | |||
3854 | case changes.PIDHoverActive: | ||
3855 | changePIDHoverActive((bool)arg); | ||
3856 | break; | ||
3857 | |||
3858 | case changes.Null: | ||
3859 | donullchange(); | ||
3860 | break; | ||
3861 | |||
3862 | |||
3863 | |||
3864 | default: | ||
3865 | donullchange(); | ||
3866 | break; | ||
3867 | } | ||
3868 | return false; | ||
3869 | } | ||
3870 | |||
3871 | public void AddChange(changes what, object arg) | ||
3872 | { | ||
3873 | _parent_scene.AddChange((PhysicsActor) this, what, arg); | ||
3874 | } | ||
3875 | |||
3876 | |||
3877 | private struct strVehicleBoolParam | ||
3878 | { | ||
3879 | public int param; | ||
3880 | public bool value; | ||
3881 | } | ||
3882 | |||
3883 | private struct strVehicleFloatParam | ||
3884 | { | ||
3885 | public int param; | ||
3886 | public float value; | ||
3887 | } | ||
3888 | |||
3889 | private struct strVehicleQuatParam | ||
3890 | { | ||
3891 | public int param; | ||
3892 | public Quaternion value; | ||
3893 | } | ||
3894 | |||
3895 | private struct strVehicleVectorParam | ||
3896 | { | ||
3897 | public int param; | ||
3898 | public Vector3 value; | ||
3899 | } | ||
3900 | } | ||
3901 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs new file mode 100644 index 0000000..d8add0c --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODERayCastRequestManager.cs | |||
@@ -0,0 +1,683 @@ | |||
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 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Reflection; | ||
31 | using System.Runtime.InteropServices; | ||
32 | using System.Text; | ||
33 | using OpenSim.Framework; | ||
34 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
35 | using OdeAPI; | ||
36 | using log4net; | ||
37 | using OpenMetaverse; | ||
38 | |||
39 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
40 | { | ||
41 | /// <summary> | ||
42 | /// Processes raycast requests as ODE is in a state to be able to do them. | ||
43 | /// This ensures that it's thread safe and there will be no conflicts. | ||
44 | /// Requests get returned by a different thread then they were requested by. | ||
45 | /// </summary> | ||
46 | public class ODERayCastRequestManager | ||
47 | { | ||
48 | /// <summary> | ||
49 | /// Pending ray requests | ||
50 | /// </summary> | ||
51 | protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>(); | ||
52 | |||
53 | /// <summary> | ||
54 | /// Scene that created this object. | ||
55 | /// </summary> | ||
56 | private OdeScene m_scene; | ||
57 | |||
58 | IntPtr ray; // the ray. we only need one for our lifetime | ||
59 | IntPtr Sphere; | ||
60 | IntPtr Box; | ||
61 | IntPtr Plane; | ||
62 | |||
63 | private int CollisionContactGeomsPerTest = 25; | ||
64 | private const int DefaultMaxCount = 25; | ||
65 | private const int MaxTimePerCallMS = 30; | ||
66 | |||
67 | /// <summary> | ||
68 | /// ODE near callback delegate | ||
69 | /// </summary> | ||
70 | private d.NearCallback nearCallback; | ||
71 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
72 | private List<ContactResult> m_contactResults = new List<ContactResult>(); | ||
73 | private RayFilterFlags CurrentRayFilter; | ||
74 | private int CurrentMaxCount; | ||
75 | |||
76 | public ODERayCastRequestManager(OdeScene pScene) | ||
77 | { | ||
78 | m_scene = pScene; | ||
79 | nearCallback = near; | ||
80 | ray = d.CreateRay(IntPtr.Zero, 1.0f); | ||
81 | d.GeomSetCategoryBits(ray, 0); | ||
82 | Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); | ||
83 | d.GeomSetCategoryBits(Box, 0); | ||
84 | Sphere = d.CreateSphere(IntPtr.Zero,1.0f); | ||
85 | d.GeomSetCategoryBits(Sphere, 0); | ||
86 | Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); | ||
87 | d.GeomSetCategoryBits(Sphere, 0); | ||
88 | } | ||
89 | |||
90 | public void QueueRequest(ODERayRequest req) | ||
91 | { | ||
92 | if (req.Count == 0) | ||
93 | req.Count = DefaultMaxCount; | ||
94 | |||
95 | m_PendingRequests.Enqueue(req); | ||
96 | } | ||
97 | |||
98 | /// <summary> | ||
99 | /// Process all queued raycast requests | ||
100 | /// </summary> | ||
101 | /// <returns>Time in MS the raycasts took to process.</returns> | ||
102 | public int ProcessQueuedRequests() | ||
103 | { | ||
104 | |||
105 | if (m_PendingRequests.Count <= 0) | ||
106 | return 0; | ||
107 | |||
108 | if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) | ||
109 | // oops something got wrong or scene isn't ready still | ||
110 | { | ||
111 | m_PendingRequests.Clear(); | ||
112 | return 0; | ||
113 | } | ||
114 | |||
115 | int time = Util.EnvironmentTickCount(); | ||
116 | |||
117 | ODERayRequest req; | ||
118 | int closestHit; | ||
119 | int backfacecull; | ||
120 | CollisionCategories catflags; | ||
121 | |||
122 | while (m_PendingRequests.Dequeue(out req)) | ||
123 | { | ||
124 | if (req.callbackMethod != null) | ||
125 | { | ||
126 | IntPtr geom = IntPtr.Zero; | ||
127 | if (req.actor != null) | ||
128 | { | ||
129 | if (m_scene.haveActor(req.actor)) | ||
130 | { | ||
131 | if (req.actor is OdePrim) | ||
132 | geom = ((OdePrim)req.actor).prim_geom; | ||
133 | else if (req.actor is OdeCharacter) | ||
134 | geom = ((OdePrim)req.actor).prim_geom; | ||
135 | } | ||
136 | if (geom == IntPtr.Zero) | ||
137 | { | ||
138 | NoContacts(req); | ||
139 | continue; | ||
140 | } | ||
141 | } | ||
142 | |||
143 | |||
144 | CurrentRayFilter = req.filter; | ||
145 | CurrentMaxCount = req.Count; | ||
146 | |||
147 | CollisionContactGeomsPerTest = req.Count & 0xffff; | ||
148 | |||
149 | closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); | ||
150 | backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); | ||
151 | |||
152 | if (req.callbackMethod is ProbeBoxCallback) | ||
153 | { | ||
154 | if (CollisionContactGeomsPerTest > 80) | ||
155 | CollisionContactGeomsPerTest = 80; | ||
156 | d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); | ||
157 | d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); | ||
158 | d.Quaternion qtmp; | ||
159 | qtmp.X = req.orientation.X; | ||
160 | qtmp.Y = req.orientation.Y; | ||
161 | qtmp.Z = req.orientation.Z; | ||
162 | qtmp.W = req.orientation.W; | ||
163 | d.GeomSetQuaternion(Box, ref qtmp); | ||
164 | } | ||
165 | else if (req.callbackMethod is ProbeSphereCallback) | ||
166 | { | ||
167 | if (CollisionContactGeomsPerTest > 80) | ||
168 | CollisionContactGeomsPerTest = 80; | ||
169 | |||
170 | d.GeomSphereSetRadius(Sphere, req.length); | ||
171 | d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); | ||
172 | } | ||
173 | else if (req.callbackMethod is ProbePlaneCallback) | ||
174 | { | ||
175 | if (CollisionContactGeomsPerTest > 80) | ||
176 | CollisionContactGeomsPerTest = 80; | ||
177 | |||
178 | d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); | ||
179 | } | ||
180 | |||
181 | else | ||
182 | { | ||
183 | if (CollisionContactGeomsPerTest > 25) | ||
184 | CollisionContactGeomsPerTest = 25; | ||
185 | |||
186 | d.GeomRaySetLength(ray, req.length); | ||
187 | d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); | ||
188 | d.GeomRaySetParams(ray, 0, backfacecull); | ||
189 | d.GeomRaySetClosestHit(ray, closestHit); | ||
190 | |||
191 | if (req.callbackMethod is RaycastCallback) | ||
192 | { | ||
193 | // if we only want one get only one per Collision pair saving memory | ||
194 | CurrentRayFilter |= RayFilterFlags.ClosestHit; | ||
195 | d.GeomRaySetClosestHit(ray, 1); | ||
196 | } | ||
197 | else | ||
198 | d.GeomRaySetClosestHit(ray, closestHit); | ||
199 | } | ||
200 | |||
201 | if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) | ||
202 | unchecked | ||
203 | { | ||
204 | CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; | ||
205 | } | ||
206 | |||
207 | if (geom == IntPtr.Zero) | ||
208 | { | ||
209 | // translate ray filter to Collision flags | ||
210 | catflags = 0; | ||
211 | if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) | ||
212 | catflags |= CollisionCategories.VolumeDtc; | ||
213 | if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) | ||
214 | catflags |= CollisionCategories.Phantom; | ||
215 | if ((CurrentRayFilter & RayFilterFlags.agent) != 0) | ||
216 | catflags |= CollisionCategories.Character; | ||
217 | if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0) | ||
218 | catflags |= CollisionCategories.Geom; | ||
219 | if ((CurrentRayFilter & RayFilterFlags.land) != 0) | ||
220 | catflags |= CollisionCategories.Land; | ||
221 | if ((CurrentRayFilter & RayFilterFlags.water) != 0) | ||
222 | catflags |= CollisionCategories.Water; | ||
223 | |||
224 | if (catflags != 0) | ||
225 | { | ||
226 | if (req.callbackMethod is ProbeBoxCallback) | ||
227 | { | ||
228 | catflags |= CollisionCategories.Space; | ||
229 | d.GeomSetCollideBits(Box, (uint)catflags); | ||
230 | d.GeomSetCategoryBits(Box, (uint)catflags); | ||
231 | doProbe(req, Box); | ||
232 | } | ||
233 | else if (req.callbackMethod is ProbeSphereCallback) | ||
234 | { | ||
235 | catflags |= CollisionCategories.Space; | ||
236 | d.GeomSetCollideBits(Sphere, (uint)catflags); | ||
237 | d.GeomSetCategoryBits(Sphere, (uint)catflags); | ||
238 | doProbe(req, Sphere); | ||
239 | } | ||
240 | else if (req.callbackMethod is ProbePlaneCallback) | ||
241 | { | ||
242 | catflags |= CollisionCategories.Space; | ||
243 | d.GeomSetCollideBits(Plane, (uint)catflags); | ||
244 | d.GeomSetCategoryBits(Plane, (uint)catflags); | ||
245 | doPlane(req,IntPtr.Zero); | ||
246 | } | ||
247 | else | ||
248 | { | ||
249 | d.GeomSetCollideBits(ray, (uint)catflags); | ||
250 | doSpaceRay(req); | ||
251 | } | ||
252 | } | ||
253 | } | ||
254 | else | ||
255 | { | ||
256 | // if we select a geom don't use filters | ||
257 | |||
258 | if (req.callbackMethod is ProbePlaneCallback) | ||
259 | { | ||
260 | d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); | ||
261 | doPlane(req,geom); | ||
262 | } | ||
263 | else | ||
264 | { | ||
265 | d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); | ||
266 | doGeomRay(req,geom); | ||
267 | } | ||
268 | } | ||
269 | } | ||
270 | |||
271 | if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) | ||
272 | break; | ||
273 | } | ||
274 | |||
275 | lock (m_contactResults) | ||
276 | m_contactResults.Clear(); | ||
277 | |||
278 | return Util.EnvironmentTickCountSubtract(time); | ||
279 | } | ||
280 | /// <summary> | ||
281 | /// Method that actually initiates the raycast with spaces | ||
282 | /// </summary> | ||
283 | /// <param name="req"></param> | ||
284 | /// | ||
285 | |||
286 | private void NoContacts(ODERayRequest req) | ||
287 | { | ||
288 | if (req.callbackMethod is RaycastCallback) | ||
289 | { | ||
290 | ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); | ||
291 | return; | ||
292 | } | ||
293 | List<ContactResult> cresult = new List<ContactResult>(); | ||
294 | |||
295 | if (req.callbackMethod is RayCallback) | ||
296 | ((RayCallback)req.callbackMethod)(cresult); | ||
297 | else if (req.callbackMethod is ProbeBoxCallback) | ||
298 | ((ProbeBoxCallback)req.callbackMethod)(cresult); | ||
299 | else if (req.callbackMethod is ProbeSphereCallback) | ||
300 | ((ProbeSphereCallback)req.callbackMethod)(cresult); | ||
301 | } | ||
302 | |||
303 | private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom; | ||
304 | // private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; | ||
305 | private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; | ||
306 | |||
307 | private void doSpaceRay(ODERayRequest req) | ||
308 | { | ||
309 | // Collide tests | ||
310 | if ((CurrentRayFilter & FilterActiveSpace) != 0) | ||
311 | { | ||
312 | d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); | ||
313 | d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); | ||
314 | } | ||
315 | if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
316 | d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); | ||
317 | if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
318 | { | ||
319 | // current ode land to ray collisions is very bad | ||
320 | // so for now limit its range badly | ||
321 | |||
322 | if (req.length > 30.0f) | ||
323 | d.GeomRaySetLength(ray, 30.0f); | ||
324 | |||
325 | d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); | ||
326 | } | ||
327 | |||
328 | if (req.callbackMethod is RaycastCallback) | ||
329 | { | ||
330 | // Define default results | ||
331 | bool hitYN = false; | ||
332 | uint hitConsumerID = 0; | ||
333 | float distance = float.MaxValue; | ||
334 | Vector3 closestcontact = Vector3.Zero; | ||
335 | Vector3 snormal = Vector3.Zero; | ||
336 | |||
337 | // Find closest contact and object. | ||
338 | lock (m_contactResults) | ||
339 | { | ||
340 | foreach (ContactResult cResult in m_contactResults) | ||
341 | { | ||
342 | if(cResult.Depth < distance) | ||
343 | { | ||
344 | closestcontact = cResult.Pos; | ||
345 | hitConsumerID = cResult.ConsumerID; | ||
346 | distance = cResult.Depth; | ||
347 | snormal = cResult.Normal; | ||
348 | } | ||
349 | } | ||
350 | m_contactResults.Clear(); | ||
351 | } | ||
352 | |||
353 | if (distance > 0 && distance < float.MaxValue) | ||
354 | hitYN = true; | ||
355 | ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); | ||
356 | } | ||
357 | else | ||
358 | { | ||
359 | List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count); | ||
360 | lock (m_PendingRequests) | ||
361 | { | ||
362 | cresult.AddRange(m_contactResults); | ||
363 | m_contactResults.Clear(); | ||
364 | } | ||
365 | ((RayCallback)req.callbackMethod)(cresult); | ||
366 | } | ||
367 | } | ||
368 | |||
369 | private void doProbe(ODERayRequest req, IntPtr probe) | ||
370 | { | ||
371 | // Collide tests | ||
372 | if ((CurrentRayFilter & FilterActiveSpace) != 0) | ||
373 | { | ||
374 | d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); | ||
375 | d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); | ||
376 | } | ||
377 | if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
378 | d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); | ||
379 | if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
380 | d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); | ||
381 | |||
382 | List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count); | ||
383 | lock (m_PendingRequests) | ||
384 | { | ||
385 | cresult.AddRange(m_contactResults); | ||
386 | m_contactResults.Clear(); | ||
387 | } | ||
388 | if (req.callbackMethod is ProbeBoxCallback) | ||
389 | ((ProbeBoxCallback)req.callbackMethod)(cresult); | ||
390 | else if (req.callbackMethod is ProbeSphereCallback) | ||
391 | ((ProbeSphereCallback)req.callbackMethod)(cresult); | ||
392 | } | ||
393 | |||
394 | private void doPlane(ODERayRequest req,IntPtr geom) | ||
395 | { | ||
396 | // Collide tests | ||
397 | if (geom == IntPtr.Zero) | ||
398 | { | ||
399 | if ((CurrentRayFilter & FilterActiveSpace) != 0) | ||
400 | { | ||
401 | d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); | ||
402 | d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); | ||
403 | } | ||
404 | if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
405 | d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); | ||
406 | if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) | ||
407 | d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); | ||
408 | } | ||
409 | else | ||
410 | { | ||
411 | d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); | ||
412 | } | ||
413 | |||
414 | List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count); | ||
415 | lock (m_PendingRequests) | ||
416 | { | ||
417 | cresult.AddRange(m_contactResults); | ||
418 | m_contactResults.Clear(); | ||
419 | } | ||
420 | |||
421 | ((ProbePlaneCallback)req.callbackMethod)(cresult); | ||
422 | } | ||
423 | |||
424 | /// <summary> | ||
425 | /// Method that actually initiates the raycast with a geom | ||
426 | /// </summary> | ||
427 | /// <param name="req"></param> | ||
428 | private void doGeomRay(ODERayRequest req, IntPtr geom) | ||
429 | { | ||
430 | // Collide test | ||
431 | d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test | ||
432 | |||
433 | if (req.callbackMethod is RaycastCallback) | ||
434 | { | ||
435 | // Define default results | ||
436 | bool hitYN = false; | ||
437 | uint hitConsumerID = 0; | ||
438 | float distance = float.MaxValue; | ||
439 | Vector3 closestcontact = Vector3.Zero; | ||
440 | Vector3 snormal = Vector3.Zero; | ||
441 | |||
442 | // Find closest contact and object. | ||
443 | lock (m_contactResults) | ||
444 | { | ||
445 | foreach (ContactResult cResult in m_contactResults) | ||
446 | { | ||
447 | if(cResult.Depth < distance ) | ||
448 | { | ||
449 | closestcontact = cResult.Pos; | ||
450 | hitConsumerID = cResult.ConsumerID; | ||
451 | distance = cResult.Depth; | ||
452 | snormal = cResult.Normal; | ||
453 | } | ||
454 | } | ||
455 | m_contactResults.Clear(); | ||
456 | } | ||
457 | |||
458 | if (distance > 0 && distance < float.MaxValue) | ||
459 | hitYN = true; | ||
460 | |||
461 | ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); | ||
462 | } | ||
463 | else | ||
464 | { | ||
465 | List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count); | ||
466 | lock (m_PendingRequests) | ||
467 | { | ||
468 | cresult.AddRange(m_contactResults); | ||
469 | m_contactResults.Clear(); | ||
470 | } | ||
471 | ((RayCallback)req.callbackMethod)(cresult); | ||
472 | } | ||
473 | } | ||
474 | |||
475 | private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) | ||
476 | { | ||
477 | IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; | ||
478 | if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) | ||
479 | return false; | ||
480 | |||
481 | IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); | ||
482 | newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); | ||
483 | return true; | ||
484 | } | ||
485 | |||
486 | // This is the standard Near. g1 is the ray | ||
487 | private void near(IntPtr space, IntPtr g1, IntPtr g2) | ||
488 | { | ||
489 | if (g2 == IntPtr.Zero || g1 == g2) | ||
490 | return; | ||
491 | |||
492 | if (m_contactResults.Count >= CurrentMaxCount) | ||
493 | return; | ||
494 | |||
495 | if (d.GeomIsSpace(g2)) | ||
496 | { | ||
497 | try | ||
498 | { | ||
499 | d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); | ||
500 | } | ||
501 | catch (Exception e) | ||
502 | { | ||
503 | m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); | ||
504 | } | ||
505 | return; | ||
506 | } | ||
507 | |||
508 | int count = 0; | ||
509 | try | ||
510 | { | ||
511 | count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
512 | } | ||
513 | catch (Exception e) | ||
514 | { | ||
515 | m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); | ||
516 | return; | ||
517 | } | ||
518 | |||
519 | if (count == 0) | ||
520 | return; | ||
521 | /* | ||
522 | uint cat1 = d.GeomGetCategoryBits(g1); | ||
523 | uint cat2 = d.GeomGetCategoryBits(g2); | ||
524 | uint col1 = d.GeomGetCollideBits(g1); | ||
525 | uint col2 = d.GeomGetCollideBits(g2); | ||
526 | */ | ||
527 | |||
528 | uint ID = 0; | ||
529 | PhysicsActor p2 = null; | ||
530 | |||
531 | m_scene.actor_name_map.TryGetValue(g2, out p2); | ||
532 | |||
533 | if (p2 == null) | ||
534 | return; | ||
535 | |||
536 | switch (p2.PhysicsActorType) | ||
537 | { | ||
538 | case (int)ActorTypes.Prim: | ||
539 | |||
540 | RayFilterFlags thisFlags; | ||
541 | |||
542 | if (p2.IsPhysical) | ||
543 | thisFlags = RayFilterFlags.physical; | ||
544 | else | ||
545 | thisFlags = RayFilterFlags.nonphysical; | ||
546 | |||
547 | if (p2.Phantom) | ||
548 | thisFlags |= RayFilterFlags.phantom; | ||
549 | |||
550 | if (p2.IsVolumeDtc) | ||
551 | thisFlags |= RayFilterFlags.volumedtc; | ||
552 | |||
553 | if ((thisFlags & CurrentRayFilter) == 0) | ||
554 | return; | ||
555 | |||
556 | ID = ((OdePrim)p2).LocalID; | ||
557 | break; | ||
558 | |||
559 | case (int)ActorTypes.Agent: | ||
560 | |||
561 | if ((CurrentRayFilter & RayFilterFlags.agent) == 0) | ||
562 | return; | ||
563 | else | ||
564 | ID = ((OdeCharacter)p2).LocalID; | ||
565 | break; | ||
566 | |||
567 | case (int)ActorTypes.Ground: | ||
568 | |||
569 | if ((CurrentRayFilter & RayFilterFlags.land) == 0) | ||
570 | return; | ||
571 | break; | ||
572 | |||
573 | case (int)ActorTypes.Water: | ||
574 | |||
575 | if ((CurrentRayFilter & RayFilterFlags.water) == 0) | ||
576 | return; | ||
577 | break; | ||
578 | |||
579 | default: | ||
580 | break; | ||
581 | } | ||
582 | |||
583 | d.ContactGeom curcontact = new d.ContactGeom(); | ||
584 | |||
585 | // closestHit for now only works for meshs, so must do it for others | ||
586 | if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) | ||
587 | { | ||
588 | // Loop all contacts, build results. | ||
589 | for (int i = 0; i < count; i++) | ||
590 | { | ||
591 | if (!GetCurContactGeom(i, ref curcontact)) | ||
592 | break; | ||
593 | |||
594 | ContactResult collisionresult = new ContactResult(); | ||
595 | collisionresult.ConsumerID = ID; | ||
596 | collisionresult.Pos.X = curcontact.pos.X; | ||
597 | collisionresult.Pos.Y = curcontact.pos.Y; | ||
598 | collisionresult.Pos.Z = curcontact.pos.Z; | ||
599 | collisionresult.Depth = curcontact.depth; | ||
600 | collisionresult.Normal.X = curcontact.normal.X; | ||
601 | collisionresult.Normal.Y = curcontact.normal.Y; | ||
602 | collisionresult.Normal.Z = curcontact.normal.Z; | ||
603 | lock (m_contactResults) | ||
604 | { | ||
605 | m_contactResults.Add(collisionresult); | ||
606 | if (m_contactResults.Count >= CurrentMaxCount) | ||
607 | return; | ||
608 | } | ||
609 | } | ||
610 | } | ||
611 | else | ||
612 | { | ||
613 | // keep only closest contact | ||
614 | ContactResult collisionresult = new ContactResult(); | ||
615 | collisionresult.ConsumerID = ID; | ||
616 | collisionresult.Depth = float.MaxValue; | ||
617 | |||
618 | for (int i = 0; i < count; i++) | ||
619 | { | ||
620 | if (!GetCurContactGeom(i, ref curcontact)) | ||
621 | break; | ||
622 | |||
623 | if (curcontact.depth < collisionresult.Depth) | ||
624 | { | ||
625 | collisionresult.Pos.X = curcontact.pos.X; | ||
626 | collisionresult.Pos.Y = curcontact.pos.Y; | ||
627 | collisionresult.Pos.Z = curcontact.pos.Z; | ||
628 | collisionresult.Depth = curcontact.depth; | ||
629 | collisionresult.Normal.X = curcontact.normal.X; | ||
630 | collisionresult.Normal.Y = curcontact.normal.Y; | ||
631 | collisionresult.Normal.Z = curcontact.normal.Z; | ||
632 | } | ||
633 | } | ||
634 | |||
635 | if (collisionresult.Depth != float.MaxValue) | ||
636 | { | ||
637 | lock (m_contactResults) | ||
638 | m_contactResults.Add(collisionresult); | ||
639 | } | ||
640 | } | ||
641 | } | ||
642 | |||
643 | /// <summary> | ||
644 | /// Dereference the creator scene so that it can be garbage collected if needed. | ||
645 | /// </summary> | ||
646 | internal void Dispose() | ||
647 | { | ||
648 | m_scene = null; | ||
649 | if (ray != IntPtr.Zero) | ||
650 | { | ||
651 | d.GeomDestroy(ray); | ||
652 | ray = IntPtr.Zero; | ||
653 | } | ||
654 | if (Box != IntPtr.Zero) | ||
655 | { | ||
656 | d.GeomDestroy(Box); | ||
657 | Box = IntPtr.Zero; | ||
658 | } | ||
659 | if (Sphere != IntPtr.Zero) | ||
660 | { | ||
661 | d.GeomDestroy(Sphere); | ||
662 | Sphere = IntPtr.Zero; | ||
663 | } | ||
664 | if (Plane != IntPtr.Zero) | ||
665 | { | ||
666 | d.GeomDestroy(Plane); | ||
667 | Plane = IntPtr.Zero; | ||
668 | } | ||
669 | } | ||
670 | } | ||
671 | |||
672 | public struct ODERayRequest | ||
673 | { | ||
674 | public PhysicsActor actor; | ||
675 | public Vector3 Origin; | ||
676 | public Vector3 Normal; | ||
677 | public int Count; | ||
678 | public float length; | ||
679 | public object callbackMethod; | ||
680 | public RayFilterFlags filter; | ||
681 | public Quaternion orientation; | ||
682 | } | ||
683 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs new file mode 100644 index 0000000..79d23b1 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/ODESitAvatar.cs | |||
@@ -0,0 +1,356 @@ | |||
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 | // Ubit 2012 | ||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Reflection; | ||
31 | using System.Runtime.InteropServices; | ||
32 | using System.Text; | ||
33 | using OpenSim.Framework; | ||
34 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
35 | using OdeAPI; | ||
36 | using log4net; | ||
37 | using OpenMetaverse; | ||
38 | |||
39 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
40 | { | ||
41 | /// <summary> | ||
42 | /// </summary> | ||
43 | public class ODESitAvatar | ||
44 | { | ||
45 | private OdeScene m_scene; | ||
46 | private ODERayCastRequestManager m_raymanager; | ||
47 | |||
48 | public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager) | ||
49 | { | ||
50 | m_scene = pScene; | ||
51 | m_raymanager = raymanager; | ||
52 | } | ||
53 | |||
54 | private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); | ||
55 | private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; | ||
56 | |||
57 | private void RotAroundZ(float x, float y, ref Quaternion ori) | ||
58 | { | ||
59 | double ang = Math.Atan2(y, x); | ||
60 | ang *= 0.5d; | ||
61 | float s = (float)Math.Sin(ang); | ||
62 | float c = (float)Math.Cos(ang); | ||
63 | |||
64 | ori.X = 0; | ||
65 | ori.Y = 0; | ||
66 | ori.Z = s; | ||
67 | ori.W = c; | ||
68 | } | ||
69 | |||
70 | |||
71 | public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) | ||
72 | { | ||
73 | if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) | ||
74 | { | ||
75 | PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); | ||
76 | return; | ||
77 | } | ||
78 | |||
79 | IntPtr geom = ((OdePrim)actor).prim_geom; | ||
80 | |||
81 | Vector3 geopos = d.GeomGetPositionOMV(geom); | ||
82 | Quaternion geomOri = d.GeomGetQuaternionOMV(geom); | ||
83 | |||
84 | // Vector3 geopos = actor.Position; | ||
85 | // Quaternion geomOri = actor.Orientation; | ||
86 | |||
87 | Quaternion geomInvOri = Quaternion.Conjugate(geomOri); | ||
88 | |||
89 | Quaternion ori = Quaternion.Identity; | ||
90 | |||
91 | Vector3 rayDir = geopos + offset - avCameraPosition; | ||
92 | |||
93 | float raylen = rayDir.Length(); | ||
94 | if (raylen < 0.001f) | ||
95 | { | ||
96 | PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); | ||
97 | return; | ||
98 | } | ||
99 | float t = 1 / raylen; | ||
100 | rayDir.X *= t; | ||
101 | rayDir.Y *= t; | ||
102 | rayDir.Z *= t; | ||
103 | |||
104 | raylen += 30f; // focal point may be far | ||
105 | List<ContactResult> rayResults; | ||
106 | |||
107 | rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); | ||
108 | if (rayResults.Count == 0) | ||
109 | { | ||
110 | /* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim | ||
111 | d.AABB aabb; | ||
112 | d.GeomGetAABB(geom, out aabb); | ||
113 | offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); | ||
114 | ori = geomInvOri; | ||
115 | offset *= geomInvOri; | ||
116 | PhysicsSitResponse(1, actor.LocalID, offset, ori); | ||
117 | */ | ||
118 | PhysicsSitResponse(0, actor.LocalID, offset, ori); | ||
119 | return; | ||
120 | } | ||
121 | |||
122 | int status = 1; | ||
123 | |||
124 | offset = rayResults[0].Pos - geopos; | ||
125 | |||
126 | d.GeomClassID geoclass = d.GeomGetClass(geom); | ||
127 | |||
128 | if (geoclass == d.GeomClassID.SphereClass) | ||
129 | { | ||
130 | float r = d.GeomSphereGetRadius(geom); | ||
131 | |||
132 | offset.Normalize(); | ||
133 | offset *= r; | ||
134 | |||
135 | RotAroundZ(offset.X, offset.Y, ref ori); | ||
136 | |||
137 | if (r < 0.4f) | ||
138 | { | ||
139 | offset = new Vector3(0, 0, r); | ||
140 | } | ||
141 | else | ||
142 | { | ||
143 | if (offset.Z < 0.4f) | ||
144 | { | ||
145 | t = offset.Z; | ||
146 | float rsq = r * r; | ||
147 | |||
148 | t = 1.0f / (rsq - t * t); | ||
149 | offset.X *= t; | ||
150 | offset.Y *= t; | ||
151 | offset.Z = 0.4f; | ||
152 | t = rsq - 0.16f; | ||
153 | offset.X *= t; | ||
154 | offset.Y *= t; | ||
155 | } | ||
156 | else if (r > 0.8f && offset.Z > 0.8f * r) | ||
157 | { | ||
158 | status = 3; | ||
159 | avOffset.X = -avOffset.X; | ||
160 | avOffset.Z *= 1.6f; | ||
161 | } | ||
162 | } | ||
163 | |||
164 | offset += avOffset * ori; | ||
165 | |||
166 | ori = geomInvOri * ori; | ||
167 | offset *= geomInvOri; | ||
168 | |||
169 | PhysicsSitResponse(status, actor.LocalID, offset, ori); | ||
170 | return; | ||
171 | } | ||
172 | |||
173 | Vector3 norm = rayResults[0].Normal; | ||
174 | |||
175 | if (norm.Z < -0.4f) | ||
176 | { | ||
177 | PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); | ||
178 | return; | ||
179 | } | ||
180 | |||
181 | |||
182 | float SitNormX = -rayDir.X; | ||
183 | float SitNormY = -rayDir.Y; | ||
184 | |||
185 | Vector3 pivot = geopos + offset; | ||
186 | |||
187 | float edgeNormalX = norm.X; | ||
188 | float edgeNormalY = norm.Y; | ||
189 | float edgeDirX = -rayDir.X; | ||
190 | float edgeDirY = -rayDir.Y; | ||
191 | Vector3 edgePos = rayResults[0].Pos; | ||
192 | float edgeDist = float.MaxValue; | ||
193 | |||
194 | bool foundEdge = false; | ||
195 | |||
196 | if (norm.Z < 0.5f) | ||
197 | { | ||
198 | float rayDist = 4.0f; | ||
199 | |||
200 | for (int i = 0; i < 6; i++) | ||
201 | { | ||
202 | pivot.X -= 0.01f * norm.X; | ||
203 | pivot.Y -= 0.01f * norm.Y; | ||
204 | pivot.Z -= 0.01f * norm.Z; | ||
205 | |||
206 | rayDir.X = -norm.X * norm.Z; | ||
207 | rayDir.Y = -norm.Y * norm.Z; | ||
208 | rayDir.Z = 1.0f - norm.Z * norm.Z; | ||
209 | rayDir.Normalize(); | ||
210 | |||
211 | rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); | ||
212 | if (rayResults.Count == 0) | ||
213 | break; | ||
214 | |||
215 | if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) | ||
216 | { | ||
217 | rayDist -= rayResults[0].Depth; | ||
218 | if (rayDist < 0f) | ||
219 | break; | ||
220 | |||
221 | pivot = rayResults[0].Pos; | ||
222 | norm = rayResults[0].Normal; | ||
223 | edgeNormalX = norm.X; | ||
224 | edgeNormalY = norm.Y; | ||
225 | edgeDirX = -rayDir.X; | ||
226 | edgeDirY = -rayDir.Y; | ||
227 | } | ||
228 | else | ||
229 | { | ||
230 | foundEdge = true; | ||
231 | edgePos = rayResults[0].Pos; | ||
232 | break; | ||
233 | } | ||
234 | } | ||
235 | |||
236 | if (!foundEdge) | ||
237 | { | ||
238 | PhysicsSitResponse(0, actor.LocalID, offset, ori); | ||
239 | return; | ||
240 | } | ||
241 | avOffset.X *= 0.5f; | ||
242 | } | ||
243 | |||
244 | else if (norm.Z > 0.866f) | ||
245 | { | ||
246 | float toCamBaseX = avCameraPosition.X - pivot.X; | ||
247 | float toCamBaseY = avCameraPosition.Y - pivot.Y; | ||
248 | float toCamX = toCamBaseX; | ||
249 | float toCamY = toCamBaseY; | ||
250 | |||
251 | for (int j = 0; j < 4; j++) | ||
252 | { | ||
253 | float rayDist = 1.0f; | ||
254 | float curEdgeDist = 0.0f; | ||
255 | |||
256 | for (int i = 0; i < 3; i++) | ||
257 | { | ||
258 | pivot.Z -= 0.01f; | ||
259 | rayDir.X = toCamX; | ||
260 | rayDir.Y = toCamY; | ||
261 | rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; | ||
262 | rayDir.Normalize(); | ||
263 | |||
264 | rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); | ||
265 | if (rayResults.Count == 0) | ||
266 | break; | ||
267 | |||
268 | curEdgeDist += rayResults[0].Depth; | ||
269 | |||
270 | if (rayResults[0].Normal.Z > 0.5f) | ||
271 | { | ||
272 | rayDist -= rayResults[0].Depth; | ||
273 | if (rayDist < 0f) | ||
274 | break; | ||
275 | |||
276 | pivot = rayResults[0].Pos; | ||
277 | norm = rayResults[0].Normal; | ||
278 | } | ||
279 | else | ||
280 | { | ||
281 | foundEdge = true; | ||
282 | if (curEdgeDist < edgeDist) | ||
283 | { | ||
284 | edgeDist = curEdgeDist; | ||
285 | edgeNormalX = rayResults[0].Normal.X; | ||
286 | edgeNormalY = rayResults[0].Normal.Y; | ||
287 | edgeDirX = rayDir.X; | ||
288 | edgeDirY = rayDir.Y; | ||
289 | edgePos = rayResults[0].Pos; | ||
290 | } | ||
291 | break; | ||
292 | } | ||
293 | } | ||
294 | if (foundEdge && edgeDist < 0.2f) | ||
295 | break; | ||
296 | |||
297 | pivot = geopos + offset; | ||
298 | |||
299 | switch (j) | ||
300 | { | ||
301 | case 0: | ||
302 | toCamX = -toCamBaseY; | ||
303 | toCamY = toCamBaseX; | ||
304 | break; | ||
305 | case 1: | ||
306 | toCamX = toCamBaseY; | ||
307 | toCamY = -toCamBaseX; | ||
308 | break; | ||
309 | case 2: | ||
310 | toCamX = -toCamBaseX; | ||
311 | toCamY = -toCamBaseY; | ||
312 | break; | ||
313 | default: | ||
314 | break; | ||
315 | } | ||
316 | } | ||
317 | |||
318 | if (!foundEdge) | ||
319 | { | ||
320 | avOffset.X = -avOffset.X; | ||
321 | avOffset.Z *= 1.6f; | ||
322 | |||
323 | RotAroundZ(SitNormX, SitNormY, ref ori); | ||
324 | |||
325 | offset += avOffset * ori; | ||
326 | |||
327 | ori = geomInvOri * ori; | ||
328 | offset *= geomInvOri; | ||
329 | |||
330 | PhysicsSitResponse(3, actor.LocalID, offset, ori); | ||
331 | return; | ||
332 | } | ||
333 | avOffset.X *= 0.5f; | ||
334 | } | ||
335 | |||
336 | SitNormX = edgeNormalX; | ||
337 | SitNormY = edgeNormalY; | ||
338 | if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) | ||
339 | { | ||
340 | SitNormX = -SitNormX; | ||
341 | SitNormY = -SitNormY; | ||
342 | } | ||
343 | |||
344 | RotAroundZ(SitNormX, SitNormY, ref ori); | ||
345 | |||
346 | offset = edgePos + avOffset * ori; | ||
347 | offset -= geopos; | ||
348 | |||
349 | ori = geomInvOri * ori; | ||
350 | offset *= geomInvOri; | ||
351 | |||
352 | PhysicsSitResponse(1, actor.LocalID, offset, ori); | ||
353 | return; | ||
354 | } | ||
355 | } | ||
356 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs b/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs new file mode 100644 index 0000000..10d7d50 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/OdeApi.cs | |||
@@ -0,0 +1,2025 @@ | |||
1 | /* | ||
2 | * based on: | ||
3 | * Ode.NET - .NET bindings for ODE | ||
4 | * Jason Perkins (starkos@industriousone.com) | ||
5 | * Licensed under the New BSD | ||
6 | * Part of the OpenDynamicsEngine | ||
7 | Open Dynamics Engine | ||
8 | Copyright (c) 2001-2007, Russell L. Smith. | ||
9 | All rights reserved. | ||
10 | |||
11 | Redistribution and use in source and binary forms, with or without | ||
12 | modification, are permitted provided that the following conditions | ||
13 | are met: | ||
14 | |||
15 | Redistributions of source code must retain the above copyright notice, | ||
16 | this list of conditions and the following disclaimer. | ||
17 | |||
18 | Redistributions in binary form must reproduce the above copyright notice, | ||
19 | this list of conditions and the following disclaimer in the documentation | ||
20 | and/or other materials provided with the distribution. | ||
21 | |||
22 | Neither the names of ODE's copyright owner nor the names of its | ||
23 | contributors may be used to endorse or promote products derived from | ||
24 | this software without specific prior written permission. | ||
25 | |||
26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
27 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
28 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
29 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
30 | OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
31 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED | ||
32 | TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR | ||
33 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF | ||
34 | LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING | ||
35 | NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
36 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
37 | * | ||
38 | * changes by opensim team; | ||
39 | * changes by Aurora team http://www.aurora-sim.org/ | ||
40 | |||
41 | * Revision/fixs by Ubit Umarov | ||
42 | */ | ||
43 | |||
44 | using System; | ||
45 | using System.Runtime.InteropServices; | ||
46 | using System.Security; | ||
47 | using OMV = OpenMetaverse; | ||
48 | namespace OdeAPI | ||
49 | { | ||
50 | //#if dDOUBLE | ||
51 | // don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim | ||
52 | // at least we save same memory and memory access time, FPU performance on intel usually is similar | ||
53 | // using dReal = System.Double; | ||
54 | //#else | ||
55 | using dReal = System.Single; | ||
56 | //#endif | ||
57 | |||
58 | public static class d | ||
59 | { | ||
60 | public static dReal Infinity = dReal.MaxValue; | ||
61 | public static int NTotalBodies = 0; | ||
62 | public static int NTotalGeoms = 0; | ||
63 | |||
64 | public const uint CONTACTS_UNIMPORTANT = 0x80000000; | ||
65 | |||
66 | #region Flags and Enumerations | ||
67 | |||
68 | [Flags] | ||
69 | public enum AllocateODEDataFlags : uint | ||
70 | { | ||
71 | BasicData = 0, | ||
72 | CollisionData = 0x00000001, | ||
73 | All = ~0u | ||
74 | } | ||
75 | |||
76 | [Flags] | ||
77 | public enum IniteODEFlags : uint | ||
78 | { | ||
79 | dInitFlagManualThreadCleanup = 0x00000001 | ||
80 | } | ||
81 | |||
82 | [Flags] | ||
83 | public enum ContactFlags : int | ||
84 | { | ||
85 | Mu2 = 0x001, | ||
86 | FDir1 = 0x002, | ||
87 | Bounce = 0x004, | ||
88 | SoftERP = 0x008, | ||
89 | SoftCFM = 0x010, | ||
90 | Motion1 = 0x020, | ||
91 | Motion2 = 0x040, | ||
92 | MotionN = 0x080, | ||
93 | Slip1 = 0x100, | ||
94 | Slip2 = 0x200, | ||
95 | Approx0 = 0x0000, | ||
96 | Approx1_1 = 0x1000, | ||
97 | Approx1_2 = 0x2000, | ||
98 | Approx1 = 0x3000 | ||
99 | } | ||
100 | |||
101 | public enum GeomClassID : int | ||
102 | { | ||
103 | SphereClass, | ||
104 | BoxClass, | ||
105 | CapsuleClass, | ||
106 | CylinderClass, | ||
107 | PlaneClass, | ||
108 | RayClass, | ||
109 | ConvexClass, | ||
110 | GeomTransformClass, | ||
111 | TriMeshClass, | ||
112 | HeightfieldClass, | ||
113 | FirstSpaceClass, | ||
114 | SimpleSpaceClass = FirstSpaceClass, | ||
115 | HashSpaceClass, | ||
116 | QuadTreeSpaceClass, | ||
117 | LastSpaceClass = QuadTreeSpaceClass, | ||
118 | UbitTerrainClass, | ||
119 | FirstUserClass, | ||
120 | LastUserClass = FirstUserClass + MaxUserClasses - 1, | ||
121 | NumClasses, | ||
122 | MaxUserClasses = 5 | ||
123 | } | ||
124 | |||
125 | public enum JointType : int | ||
126 | { | ||
127 | None, | ||
128 | Ball, | ||
129 | Hinge, | ||
130 | Slider, | ||
131 | Contact, | ||
132 | Universal, | ||
133 | Hinge2, | ||
134 | Fixed, | ||
135 | Null, | ||
136 | AMotor, | ||
137 | LMotor, | ||
138 | Plane2D | ||
139 | } | ||
140 | |||
141 | public enum JointParam : int | ||
142 | { | ||
143 | LoStop, | ||
144 | HiStop, | ||
145 | Vel, | ||
146 | FMax, | ||
147 | FudgeFactor, | ||
148 | Bounce, | ||
149 | CFM, | ||
150 | StopERP, | ||
151 | StopCFM, | ||
152 | SuspensionERP, | ||
153 | SuspensionCFM, | ||
154 | LoStop2 = 256, | ||
155 | HiStop2, | ||
156 | Vel2, | ||
157 | FMax2, | ||
158 | FudgeFactor2, | ||
159 | Bounce2, | ||
160 | CFM2, | ||
161 | StopERP2, | ||
162 | StopCFM2, | ||
163 | SuspensionERP2, | ||
164 | SuspensionCFM2, | ||
165 | LoStop3 = 512, | ||
166 | HiStop3, | ||
167 | Vel3, | ||
168 | FMax3, | ||
169 | FudgeFactor3, | ||
170 | Bounce3, | ||
171 | CFM3, | ||
172 | StopERP3, | ||
173 | StopCFM3, | ||
174 | SuspensionERP3, | ||
175 | SuspensionCFM3 | ||
176 | } | ||
177 | |||
178 | public enum dSweepAndPruneAxis : int | ||
179 | { | ||
180 | XYZ = ((0)|(1<<2)|(2<<4)), | ||
181 | XZY = ((0)|(2<<2)|(1<<4)), | ||
182 | YXZ = ((1)|(0<<2)|(2<<4)), | ||
183 | YZX = ((1)|(2<<2)|(0<<4)), | ||
184 | ZXY = ((2)|(0<<2)|(1<<4)), | ||
185 | ZYX = ((2)|(1<<2)|(0<<4)) | ||
186 | } | ||
187 | |||
188 | #endregion | ||
189 | |||
190 | #region Callbacks | ||
191 | |||
192 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
193 | public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); | ||
194 | |||
195 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
196 | public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); | ||
197 | |||
198 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
199 | public delegate void GetAABBFn(IntPtr geom, out AABB aabb); | ||
200 | |||
201 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
202 | public delegate ColliderFn GetColliderFnFn(int num); | ||
203 | |||
204 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
205 | public delegate void GeomDtorFn(IntPtr o); | ||
206 | |||
207 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
208 | public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); | ||
209 | |||
210 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
211 | public delegate dReal UbitTerrainGetHeight(IntPtr p_user_data, int x, int z); | ||
212 | |||
213 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
214 | public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); | ||
215 | |||
216 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
217 | public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); | ||
218 | |||
219 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
220 | public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); | ||
221 | |||
222 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
223 | public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); | ||
224 | |||
225 | #endregion | ||
226 | |||
227 | #region Structs | ||
228 | |||
229 | [StructLayout(LayoutKind.Sequential)] | ||
230 | public struct AABB | ||
231 | { | ||
232 | public dReal MinX, MaxX; | ||
233 | public dReal MinY, MaxY; | ||
234 | public dReal MinZ, MaxZ; | ||
235 | } | ||
236 | |||
237 | |||
238 | [StructLayout(LayoutKind.Sequential)] | ||
239 | public struct Contact | ||
240 | { | ||
241 | public SurfaceParameters surface; | ||
242 | public ContactGeom geom; | ||
243 | public Vector3 fdir1; | ||
244 | public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); | ||
245 | } | ||
246 | |||
247 | |||
248 | [StructLayout(LayoutKind.Sequential)] | ||
249 | public struct ContactGeom | ||
250 | { | ||
251 | |||
252 | public Vector3 pos; | ||
253 | public Vector3 normal; | ||
254 | public dReal depth; | ||
255 | public IntPtr g1; | ||
256 | public IntPtr g2; | ||
257 | public int side1; | ||
258 | public int side2; | ||
259 | public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); | ||
260 | } | ||
261 | |||
262 | [StructLayout(LayoutKind.Sequential)] | ||
263 | public struct GeomClass | ||
264 | { | ||
265 | public int bytes; | ||
266 | public GetColliderFnFn collider; | ||
267 | public GetAABBFn aabb; | ||
268 | public AABBTestFn aabb_test; | ||
269 | public GeomDtorFn dtor; | ||
270 | } | ||
271 | |||
272 | |||
273 | [StructLayout(LayoutKind.Sequential)] | ||
274 | public struct JointFeedback | ||
275 | { | ||
276 | public Vector3 f1; | ||
277 | public Vector3 t1; | ||
278 | public Vector3 f2; | ||
279 | public Vector3 t2; | ||
280 | } | ||
281 | |||
282 | |||
283 | [StructLayout(LayoutKind.Sequential)] | ||
284 | public struct Mass | ||
285 | { | ||
286 | public dReal mass; | ||
287 | public Vector4 c; | ||
288 | public Matrix3 I; | ||
289 | } | ||
290 | |||
291 | |||
292 | [StructLayout(LayoutKind.Sequential)] | ||
293 | public struct Matrix3 | ||
294 | { | ||
295 | public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) | ||
296 | { | ||
297 | M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; | ||
298 | M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; | ||
299 | M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; | ||
300 | } | ||
301 | public dReal M00, M10, M20; | ||
302 | private dReal _m30; | ||
303 | public dReal M01, M11, M21; | ||
304 | private dReal _m31; | ||
305 | public dReal M02, M12, M22; | ||
306 | private dReal _m32; | ||
307 | } | ||
308 | |||
309 | [StructLayout(LayoutKind.Sequential)] | ||
310 | public struct Matrix4 | ||
311 | { | ||
312 | public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, | ||
313 | dReal m01, dReal m11, dReal m21, dReal m31, | ||
314 | dReal m02, dReal m12, dReal m22, dReal m32, | ||
315 | dReal m03, dReal m13, dReal m23, dReal m33) | ||
316 | { | ||
317 | M00 = m00; M10 = m10; M20 = m20; M30 = m30; | ||
318 | M01 = m01; M11 = m11; M21 = m21; M31 = m31; | ||
319 | M02 = m02; M12 = m12; M22 = m22; M32 = m32; | ||
320 | M03 = m03; M13 = m13; M23 = m23; M33 = m33; | ||
321 | } | ||
322 | public dReal M00, M10, M20, M30; | ||
323 | public dReal M01, M11, M21, M31; | ||
324 | public dReal M02, M12, M22, M32; | ||
325 | public dReal M03, M13, M23, M33; | ||
326 | } | ||
327 | |||
328 | [StructLayout(LayoutKind.Sequential)] | ||
329 | public struct Quaternion | ||
330 | { | ||
331 | public dReal W, X, Y, Z; | ||
332 | } | ||
333 | |||
334 | |||
335 | [StructLayout(LayoutKind.Sequential)] | ||
336 | public struct SurfaceParameters | ||
337 | { | ||
338 | public ContactFlags mode; | ||
339 | public dReal mu; | ||
340 | public dReal mu2; | ||
341 | public dReal bounce; | ||
342 | public dReal bounce_vel; | ||
343 | public dReal soft_erp; | ||
344 | public dReal soft_cfm; | ||
345 | public dReal motion1; | ||
346 | public dReal motion2; | ||
347 | public dReal motionN; | ||
348 | public dReal slip1; | ||
349 | public dReal slip2; | ||
350 | } | ||
351 | |||
352 | |||
353 | [StructLayout(LayoutKind.Sequential)] | ||
354 | public struct Vector3 | ||
355 | { | ||
356 | public Vector3(dReal x, dReal y, dReal z) | ||
357 | { | ||
358 | X = x; Y = y; Z = z; _w = 0.0f; | ||
359 | } | ||
360 | public dReal X, Y, Z; | ||
361 | private dReal _w; | ||
362 | } | ||
363 | |||
364 | |||
365 | [StructLayout(LayoutKind.Sequential)] | ||
366 | public struct Vector4 | ||
367 | { | ||
368 | public Vector4(dReal x, dReal y, dReal z, dReal w) | ||
369 | { | ||
370 | X = x; Y = y; Z = z; W = w; | ||
371 | } | ||
372 | public dReal X, Y, Z, W; | ||
373 | } | ||
374 | |||
375 | #endregion | ||
376 | |||
377 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] | ||
378 | public static extern int AllocateODEDataForThread(uint ODEInitFlags); | ||
379 | |||
380 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] | ||
381 | public static extern bool AreConnected(IntPtr b1, IntPtr b2); | ||
382 | |||
383 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] | ||
384 | public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); | ||
385 | |||
386 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] | ||
387 | public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); | ||
388 | |||
389 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] | ||
390 | public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); | ||
391 | |||
392 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] | ||
393 | public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); | ||
394 | |||
395 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] | ||
396 | public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); | ||
397 | |||
398 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] | ||
399 | public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); | ||
400 | |||
401 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] | ||
402 | public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); | ||
403 | |||
404 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] | ||
405 | public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); | ||
406 | |||
407 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] | ||
408 | public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); | ||
409 | |||
410 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] | ||
411 | public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); | ||
412 | |||
413 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] | ||
414 | public static extern void BodyCopyPosition(IntPtr body, out dReal X); | ||
415 | |||
416 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] | ||
417 | public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); | ||
418 | |||
419 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] | ||
420 | public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); | ||
421 | |||
422 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] | ||
423 | public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); | ||
424 | |||
425 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] | ||
426 | public static extern void BodyCopyRotation(IntPtr body, out dReal M00); | ||
427 | |||
428 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] | ||
429 | public static extern IntPtr BodyiCreate(IntPtr world); | ||
430 | public static IntPtr BodyCreate(IntPtr world) | ||
431 | { | ||
432 | NTotalBodies++; | ||
433 | return BodyiCreate(world); | ||
434 | } | ||
435 | |||
436 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] | ||
437 | public static extern void BodyiDestroy(IntPtr body); | ||
438 | public static void BodyDestroy(IntPtr body) | ||
439 | { | ||
440 | NTotalBodies--; | ||
441 | BodyiDestroy(body); | ||
442 | } | ||
443 | |||
444 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] | ||
445 | public static extern void BodyDisable(IntPtr body); | ||
446 | |||
447 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] | ||
448 | public static extern void BodyEnable(IntPtr body); | ||
449 | |||
450 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] | ||
451 | public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); | ||
452 | |||
453 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] | ||
454 | public static extern bool BodyGetAutoDisableFlag(IntPtr body); | ||
455 | |||
456 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] | ||
457 | public static extern void BodyGetAutoDisableDefaults(IntPtr body); | ||
458 | |||
459 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] | ||
460 | public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); | ||
461 | |||
462 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] | ||
463 | public static extern int BodyGetAutoDisableSteps(IntPtr body); | ||
464 | |||
465 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] | ||
466 | public static extern dReal BodyGetAutoDisableTime(IntPtr body); | ||
467 | |||
468 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] | ||
469 | public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); | ||
470 | public static Vector3 BodyGetAngularVel(IntPtr body) | ||
471 | { | ||
472 | unsafe { return *(BodyGetAngularVelUnsafe(body)); } | ||
473 | } | ||
474 | |||
475 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] | ||
476 | public static extern IntPtr BodyGetData(IntPtr body); | ||
477 | |||
478 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] | ||
479 | public static extern int BodyGetFiniteRotationMode(IntPtr body); | ||
480 | |||
481 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] | ||
482 | public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); | ||
483 | |||
484 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] | ||
485 | public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); | ||
486 | public static Vector3 BodyGetForce(IntPtr body) | ||
487 | { | ||
488 | unsafe { return *(BodyGetForceUnsafe(body)); } | ||
489 | } | ||
490 | |||
491 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] | ||
492 | public static extern bool BodyGetGravityMode(IntPtr body); | ||
493 | |||
494 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] | ||
495 | public static extern int BodyGetGyroscopicMode(IntPtr body); | ||
496 | |||
497 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] | ||
498 | public static extern IntPtr BodyGetJoint(IntPtr body, int index); | ||
499 | |||
500 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] | ||
501 | public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); | ||
502 | public static Vector3 BodyGetLinearVel(IntPtr body) | ||
503 | { | ||
504 | unsafe { return *(BodyGetLinearVelUnsafe(body)); } | ||
505 | } | ||
506 | |||
507 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] | ||
508 | public static extern void BodyGetMass(IntPtr body, out Mass mass); | ||
509 | |||
510 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] | ||
511 | public static extern int BodyGetNumJoints(IntPtr body); | ||
512 | |||
513 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] | ||
514 | public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
515 | |||
516 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] | ||
517 | public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); | ||
518 | public static Vector3 BodyGetPosition(IntPtr body) | ||
519 | { | ||
520 | unsafe { return *(BodyGetPositionUnsafe(body)); } | ||
521 | } | ||
522 | |||
523 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] | ||
524 | public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
525 | |||
526 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
527 | public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); | ||
528 | public static Quaternion BodyGetQuaternion(IntPtr body) | ||
529 | { | ||
530 | unsafe { return *(BodyGetQuaternionUnsafe(body)); } | ||
531 | } | ||
532 | |||
533 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] | ||
534 | public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
535 | |||
536 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] | ||
537 | public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
538 | |||
539 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] | ||
540 | public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); | ||
541 | public static Matrix3 BodyGetRotation(IntPtr body) | ||
542 | { | ||
543 | unsafe { return *(BodyGetRotationUnsafe(body)); } | ||
544 | } | ||
545 | |||
546 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] | ||
547 | public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); | ||
548 | public static Vector3 BodyGetTorque(IntPtr body) | ||
549 | { | ||
550 | unsafe { return *(BodyGetTorqueUnsafe(body)); } | ||
551 | } | ||
552 | |||
553 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] | ||
554 | public static extern IntPtr BodyGetWorld(IntPtr body); | ||
555 | |||
556 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] | ||
557 | public static extern IntPtr BodyGetFirstGeom(IntPtr body); | ||
558 | |||
559 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] | ||
560 | public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); | ||
561 | |||
562 | |||
563 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] | ||
564 | public static extern bool BodyIsEnabled(IntPtr body); | ||
565 | |||
566 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] | ||
567 | public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); | ||
568 | |||
569 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] | ||
570 | public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); | ||
571 | |||
572 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] | ||
573 | public static extern void BodySetAutoDisableDefaults(IntPtr body); | ||
574 | |||
575 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] | ||
576 | public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); | ||
577 | |||
578 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] | ||
579 | public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); | ||
580 | |||
581 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] | ||
582 | public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); | ||
583 | |||
584 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] | ||
585 | public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); | ||
586 | |||
587 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] | ||
588 | public static extern void BodySetData(IntPtr body, IntPtr data); | ||
589 | |||
590 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] | ||
591 | public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); | ||
592 | |||
593 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] | ||
594 | public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); | ||
595 | |||
596 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] | ||
597 | public static extern void BodySetLinearDamping(IntPtr body, dReal scale); | ||
598 | |||
599 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] | ||
600 | public static extern void BodySetAngularDamping(IntPtr body, dReal scale); | ||
601 | |||
602 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] | ||
603 | public static extern dReal BodyGetLinearDamping(IntPtr body); | ||
604 | |||
605 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] | ||
606 | public static extern dReal BodyGetAngularDamping(IntPtr body); | ||
607 | |||
608 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] | ||
609 | public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); | ||
610 | |||
611 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
612 | public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); | ||
613 | |||
614 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
615 | public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); | ||
616 | |||
617 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
618 | public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); | ||
619 | |||
620 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
621 | public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); | ||
622 | |||
623 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] | ||
624 | public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); | ||
625 | |||
626 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] | ||
627 | public static extern void BodySetGravityMode(IntPtr body, bool mode); | ||
628 | |||
629 | /// <summary> | ||
630 | /// Sets the Gyroscopic term status on the body specified. | ||
631 | /// </summary> | ||
632 | /// <param name="body">Pointer to body</param> | ||
633 | /// <param name="enabled">NonZero enabled, Zero disabled</param> | ||
634 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] | ||
635 | public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); | ||
636 | |||
637 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] | ||
638 | public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); | ||
639 | |||
640 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] | ||
641 | public static extern void BodySetMass(IntPtr body, ref Mass mass); | ||
642 | |||
643 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] | ||
644 | public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); | ||
645 | |||
646 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
647 | public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); | ||
648 | |||
649 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
650 | public static extern void BodySetQuaternion(IntPtr body, ref dReal w); | ||
651 | |||
652 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] | ||
653 | public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); | ||
654 | |||
655 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] | ||
656 | public static extern void BodySetRotation(IntPtr body, ref dReal M00); | ||
657 | |||
658 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] | ||
659 | public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); | ||
660 | |||
661 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] | ||
662 | public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
663 | |||
664 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] | ||
665 | public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); | ||
666 | |||
667 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] | ||
668 | public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, | ||
669 | ref Vector3 side1, ref Vector3 p2, | ||
670 | ref Matrix3 R2, ref Vector3 side2, | ||
671 | ref Vector3 normal, out dReal depth, out int return_code, | ||
672 | int maxc, out ContactGeom contact, int skip); | ||
673 | |||
674 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] | ||
675 | public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, | ||
676 | ref Vector3 side1, ref Vector3 _p2, | ||
677 | ref Matrix3 R2, ref Vector3 side2); | ||
678 | |||
679 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] | ||
680 | public static extern void CleanupODEAllDataForThread(); | ||
681 | |||
682 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] | ||
683 | public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, | ||
684 | ref Vector3 b1, ref Vector3 b2, | ||
685 | ref Vector3 cp1, ref Vector3 cp2); | ||
686 | |||
687 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] | ||
688 | public static extern void CloseODE(); | ||
689 | |||
690 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] | ||
691 | public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); | ||
692 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] | ||
693 | public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); | ||
694 | |||
695 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] | ||
696 | public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); | ||
697 | |||
698 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] | ||
699 | public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); | ||
700 | public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) | ||
701 | { | ||
702 | NTotalGeoms++; | ||
703 | return CreateiBox(space, lx, ly, lz); | ||
704 | } | ||
705 | |||
706 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] | ||
707 | public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); | ||
708 | public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) | ||
709 | { | ||
710 | NTotalGeoms++; | ||
711 | return CreateiCapsule(space, radius, length); | ||
712 | } | ||
713 | |||
714 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] | ||
715 | public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); | ||
716 | public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) | ||
717 | { | ||
718 | NTotalGeoms++; | ||
719 | return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); | ||
720 | } | ||
721 | |||
722 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] | ||
723 | public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); | ||
724 | public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) | ||
725 | { | ||
726 | NTotalGeoms++; | ||
727 | return CreateiCylinder(space, radius, length); | ||
728 | } | ||
729 | |||
730 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] | ||
731 | public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); | ||
732 | public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) | ||
733 | { | ||
734 | NTotalGeoms++; | ||
735 | return CreateiHeightfield(space, data, bPlaceable); | ||
736 | } | ||
737 | |||
738 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateUbitTerrain"), SuppressUnmanagedCodeSecurity] | ||
739 | public static extern IntPtr CreateiUbitTerrain(IntPtr space, IntPtr data, int bPlaceable); | ||
740 | public static IntPtr CreateUbitTerrain(IntPtr space, IntPtr data, int bPlaceable) | ||
741 | { | ||
742 | NTotalGeoms++; | ||
743 | return CreateiUbitTerrain(space, data, bPlaceable); | ||
744 | } | ||
745 | |||
746 | |||
747 | |||
748 | |||
749 | |||
750 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] | ||
751 | public static extern IntPtr CreateiGeom(int classnum); | ||
752 | public static IntPtr CreateGeom(int classnum) | ||
753 | { | ||
754 | NTotalGeoms++; | ||
755 | return CreateiGeom(classnum); | ||
756 | } | ||
757 | |||
758 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] | ||
759 | public static extern int CreateGeomClass(ref GeomClass classptr); | ||
760 | |||
761 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] | ||
762 | public static extern IntPtr CreateGeomTransform(IntPtr space); | ||
763 | |||
764 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] | ||
765 | public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); | ||
766 | public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) | ||
767 | { | ||
768 | NTotalGeoms++; | ||
769 | return CreateiPlane(space, a, b, c, d); | ||
770 | } | ||
771 | |||
772 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] | ||
773 | public static extern IntPtr CreateiRay(IntPtr space, dReal length); | ||
774 | public static IntPtr CreateRay(IntPtr space, dReal length) | ||
775 | { | ||
776 | NTotalGeoms++; | ||
777 | return CreateiRay(space, length); | ||
778 | } | ||
779 | |||
780 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] | ||
781 | public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); | ||
782 | public static IntPtr CreateSphere(IntPtr space, dReal radius) | ||
783 | { | ||
784 | NTotalGeoms++; | ||
785 | return CreateiSphere(space, radius); | ||
786 | } | ||
787 | |||
788 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] | ||
789 | public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, | ||
790 | TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); | ||
791 | public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, | ||
792 | TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) | ||
793 | { | ||
794 | NTotalGeoms++; | ||
795 | return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); | ||
796 | } | ||
797 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] | ||
798 | public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); | ||
799 | |||
800 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] | ||
801 | public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); | ||
802 | |||
803 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] | ||
804 | public static extern int FactorCholesky(ref dReal A00, int n); | ||
805 | |||
806 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] | ||
807 | public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); | ||
808 | |||
809 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] | ||
810 | public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); | ||
811 | |||
812 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] | ||
813 | public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); | ||
814 | |||
815 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] | ||
816 | public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); | ||
817 | |||
818 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] | ||
819 | public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); | ||
820 | |||
821 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] | ||
822 | public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); | ||
823 | |||
824 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] | ||
825 | public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); | ||
826 | |||
827 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] | ||
828 | public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); | ||
829 | |||
830 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] | ||
831 | public static extern void GeomClearOffset(IntPtr geom); | ||
832 | |||
833 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] | ||
834 | public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); | ||
835 | |||
836 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] | ||
837 | public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); | ||
838 | |||
839 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
840 | public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); | ||
841 | |||
842 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
843 | public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); | ||
844 | |||
845 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] | ||
846 | public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); | ||
847 | |||
848 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] | ||
849 | public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); | ||
850 | |||
851 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] | ||
852 | public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); | ||
853 | |||
854 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] | ||
855 | public static extern void GeomCopyPosition(IntPtr geom, out dReal X); | ||
856 | |||
857 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] | ||
858 | public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); | ||
859 | |||
860 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] | ||
861 | public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); | ||
862 | |||
863 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] | ||
864 | public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); | ||
865 | |||
866 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] | ||
867 | public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); | ||
868 | |||
869 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] | ||
870 | public static extern void GeomiDestroy(IntPtr geom); | ||
871 | public static void GeomDestroy(IntPtr geom) | ||
872 | { | ||
873 | NTotalGeoms--; | ||
874 | GeomiDestroy(geom); | ||
875 | } | ||
876 | |||
877 | |||
878 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] | ||
879 | public static extern void GeomDisable(IntPtr geom); | ||
880 | |||
881 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] | ||
882 | public static extern void GeomEnable(IntPtr geom); | ||
883 | |||
884 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] | ||
885 | public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); | ||
886 | |||
887 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] | ||
888 | public static extern void GeomGetAABB(IntPtr geom, out dReal minX); | ||
889 | |||
890 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] | ||
891 | public static extern IntPtr GeomGetBody(IntPtr geom); | ||
892 | |||
893 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] | ||
894 | public static extern uint GeomGetCategoryBits(IntPtr geom); | ||
895 | |||
896 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] | ||
897 | public static extern IntPtr GeomGetClassData(IntPtr geom); | ||
898 | |||
899 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] | ||
900 | public static extern uint GeomGetCollideBits(IntPtr geom); | ||
901 | |||
902 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] | ||
903 | public static extern GeomClassID GeomGetClass(IntPtr geom); | ||
904 | |||
905 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] | ||
906 | public static extern IntPtr GeomGetData(IntPtr geom); | ||
907 | |||
908 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] | ||
909 | public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); | ||
910 | public static Vector3 GeomGetOffsetPosition(IntPtr geom) | ||
911 | { | ||
912 | unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } | ||
913 | } | ||
914 | |||
915 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] | ||
916 | public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); | ||
917 | public static Matrix3 GeomGetOffsetRotation(IntPtr geom) | ||
918 | { | ||
919 | unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } | ||
920 | } | ||
921 | |||
922 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] | ||
923 | public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); | ||
924 | public static Vector3 GeomGetPosition(IntPtr geom) | ||
925 | { | ||
926 | unsafe { return *(GeomGetPositionUnsafe(geom)); } | ||
927 | } | ||
928 | public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) | ||
929 | { | ||
930 | Vector3 vtmp = GeomGetPosition(geom); | ||
931 | return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); | ||
932 | } | ||
933 | |||
934 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
935 | public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); | ||
936 | public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) | ||
937 | { | ||
938 | Quaternion qtmp; | ||
939 | GeomCopyQuaternion(geom, out qtmp); | ||
940 | return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); | ||
941 | } | ||
942 | |||
943 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
944 | public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); | ||
945 | |||
946 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] | ||
947 | public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); | ||
948 | public static Matrix3 GeomGetRotation(IntPtr geom) | ||
949 | { | ||
950 | unsafe { return *(GeomGetRotationUnsafe(geom)); } | ||
951 | } | ||
952 | |||
953 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] | ||
954 | public static extern IntPtr GeomGetSpace(IntPtr geom); | ||
955 | |||
956 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] | ||
957 | public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, | ||
958 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
959 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
960 | |||
961 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] | ||
962 | public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, | ||
963 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
964 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
965 | |||
966 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] | ||
967 | public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, | ||
968 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
969 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
970 | |||
971 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] | ||
972 | public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, | ||
973 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
974 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
975 | |||
976 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] | ||
977 | public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, | ||
978 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
979 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
980 | |||
981 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] | ||
982 | public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, | ||
983 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
984 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
985 | |||
986 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] | ||
987 | public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, | ||
988 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
989 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
990 | |||
991 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] | ||
992 | public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, | ||
993 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
994 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
995 | |||
996 | |||
997 | |||
998 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] | ||
999 | public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, | ||
1000 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
1001 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
1002 | |||
1003 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] | ||
1004 | public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, | ||
1005 | dReal width, dReal depth, int widthSamples, int depthSamples, | ||
1006 | dReal scale, dReal offset, dReal thickness, int bWrap); | ||
1007 | |||
1008 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] | ||
1009 | public static extern IntPtr GeomHeightfieldDataCreate(); | ||
1010 | |||
1011 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] | ||
1012 | public static extern void GeomHeightfieldDataDestroy(IntPtr d); | ||
1013 | |||
1014 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] | ||
1015 | public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); | ||
1016 | |||
1017 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] | ||
1018 | public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); | ||
1019 | |||
1020 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] | ||
1021 | public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); | ||
1022 | |||
1023 | |||
1024 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] | ||
1025 | public static extern void GeomUbitTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, | ||
1026 | dReal sampleSize, int widthSamples, int depthSamples, | ||
1027 | dReal offset, dReal thickness, int bWrap); | ||
1028 | |||
1029 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] | ||
1030 | public static extern void GeomUbitTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, | ||
1031 | dReal sampleSize, int widthSamples, int depthSamples, | ||
1032 | dReal thickness, int bWrap); | ||
1033 | |||
1034 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataCreate"), SuppressUnmanagedCodeSecurity] | ||
1035 | public static extern IntPtr GeomUbitTerrainDataCreate(); | ||
1036 | |||
1037 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] | ||
1038 | public static extern void GeomUbitTerrainDataDestroy(IntPtr d); | ||
1039 | |||
1040 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] | ||
1041 | public static extern void GeomUbitTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); | ||
1042 | |||
1043 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] | ||
1044 | public static extern IntPtr GeomUbitTerrainGetHeightfieldData(IntPtr g); | ||
1045 | |||
1046 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomUbitTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] | ||
1047 | public static extern void GeomUbitTerrainSetHeightfieldData(IntPtr g, IntPtr d); | ||
1048 | |||
1049 | |||
1050 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] | ||
1051 | public static extern bool GeomIsEnabled(IntPtr geom); | ||
1052 | |||
1053 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] | ||
1054 | public static extern bool GeomIsOffset(IntPtr geom); | ||
1055 | |||
1056 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] | ||
1057 | public static extern bool GeomIsSpace(IntPtr geom); | ||
1058 | |||
1059 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] | ||
1060 | public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); | ||
1061 | |||
1062 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] | ||
1063 | public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); | ||
1064 | |||
1065 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] | ||
1066 | public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); | ||
1067 | |||
1068 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] | ||
1069 | public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); | ||
1070 | |||
1071 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] | ||
1072 | public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); | ||
1073 | |||
1074 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] | ||
1075 | public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); | ||
1076 | |||
1077 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] | ||
1078 | public static extern int GeomRayGetClosestHit(IntPtr ray); | ||
1079 | |||
1080 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] | ||
1081 | public static extern dReal GeomRayGetLength(IntPtr ray); | ||
1082 | |||
1083 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] | ||
1084 | public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); | ||
1085 | |||
1086 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] | ||
1087 | public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); | ||
1088 | |||
1089 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] | ||
1090 | public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); | ||
1091 | |||
1092 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] | ||
1093 | public static extern void GeomRaySetLength(IntPtr ray, dReal length); | ||
1094 | |||
1095 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] | ||
1096 | public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); | ||
1097 | |||
1098 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] | ||
1099 | public static extern void GeomSetBody(IntPtr geom, IntPtr body); | ||
1100 | |||
1101 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] | ||
1102 | public static extern void GeomSetCategoryBits(IntPtr geom, uint bits); | ||
1103 | |||
1104 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] | ||
1105 | public static extern void GeomSetCollideBits(IntPtr geom, uint bits); | ||
1106 | |||
1107 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] | ||
1108 | public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); | ||
1109 | |||
1110 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] | ||
1111 | public static extern void GeomSetData(IntPtr geom, IntPtr data); | ||
1112 | |||
1113 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] | ||
1114 | public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); | ||
1115 | |||
1116 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1117 | public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); | ||
1118 | |||
1119 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1120 | public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); | ||
1121 | |||
1122 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] | ||
1123 | public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); | ||
1124 | |||
1125 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] | ||
1126 | public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); | ||
1127 | |||
1128 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] | ||
1129 | public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); | ||
1130 | |||
1131 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1132 | public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); | ||
1133 | |||
1134 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1135 | public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); | ||
1136 | |||
1137 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] | ||
1138 | public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); | ||
1139 | |||
1140 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] | ||
1141 | public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); | ||
1142 | |||
1143 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] | ||
1144 | public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); | ||
1145 | |||
1146 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1147 | public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); | ||
1148 | |||
1149 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] | ||
1150 | public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); | ||
1151 | |||
1152 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] | ||
1153 | public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); | ||
1154 | |||
1155 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] | ||
1156 | public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); | ||
1157 | |||
1158 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] | ||
1159 | public static extern dReal GeomSphereGetRadius(IntPtr geom); | ||
1160 | |||
1161 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] | ||
1162 | public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); | ||
1163 | |||
1164 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] | ||
1165 | public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); | ||
1166 | |||
1167 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] | ||
1168 | public static extern int GeomTransformGetCleanup(IntPtr geom); | ||
1169 | |||
1170 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] | ||
1171 | public static extern IntPtr GeomTransformGetGeom(IntPtr geom); | ||
1172 | |||
1173 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] | ||
1174 | public static extern int GeomTransformGetInfo(IntPtr geom); | ||
1175 | |||
1176 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] | ||
1177 | public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); | ||
1178 | |||
1179 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] | ||
1180 | public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); | ||
1181 | |||
1182 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] | ||
1183 | public static extern void GeomTransformSetInfo(IntPtr geom, int info); | ||
1184 | |||
1185 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] | ||
1186 | public static extern void GeomTriMeshDataBuildDouble(IntPtr d, | ||
1187 | double[] vertices, int vertexStride, int vertexCount, | ||
1188 | int[] indices, int indexCount, int triStride); | ||
1189 | |||
1190 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] | ||
1191 | public static extern void GeomTriMeshDataBuildDouble(IntPtr d, | ||
1192 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1193 | IntPtr indices, int indexCount, int triStride); | ||
1194 | |||
1195 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] | ||
1196 | public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, | ||
1197 | double[] vertices, int vertexStride, int vertexCount, | ||
1198 | int[] indices, int indexCount, int triStride, | ||
1199 | double[] normals); | ||
1200 | |||
1201 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] | ||
1202 | public static extern void GeomTriMeshDataBuildDouble(IntPtr d, | ||
1203 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1204 | IntPtr indices, int indexCount, int triStride, | ||
1205 | IntPtr normals); | ||
1206 | |||
1207 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] | ||
1208 | public static extern void GeomTriMeshDataBuildSingle(IntPtr d, | ||
1209 | dReal[] vertices, int vertexStride, int vertexCount, | ||
1210 | int[] indices, int indexCount, int triStride); | ||
1211 | |||
1212 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] | ||
1213 | public static extern void GeomTriMeshDataBuildSingle(IntPtr d, | ||
1214 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1215 | IntPtr indices, int indexCount, int triStride); | ||
1216 | |||
1217 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] | ||
1218 | public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, | ||
1219 | dReal[] vertices, int vertexStride, int vertexCount, | ||
1220 | int[] indices, int indexCount, int triStride, | ||
1221 | dReal[] normals); | ||
1222 | |||
1223 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] | ||
1224 | public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, | ||
1225 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1226 | IntPtr indices, int indexCount, int triStride, | ||
1227 | IntPtr normals); | ||
1228 | |||
1229 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] | ||
1230 | public static extern void GeomTriMeshDataBuildSimple(IntPtr d, | ||
1231 | float[] vertices, int vertexStride, int vertexCount, | ||
1232 | int[] indices, int indexCount, int triStride); | ||
1233 | |||
1234 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] | ||
1235 | public static extern void GeomTriMeshDataBuildSimple(IntPtr d, | ||
1236 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1237 | IntPtr indices, int indexCount, int triStride); | ||
1238 | |||
1239 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] | ||
1240 | public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, | ||
1241 | float[] vertices, int vertexStride, int vertexCount, | ||
1242 | int[] indices, int indexCount, int triStride, | ||
1243 | float[] normals); | ||
1244 | |||
1245 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] | ||
1246 | public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, | ||
1247 | IntPtr vertices, int vertexStride, int vertexCount, | ||
1248 | IntPtr indices, int indexCount, int triStride, | ||
1249 | IntPtr normals); | ||
1250 | |||
1251 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] | ||
1252 | public static extern void GeomTriMeshClearTCCache(IntPtr g); | ||
1253 | |||
1254 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] | ||
1255 | public static extern IntPtr GeomTriMeshDataCreate(); | ||
1256 | |||
1257 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] | ||
1258 | public static extern void GeomTriMeshDataDestroy(IntPtr d); | ||
1259 | |||
1260 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] | ||
1261 | public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); | ||
1262 | |||
1263 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] | ||
1264 | public static extern void GeomTriMeshDataPreprocess(IntPtr d); | ||
1265 | |||
1266 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] | ||
1267 | public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); | ||
1268 | |||
1269 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] | ||
1270 | public static extern void GeomTriMeshDataUpdate(IntPtr d); | ||
1271 | |||
1272 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] | ||
1273 | public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); | ||
1274 | |||
1275 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] | ||
1276 | public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); | ||
1277 | |||
1278 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] | ||
1279 | public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); | ||
1280 | |||
1281 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] | ||
1282 | public static extern IntPtr GeomTriMeshGetData(IntPtr g); | ||
1283 | |||
1284 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] | ||
1285 | public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); | ||
1286 | public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) | ||
1287 | { | ||
1288 | unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } | ||
1289 | } | ||
1290 | |||
1291 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] | ||
1292 | public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); | ||
1293 | |||
1294 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] | ||
1295 | public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); | ||
1296 | |||
1297 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] | ||
1298 | public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); | ||
1299 | |||
1300 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] | ||
1301 | public extern static int GeomTriMeshGetTriangleCount(IntPtr g); | ||
1302 | |||
1303 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] | ||
1304 | public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); | ||
1305 | |||
1306 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] | ||
1307 | public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); | ||
1308 | |||
1309 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] | ||
1310 | public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); | ||
1311 | |||
1312 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] | ||
1313 | public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); | ||
1314 | |||
1315 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] | ||
1316 | public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); | ||
1317 | |||
1318 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] | ||
1319 | public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); | ||
1320 | |||
1321 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] | ||
1322 | public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); | ||
1323 | |||
1324 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] | ||
1325 | public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); | ||
1326 | |||
1327 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] | ||
1328 | public static extern IntPtr iGetConfiguration(); | ||
1329 | |||
1330 | public static string GetConfiguration() | ||
1331 | { | ||
1332 | IntPtr ptr = iGetConfiguration(); | ||
1333 | string s = Marshal.PtrToStringAnsi(ptr); | ||
1334 | return s; | ||
1335 | } | ||
1336 | |||
1337 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] | ||
1338 | public static extern IntPtr HashSpaceCreate(IntPtr space); | ||
1339 | |||
1340 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] | ||
1341 | public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); | ||
1342 | |||
1343 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] | ||
1344 | public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); | ||
1345 | |||
1346 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] | ||
1347 | public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); | ||
1348 | |||
1349 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] | ||
1350 | public static extern void InitODE(); | ||
1351 | |||
1352 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] | ||
1353 | public static extern int InitODE2(uint ODEInitFlags); | ||
1354 | |||
1355 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] | ||
1356 | public static extern int IsPositiveDefinite(ref dReal A, int n); | ||
1357 | |||
1358 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] | ||
1359 | public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); | ||
1360 | |||
1361 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] | ||
1362 | public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); | ||
1363 | |||
1364 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] | ||
1365 | public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); | ||
1366 | |||
1367 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] | ||
1368 | public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); | ||
1369 | |||
1370 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] | ||
1371 | public static extern void JointAddPRTorque(IntPtr joint, dReal torque); | ||
1372 | |||
1373 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] | ||
1374 | public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); | ||
1375 | |||
1376 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] | ||
1377 | public static extern void JointAddSliderForce(IntPtr joint, dReal force); | ||
1378 | |||
1379 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] | ||
1380 | public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); | ||
1381 | |||
1382 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] | ||
1383 | public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); | ||
1384 | |||
1385 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] | ||
1386 | public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); | ||
1387 | |||
1388 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] | ||
1389 | public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); | ||
1390 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] | ||
1391 | public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); | ||
1392 | |||
1393 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] | ||
1394 | public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); | ||
1395 | |||
1396 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] | ||
1397 | public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); | ||
1398 | |||
1399 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] | ||
1400 | public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); | ||
1401 | |||
1402 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] | ||
1403 | public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); | ||
1404 | |||
1405 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] | ||
1406 | public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); | ||
1407 | |||
1408 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] | ||
1409 | public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); | ||
1410 | |||
1411 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] | ||
1412 | public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); | ||
1413 | |||
1414 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] | ||
1415 | public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); | ||
1416 | |||
1417 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] | ||
1418 | public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); | ||
1419 | |||
1420 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] | ||
1421 | public static extern void JointDestroy(IntPtr j); | ||
1422 | |||
1423 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] | ||
1424 | public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); | ||
1425 | |||
1426 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] | ||
1427 | public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); | ||
1428 | |||
1429 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] | ||
1430 | public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); | ||
1431 | |||
1432 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] | ||
1433 | public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); | ||
1434 | |||
1435 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] | ||
1436 | public static extern int JointGetAMotorMode(IntPtr j); | ||
1437 | |||
1438 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] | ||
1439 | public static extern int JointGetAMotorNumAxes(IntPtr j); | ||
1440 | |||
1441 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] | ||
1442 | public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); | ||
1443 | |||
1444 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] | ||
1445 | public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); | ||
1446 | |||
1447 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] | ||
1448 | public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); | ||
1449 | |||
1450 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] | ||
1451 | public static extern IntPtr JointGetBody(IntPtr j); | ||
1452 | |||
1453 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] | ||
1454 | public static extern IntPtr JointGetData(IntPtr j); | ||
1455 | |||
1456 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] | ||
1457 | public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); | ||
1458 | public static JointFeedback JointGetFeedback(IntPtr j) | ||
1459 | { | ||
1460 | unsafe { return *(JointGetFeedbackUnsafe(j)); } | ||
1461 | } | ||
1462 | |||
1463 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] | ||
1464 | public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); | ||
1465 | |||
1466 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] | ||
1467 | public static extern dReal JointGetHingeAngle(IntPtr j); | ||
1468 | |||
1469 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] | ||
1470 | public static extern dReal JointGetHingeAngleRate(IntPtr j); | ||
1471 | |||
1472 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] | ||
1473 | public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); | ||
1474 | |||
1475 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] | ||
1476 | public static extern dReal JointGetHingeParam(IntPtr j, int parameter); | ||
1477 | |||
1478 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] | ||
1479 | public static extern dReal JointGetHinge2Angle1(IntPtr j); | ||
1480 | |||
1481 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] | ||
1482 | public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); | ||
1483 | |||
1484 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] | ||
1485 | public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); | ||
1486 | |||
1487 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] | ||
1488 | public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); | ||
1489 | |||
1490 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] | ||
1491 | public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); | ||
1492 | |||
1493 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] | ||
1494 | public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); | ||
1495 | |||
1496 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] | ||
1497 | public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); | ||
1498 | |||
1499 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] | ||
1500 | public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); | ||
1501 | |||
1502 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] | ||
1503 | public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); | ||
1504 | |||
1505 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] | ||
1506 | public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); | ||
1507 | |||
1508 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] | ||
1509 | public static extern int JointGetLMotorNumAxes(IntPtr j); | ||
1510 | |||
1511 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] | ||
1512 | public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); | ||
1513 | |||
1514 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] | ||
1515 | public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); | ||
1516 | |||
1517 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] | ||
1518 | public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); | ||
1519 | |||
1520 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] | ||
1521 | public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); | ||
1522 | |||
1523 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] | ||
1524 | public static extern dReal JointGetPRParam(IntPtr j, int parameter); | ||
1525 | |||
1526 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] | ||
1527 | public static extern dReal JointGetPRPosition(IntPtr j); | ||
1528 | |||
1529 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] | ||
1530 | public static extern dReal JointGetPRPositionRate(IntPtr j); | ||
1531 | |||
1532 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] | ||
1533 | public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); | ||
1534 | |||
1535 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] | ||
1536 | public static extern dReal JointGetSliderParam(IntPtr j, int parameter); | ||
1537 | |||
1538 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] | ||
1539 | public static extern dReal JointGetSliderPosition(IntPtr j); | ||
1540 | |||
1541 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] | ||
1542 | public static extern dReal JointGetSliderPositionRate(IntPtr j); | ||
1543 | |||
1544 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] | ||
1545 | public static extern JointType JointGetType(IntPtr j); | ||
1546 | |||
1547 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] | ||
1548 | public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); | ||
1549 | |||
1550 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] | ||
1551 | public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); | ||
1552 | |||
1553 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] | ||
1554 | public static extern dReal JointGetUniversalAngle1(IntPtr j); | ||
1555 | |||
1556 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] | ||
1557 | public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); | ||
1558 | |||
1559 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] | ||
1560 | public static extern dReal JointGetUniversalAngle2(IntPtr j); | ||
1561 | |||
1562 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] | ||
1563 | public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); | ||
1564 | |||
1565 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] | ||
1566 | public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); | ||
1567 | |||
1568 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] | ||
1569 | public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); | ||
1570 | |||
1571 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] | ||
1572 | public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); | ||
1573 | |||
1574 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] | ||
1575 | public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); | ||
1576 | |||
1577 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] | ||
1578 | public static extern IntPtr JointGroupCreate(int max_size); | ||
1579 | |||
1580 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] | ||
1581 | public static extern void JointGroupDestroy(IntPtr group); | ||
1582 | |||
1583 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] | ||
1584 | public static extern void JointGroupEmpty(IntPtr group); | ||
1585 | |||
1586 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] | ||
1587 | public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); | ||
1588 | |||
1589 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] | ||
1590 | public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); | ||
1591 | |||
1592 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] | ||
1593 | public static extern void JointSetAMotorMode(IntPtr j, int mode); | ||
1594 | |||
1595 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] | ||
1596 | public static extern void JointSetAMotorNumAxes(IntPtr group, int num); | ||
1597 | |||
1598 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] | ||
1599 | public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); | ||
1600 | |||
1601 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] | ||
1602 | public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); | ||
1603 | |||
1604 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] | ||
1605 | public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); | ||
1606 | |||
1607 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] | ||
1608 | public static extern void JointSetData(IntPtr j, IntPtr data); | ||
1609 | |||
1610 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] | ||
1611 | public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); | ||
1612 | |||
1613 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] | ||
1614 | public static extern void JointSetFixed(IntPtr j); | ||
1615 | |||
1616 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] | ||
1617 | public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); | ||
1618 | |||
1619 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] | ||
1620 | public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); | ||
1621 | |||
1622 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] | ||
1623 | public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); | ||
1624 | |||
1625 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] | ||
1626 | public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); | ||
1627 | |||
1628 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] | ||
1629 | public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); | ||
1630 | |||
1631 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] | ||
1632 | public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); | ||
1633 | |||
1634 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] | ||
1635 | public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); | ||
1636 | |||
1637 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] | ||
1638 | public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); | ||
1639 | |||
1640 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] | ||
1641 | public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); | ||
1642 | |||
1643 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] | ||
1644 | public static extern void JointSetLMotorNumAxes(IntPtr j, int num); | ||
1645 | |||
1646 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] | ||
1647 | public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); | ||
1648 | |||
1649 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] | ||
1650 | public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); | ||
1651 | |||
1652 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] | ||
1653 | public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); | ||
1654 | |||
1655 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] | ||
1656 | public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); | ||
1657 | |||
1658 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] | ||
1659 | public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); | ||
1660 | |||
1661 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] | ||
1662 | public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); | ||
1663 | |||
1664 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] | ||
1665 | public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); | ||
1666 | |||
1667 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] | ||
1668 | public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); | ||
1669 | |||
1670 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] | ||
1671 | public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); | ||
1672 | |||
1673 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] | ||
1674 | public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); | ||
1675 | |||
1676 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] | ||
1677 | public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); | ||
1678 | |||
1679 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] | ||
1680 | public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); | ||
1681 | |||
1682 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] | ||
1683 | public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); | ||
1684 | |||
1685 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] | ||
1686 | public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); | ||
1687 | |||
1688 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] | ||
1689 | public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); | ||
1690 | |||
1691 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] | ||
1692 | public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); | ||
1693 | |||
1694 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] | ||
1695 | public static extern void MassAdd(ref Mass a, ref Mass b); | ||
1696 | |||
1697 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] | ||
1698 | public static extern void MassAdjust(ref Mass m, dReal newmass); | ||
1699 | |||
1700 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] | ||
1701 | public static extern bool MassCheck(ref Mass m); | ||
1702 | |||
1703 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] | ||
1704 | public static extern void MassRotate(ref Mass mass, ref Matrix3 R); | ||
1705 | |||
1706 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] | ||
1707 | public static extern void MassRotate(ref Mass mass, ref dReal M00); | ||
1708 | |||
1709 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] | ||
1710 | public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); | ||
1711 | |||
1712 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] | ||
1713 | public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); | ||
1714 | |||
1715 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] | ||
1716 | public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); | ||
1717 | |||
1718 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] | ||
1719 | public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); | ||
1720 | |||
1721 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] | ||
1722 | public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); | ||
1723 | |||
1724 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] | ||
1725 | public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); | ||
1726 | |||
1727 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] | ||
1728 | public static extern void MassSetParameters(out Mass mass, dReal themass, | ||
1729 | dReal cgx, dReal cgy, dReal cgz, | ||
1730 | dReal i11, dReal i22, dReal i33, | ||
1731 | dReal i12, dReal i13, dReal i23); | ||
1732 | |||
1733 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] | ||
1734 | public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); | ||
1735 | |||
1736 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] | ||
1737 | public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); | ||
1738 | |||
1739 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] | ||
1740 | public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); | ||
1741 | |||
1742 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] | ||
1743 | public static extern void MassSetZero(out Mass mass); | ||
1744 | |||
1745 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] | ||
1746 | public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); | ||
1747 | |||
1748 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] | ||
1749 | public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); | ||
1750 | |||
1751 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] | ||
1752 | private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); | ||
1753 | public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) | ||
1754 | { | ||
1755 | MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); | ||
1756 | } | ||
1757 | |||
1758 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] | ||
1759 | public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); | ||
1760 | |||
1761 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] | ||
1762 | public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); | ||
1763 | |||
1764 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] | ||
1765 | public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); | ||
1766 | |||
1767 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] | ||
1768 | public static extern void QfromR(out Quaternion q, ref Matrix3 R); | ||
1769 | |||
1770 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] | ||
1771 | public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); | ||
1772 | |||
1773 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] | ||
1774 | public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); | ||
1775 | |||
1776 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] | ||
1777 | public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); | ||
1778 | |||
1779 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] | ||
1780 | public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); | ||
1781 | |||
1782 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] | ||
1783 | public static extern void QSetIdentity(out Quaternion q); | ||
1784 | |||
1785 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] | ||
1786 | public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); | ||
1787 | |||
1788 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] | ||
1789 | public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); | ||
1790 | |||
1791 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] | ||
1792 | public static extern dReal RandReal(); | ||
1793 | |||
1794 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] | ||
1795 | public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); | ||
1796 | |||
1797 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] | ||
1798 | public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); | ||
1799 | |||
1800 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] | ||
1801 | public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); | ||
1802 | |||
1803 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] | ||
1804 | public static extern void RfromQ(out Matrix3 R, ref Quaternion q); | ||
1805 | |||
1806 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] | ||
1807 | public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); | ||
1808 | |||
1809 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] | ||
1810 | public static extern void RSetIdentity(out Matrix3 R); | ||
1811 | |||
1812 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] | ||
1813 | public static extern void SetValue(out dReal a, int n); | ||
1814 | |||
1815 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] | ||
1816 | public static extern void SetZero(out dReal a, int n); | ||
1817 | |||
1818 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] | ||
1819 | public static extern IntPtr SimpleSpaceCreate(IntPtr space); | ||
1820 | |||
1821 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] | ||
1822 | public static extern void SolveCholesky(ref dReal L, out dReal b, int n); | ||
1823 | |||
1824 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] | ||
1825 | public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); | ||
1826 | |||
1827 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] | ||
1828 | public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); | ||
1829 | |||
1830 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] | ||
1831 | public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); | ||
1832 | |||
1833 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] | ||
1834 | public static extern void SpaceAdd(IntPtr space, IntPtr geom); | ||
1835 | |||
1836 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] | ||
1837 | public static extern bool SpaceLockQuery(IntPtr space); | ||
1838 | |||
1839 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] | ||
1840 | public static extern void SpaceClean(IntPtr space); | ||
1841 | |||
1842 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] | ||
1843 | public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); | ||
1844 | |||
1845 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] | ||
1846 | public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); | ||
1847 | |||
1848 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] | ||
1849 | public static extern void SpaceDestroy(IntPtr space); | ||
1850 | |||
1851 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] | ||
1852 | public static extern bool SpaceGetCleanup(IntPtr space); | ||
1853 | |||
1854 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] | ||
1855 | public static extern int SpaceGetNumGeoms(IntPtr space); | ||
1856 | |||
1857 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] | ||
1858 | public static extern IntPtr SpaceGetGeom(IntPtr space, int i); | ||
1859 | |||
1860 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] | ||
1861 | public static extern int SpaceGetSublevel(IntPtr space); | ||
1862 | |||
1863 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] | ||
1864 | public static extern bool SpaceQuery(IntPtr space, IntPtr geom); | ||
1865 | |||
1866 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] | ||
1867 | public static extern void SpaceRemove(IntPtr space, IntPtr geom); | ||
1868 | |||
1869 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] | ||
1870 | public static extern void SpaceSetCleanup(IntPtr space, bool mode); | ||
1871 | |||
1872 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] | ||
1873 | public static extern void SpaceSetSublevel(IntPtr space, int sublevel); | ||
1874 | |||
1875 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] | ||
1876 | public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); | ||
1877 | |||
1878 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] | ||
1879 | public static extern void VectorScale(out dReal a, ref dReal d, int n); | ||
1880 | |||
1881 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] | ||
1882 | public static extern IntPtr WorldCreate(); | ||
1883 | |||
1884 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] | ||
1885 | public static extern void WorldDestroy(IntPtr world); | ||
1886 | |||
1887 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] | ||
1888 | public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); | ||
1889 | |||
1890 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] | ||
1891 | public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); | ||
1892 | |||
1893 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] | ||
1894 | public static extern bool WorldGetAutoDisableFlag(IntPtr world); | ||
1895 | |||
1896 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] | ||
1897 | public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); | ||
1898 | |||
1899 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] | ||
1900 | public static extern int WorldGetAutoDisableSteps(IntPtr world); | ||
1901 | |||
1902 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] | ||
1903 | public static extern dReal WorldGetAutoDisableTime(IntPtr world); | ||
1904 | |||
1905 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] | ||
1906 | public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); | ||
1907 | |||
1908 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] | ||
1909 | public static extern dReal WorldGetCFM(IntPtr world); | ||
1910 | |||
1911 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] | ||
1912 | public static extern dReal WorldGetERP(IntPtr world); | ||
1913 | |||
1914 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] | ||
1915 | public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); | ||
1916 | |||
1917 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] | ||
1918 | public static extern void WorldGetGravity(IntPtr world, out dReal X); | ||
1919 | |||
1920 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] | ||
1921 | public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); | ||
1922 | |||
1923 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] | ||
1924 | public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); | ||
1925 | |||
1926 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] | ||
1927 | public static extern dReal WorldGetAngularDamping(IntPtr world); | ||
1928 | |||
1929 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
1930 | public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); | ||
1931 | |||
1932 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] | ||
1933 | public static extern dReal WorldGetLinearDamping(IntPtr world); | ||
1934 | |||
1935 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
1936 | public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); | ||
1937 | |||
1938 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] | ||
1939 | public static extern int WorldGetQuickStepNumIterations(IntPtr world); | ||
1940 | |||
1941 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] | ||
1942 | public static extern dReal WorldGetQuickStepW(IntPtr world); | ||
1943 | |||
1944 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] | ||
1945 | public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); | ||
1946 | |||
1947 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] | ||
1948 | public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); | ||
1949 | |||
1950 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] | ||
1951 | public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); | ||
1952 | |||
1953 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] | ||
1954 | public static extern void WorldQuickStep(IntPtr world, dReal stepsize); | ||
1955 | |||
1956 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] | ||
1957 | public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); | ||
1958 | |||
1959 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
1960 | public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); | ||
1961 | |||
1962 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] | ||
1963 | public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); | ||
1964 | |||
1965 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] | ||
1966 | public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); | ||
1967 | |||
1968 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] | ||
1969 | public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); | ||
1970 | |||
1971 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] | ||
1972 | public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); | ||
1973 | |||
1974 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] | ||
1975 | public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); | ||
1976 | |||
1977 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] | ||
1978 | public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); | ||
1979 | |||
1980 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] | ||
1981 | public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); | ||
1982 | |||
1983 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] | ||
1984 | public static extern void WorldSetCFM(IntPtr world, dReal cfm); | ||
1985 | |||
1986 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] | ||
1987 | public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); | ||
1988 | |||
1989 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] | ||
1990 | public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); | ||
1991 | |||
1992 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] | ||
1993 | public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); | ||
1994 | |||
1995 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] | ||
1996 | public static extern void WorldSetERP(IntPtr world, dReal erp); | ||
1997 | |||
1998 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] | ||
1999 | public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); | ||
2000 | |||
2001 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] | ||
2002 | public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); | ||
2003 | |||
2004 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] | ||
2005 | public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); | ||
2006 | |||
2007 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] | ||
2008 | public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); | ||
2009 | |||
2010 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] | ||
2011 | public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); | ||
2012 | |||
2013 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] | ||
2014 | public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); | ||
2015 | |||
2016 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] | ||
2017 | public static extern void WorldStep(IntPtr world, dReal stepsize); | ||
2018 | |||
2019 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] | ||
2020 | public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); | ||
2021 | |||
2022 | [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] | ||
2023 | public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); | ||
2024 | } | ||
2025 | } | ||
diff --git a/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs b/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs new file mode 100644 index 0000000..7735db6 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOde/OdeScene.cs | |||
@@ -0,0 +1,2930 @@ | |||
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/13 by Ubit Umarov | ||
29 | //#define SPAM | ||
30 | |||
31 | using System; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using System.Runtime.InteropServices; | ||
35 | using System.Threading; | ||
36 | using System.IO; | ||
37 | using System.Diagnostics; | ||
38 | using log4net; | ||
39 | using Nini.Config; | ||
40 | using OdeAPI; | ||
41 | using OpenSim.Framework; | ||
42 | using OpenSim.Region.Framework.Interfaces; | ||
43 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
44 | using OpenMetaverse; | ||
45 | |||
46 | namespace OpenSim.Region.PhysicsModules.UbitOde | ||
47 | { | ||
48 | // colision flags of things others can colide with | ||
49 | // rays, sensors, probes removed since can't be colided with | ||
50 | // The top space where things are placed provided further selection | ||
51 | // ie physical are in active space nonphysical in static | ||
52 | // this should be exclusive as possible | ||
53 | |||
54 | [Flags] | ||
55 | public enum CollisionCategories : uint | ||
56 | { | ||
57 | Disabled = 0, | ||
58 | //by 'things' types | ||
59 | Space = 0x01, | ||
60 | Geom = 0x02, // aka prim/part | ||
61 | Character = 0x04, | ||
62 | Land = 0x08, | ||
63 | Water = 0x010, | ||
64 | |||
65 | // by state | ||
66 | Phantom = 0x01000, | ||
67 | VolumeDtc = 0x02000, | ||
68 | Selected = 0x04000, | ||
69 | NoShape = 0x08000, | ||
70 | |||
71 | |||
72 | All = 0xffffffff | ||
73 | } | ||
74 | |||
75 | /// <summary> | ||
76 | /// Material type for a primitive | ||
77 | /// </summary> | ||
78 | public enum Material : int | ||
79 | { | ||
80 | /// <summary></summary> | ||
81 | Stone = 0, | ||
82 | /// <summary></summary> | ||
83 | Metal = 1, | ||
84 | /// <summary></summary> | ||
85 | Glass = 2, | ||
86 | /// <summary></summary> | ||
87 | Wood = 3, | ||
88 | /// <summary></summary> | ||
89 | Flesh = 4, | ||
90 | /// <summary></summary> | ||
91 | Plastic = 5, | ||
92 | /// <summary></summary> | ||
93 | Rubber = 6, | ||
94 | |||
95 | light = 7 // compatibility with old viewers | ||
96 | } | ||
97 | |||
98 | public enum changes : int | ||
99 | { | ||
100 | Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) | ||
101 | Remove, | ||
102 | Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root | ||
103 | // or removes from a object if arg is null | ||
104 | DeLink, | ||
105 | Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child | ||
106 | Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child | ||
107 | PosOffset, // not in use | ||
108 | // arg Vector3 new position in local coords. Changes prim position in object | ||
109 | OriOffset, // not in use | ||
110 | // arg Vector3 new position in local coords. Changes prim position in object | ||
111 | Velocity, | ||
112 | AngVelocity, | ||
113 | Acceleration, | ||
114 | Force, | ||
115 | Torque, | ||
116 | Momentum, | ||
117 | |||
118 | AddForce, | ||
119 | AddAngForce, | ||
120 | AngLock, | ||
121 | |||
122 | Buoyancy, | ||
123 | |||
124 | PIDTarget, | ||
125 | PIDTau, | ||
126 | PIDActive, | ||
127 | |||
128 | PIDHoverHeight, | ||
129 | PIDHoverType, | ||
130 | PIDHoverTau, | ||
131 | PIDHoverActive, | ||
132 | |||
133 | Size, | ||
134 | AvatarSize, | ||
135 | Shape, | ||
136 | PhysRepData, | ||
137 | AddPhysRep, | ||
138 | |||
139 | CollidesWater, | ||
140 | VolumeDtc, | ||
141 | |||
142 | Physical, | ||
143 | Phantom, | ||
144 | Selected, | ||
145 | disabled, | ||
146 | building, | ||
147 | |||
148 | VehicleType, | ||
149 | VehicleFloatParam, | ||
150 | VehicleVectorParam, | ||
151 | VehicleRotationParam, | ||
152 | VehicleFlags, | ||
153 | SetVehicle, | ||
154 | |||
155 | Null //keep this last used do dim the methods array. does nothing but pulsing the prim | ||
156 | } | ||
157 | |||
158 | public struct ODEchangeitem | ||
159 | { | ||
160 | public PhysicsActor actor; | ||
161 | public OdeCharacter character; | ||
162 | public changes what; | ||
163 | public Object arg; | ||
164 | } | ||
165 | |||
166 | |||
167 | |||
168 | public class OdeScene : PhysicsScene, INonSharedRegionModule | ||
169 | { | ||
170 | private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); | ||
171 | // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); | ||
172 | |||
173 | public bool OdeUbitLib = false; | ||
174 | public bool m_suportCombine = false; // mega suport not tested | ||
175 | |||
176 | // private int threadid = 0; | ||
177 | // private Random fluidRandomizer = new Random(Environment.TickCount); | ||
178 | |||
179 | // const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; | ||
180 | |||
181 | const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; | ||
182 | const float comumContactERP = 0.7f; | ||
183 | const float comumContactCFM = 0.0001f; | ||
184 | const float comumContactSLIP = 0f; | ||
185 | |||
186 | float frictionMovementMult = 0.8f; | ||
187 | |||
188 | float TerrainBounce = 0.1f; | ||
189 | float TerrainFriction = 0.3f; | ||
190 | |||
191 | public float AvatarFriction = 0;// 0.9f * 0.5f; | ||
192 | |||
193 | // this netx dimensions are only relevant for terrain partition (mega regions) | ||
194 | // WorldExtents below has the simulation dimensions | ||
195 | // they should be identical except on mega regions | ||
196 | private uint m_regionWidth = Constants.RegionSize; | ||
197 | private uint m_regionHeight = Constants.RegionSize; | ||
198 | |||
199 | public float ODE_STEPSIZE = 0.020f; | ||
200 | public float HalfOdeStep = 0.01f; | ||
201 | public int odetimestepMS = 20; // rounded | ||
202 | private float metersInSpace = 25.6f; | ||
203 | private float m_timeDilation = 1.0f; | ||
204 | |||
205 | private DateTime m_lastframe; | ||
206 | private DateTime m_lastMeshExpire; | ||
207 | |||
208 | public float gravityx = 0f; | ||
209 | public float gravityy = 0f; | ||
210 | public float gravityz = -9.8f; | ||
211 | |||
212 | private float waterlevel = 0f; | ||
213 | private int framecount = 0; | ||
214 | |||
215 | // private int m_meshExpireCntr; | ||
216 | |||
217 | private float avDensity = 3f; | ||
218 | private float avMovementDivisorWalk = 1.3f; | ||
219 | private float avMovementDivisorRun = 0.8f; | ||
220 | private float minimumGroundFlightOffset = 3f; | ||
221 | public float maximumMassObject = 10000.01f; | ||
222 | |||
223 | |||
224 | public float geomDefaultDensity = 10.000006836f; | ||
225 | |||
226 | public float bodyPIDD = 35f; | ||
227 | public float bodyPIDG = 25; | ||
228 | |||
229 | public int bodyFramesAutoDisable = 5; | ||
230 | |||
231 | private d.NearCallback nearCallback; | ||
232 | |||
233 | private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); | ||
234 | private HashSet<OdePrim> _prims = new HashSet<OdePrim>(); | ||
235 | private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); | ||
236 | private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>(); | ||
237 | |||
238 | public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>(); | ||
239 | |||
240 | /// <summary> | ||
241 | /// A list of actors that should receive collision events. | ||
242 | /// </summary> | ||
243 | private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); | ||
244 | private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>(); | ||
245 | |||
246 | private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>(); | ||
247 | // public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); | ||
248 | public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); | ||
249 | |||
250 | private float contactsurfacelayer = 0.001f; | ||
251 | |||
252 | private int contactsPerCollision = 80; | ||
253 | internal IntPtr ContactgeomsArray = IntPtr.Zero; | ||
254 | private IntPtr GlobalContactsArray = IntPtr.Zero; | ||
255 | private d.Contact SharedTmpcontact = new d.Contact(); | ||
256 | |||
257 | const int maxContactsbeforedeath = 4000; | ||
258 | private volatile int m_global_contactcount = 0; | ||
259 | |||
260 | private IntPtr contactgroup; | ||
261 | |||
262 | public ContactData[] m_materialContactsData = new ContactData[8]; | ||
263 | |||
264 | private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>(); | ||
265 | private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>(); | ||
266 | private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>(); | ||
267 | |||
268 | private int m_physicsiterations = 15; | ||
269 | private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag | ||
270 | // private PhysicsActor PANull = new NullPhysicsActor(); | ||
271 | private float step_time = 0.0f; | ||
272 | |||
273 | public IntPtr world; | ||
274 | |||
275 | |||
276 | // split the spaces acording to contents type | ||
277 | // ActiveSpace contains characters and active prims | ||
278 | // StaticSpace contains land and other that is mostly static in enviroment | ||
279 | // this can contain subspaces, like the grid in staticspace | ||
280 | // as now space only contains this 2 top spaces | ||
281 | |||
282 | public IntPtr TopSpace; // the global space | ||
283 | public IntPtr ActiveSpace; // space for active prims | ||
284 | public IntPtr CharsSpace; // space for active prims | ||
285 | public IntPtr StaticSpace; // space for the static things around | ||
286 | public IntPtr GroundSpace; // space for ground | ||
287 | |||
288 | // some speedup variables | ||
289 | private int spaceGridMaxX; | ||
290 | private int spaceGridMaxY; | ||
291 | private float spacesPerMeterX; | ||
292 | private float spacesPerMeterY; | ||
293 | |||
294 | // split static geometry collision into a grid as before | ||
295 | private IntPtr[,] staticPrimspace; | ||
296 | private IntPtr[] staticPrimspaceOffRegion; | ||
297 | |||
298 | public Object OdeLock; | ||
299 | public static Object SimulationLock; | ||
300 | |||
301 | public IMesher mesher; | ||
302 | |||
303 | private IConfigSource m_config; | ||
304 | |||
305 | public bool physics_logging = false; | ||
306 | public int physics_logging_interval = 0; | ||
307 | public bool physics_logging_append_existing_logfile = false; | ||
308 | |||
309 | private Vector3 m_worldOffset = Vector3.Zero; | ||
310 | public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | ||
311 | private PhysicsScene m_parentScene = null; | ||
312 | |||
313 | private ODERayCastRequestManager m_rayCastManager; | ||
314 | public ODEMeshWorker m_meshWorker; | ||
315 | |||
316 | /* maybe needed if ode uses tls | ||
317 | private void checkThread() | ||
318 | { | ||
319 | |||
320 | int th = Thread.CurrentThread.ManagedThreadId; | ||
321 | if(th != threadid) | ||
322 | { | ||
323 | threadid = th; | ||
324 | d.AllocateODEDataForThread(~0U); | ||
325 | } | ||
326 | } | ||
327 | */ | ||
328 | #region INonSharedRegionModule | ||
329 | public string Name | ||
330 | { | ||
331 | get { return "UbitODE"; } | ||
332 | } | ||
333 | |||
334 | public Type ReplaceableInterface | ||
335 | { | ||
336 | get { return null; } | ||
337 | } | ||
338 | |||
339 | public void Initialise(IConfigSource source) | ||
340 | { | ||
341 | // TODO: Move this out of Startup | ||
342 | IConfig config = source.Configs["Startup"]; | ||
343 | if (config != null) | ||
344 | { | ||
345 | string physics = config.GetString("physics", string.Empty); | ||
346 | if (physics == Name) | ||
347 | { | ||
348 | m_Enabled = true; | ||
349 | m_Config = source; | ||
350 | } | ||
351 | } | ||
352 | |||
353 | } | ||
354 | |||
355 | public void Close() | ||
356 | { | ||
357 | } | ||
358 | |||
359 | public void AddRegion(Scene scene) | ||
360 | { | ||
361 | if (!m_Enabled) | ||
362 | return; | ||
363 | |||
364 | EngineType = Name; | ||
365 | RegionName = scene.RegionInfo.RegionName; | ||
366 | PhysicsSceneName = EngineType + "/" + RegionName; | ||
367 | |||
368 | scene.RegisterModuleInterface<PhysicsScene>(this); | ||
369 | Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ); | ||
370 | RawInitialization(); | ||
371 | Initialise(m_Config, extent); | ||
372 | |||
373 | base.Initialise(scene.PhysicsRequestAsset, | ||
374 | (scene.Heightmap != null ? scene.Heightmap.GetFloatsSerialised() : new float[scene.RegionInfo.RegionSizeX * scene.RegionInfo.RegionSizeY]), | ||
375 | (float)scene.RegionInfo.RegionSettings.WaterHeight); | ||
376 | |||
377 | } | ||
378 | |||
379 | public void RemoveRegion(Scene scene) | ||
380 | { | ||
381 | if (!m_Enabled) | ||
382 | return; | ||
383 | } | ||
384 | |||
385 | public void RegionLoaded(Scene scene) | ||
386 | { | ||
387 | if (!m_Enabled) | ||
388 | return; | ||
389 | |||
390 | mesher = scene.RequestModuleInterface<IMesher>(); | ||
391 | if (mesher == null) | ||
392 | m_log.WarnFormat("{0} No mesher. Things will not work well.", LogHeader); | ||
393 | |||
394 | scene.PhysicsEnabled = true; | ||
395 | } | ||
396 | #endregion | ||
397 | |||
398 | /// <summary> | ||
399 | /// Initiailizes the scene | ||
400 | /// Sets many properties that ODE requires to be stable | ||
401 | /// These settings need to be tweaked 'exactly' right or weird stuff happens. | ||
402 | /// </summary> | ||
403 | private void RawInitialization() | ||
404 | { | ||
405 | |||
406 | // checkThread(); | ||
407 | |||
408 | OdeLock = new Object(); | ||
409 | SimulationLock = new Object(); | ||
410 | |||
411 | nearCallback = near; | ||
412 | |||
413 | m_rayCastManager = new ODERayCastRequestManager(this); | ||
414 | |||
415 | lock (OdeLock) | ||
416 | { | ||
417 | // Create the world and the first space | ||
418 | try | ||
419 | { | ||
420 | world = d.WorldCreate(); | ||
421 | TopSpace = d.HashSpaceCreate(IntPtr.Zero); | ||
422 | |||
423 | // now the major subspaces | ||
424 | ActiveSpace = d.HashSpaceCreate(TopSpace); | ||
425 | CharsSpace = d.HashSpaceCreate(TopSpace); | ||
426 | StaticSpace = d.HashSpaceCreate(TopSpace); | ||
427 | GroundSpace = d.HashSpaceCreate(TopSpace); | ||
428 | } | ||
429 | catch | ||
430 | { | ||
431 | // i must RtC#FM | ||
432 | // i did! | ||
433 | } | ||
434 | |||
435 | d.HashSpaceSetLevels(TopSpace, -2, 8); | ||
436 | d.HashSpaceSetLevels(ActiveSpace, -2, 8); | ||
437 | d.HashSpaceSetLevels(CharsSpace, -4, 3); | ||
438 | d.HashSpaceSetLevels(StaticSpace, -2, 8); | ||
439 | d.HashSpaceSetLevels(GroundSpace, 0, 8); | ||
440 | |||
441 | // demote to second level | ||
442 | d.SpaceSetSublevel(ActiveSpace, 1); | ||
443 | d.SpaceSetSublevel(CharsSpace, 1); | ||
444 | d.SpaceSetSublevel(StaticSpace, 1); | ||
445 | d.SpaceSetSublevel(GroundSpace, 1); | ||
446 | |||
447 | d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | | ||
448 | CollisionCategories.Geom | | ||
449 | CollisionCategories.Character | | ||
450 | CollisionCategories.Phantom | | ||
451 | CollisionCategories.VolumeDtc | ||
452 | )); | ||
453 | d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | | ||
454 | CollisionCategories.Geom | | ||
455 | CollisionCategories.Character | | ||
456 | CollisionCategories.Phantom | | ||
457 | CollisionCategories.VolumeDtc | ||
458 | )); | ||
459 | d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | | ||
460 | CollisionCategories.Geom | | ||
461 | CollisionCategories.Character | | ||
462 | CollisionCategories.Phantom | | ||
463 | CollisionCategories.VolumeDtc | ||
464 | )); | ||
465 | d.GeomSetCollideBits(CharsSpace, 0); | ||
466 | |||
467 | d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | | ||
468 | CollisionCategories.Geom | | ||
469 | // CollisionCategories.Land | | ||
470 | // CollisionCategories.Water | | ||
471 | CollisionCategories.Phantom | | ||
472 | CollisionCategories.VolumeDtc | ||
473 | )); | ||
474 | d.GeomSetCollideBits(StaticSpace, 0); | ||
475 | |||
476 | d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); | ||
477 | d.GeomSetCollideBits(GroundSpace, 0); | ||
478 | |||
479 | contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); | ||
480 | //contactgroup | ||
481 | |||
482 | d.WorldSetAutoDisableFlag(world, false); | ||
483 | } | ||
484 | } | ||
485 | |||
486 | public void Initialise(IConfigSource config, Vector3 regionExtent) | ||
487 | { | ||
488 | WorldExtents.X = regionExtent.X; | ||
489 | m_regionWidth = (uint)regionExtent.X; | ||
490 | WorldExtents.Y = regionExtent.Y; | ||
491 | m_regionHeight = (uint)regionExtent.Y; | ||
492 | |||
493 | m_suportCombine = false; | ||
494 | // checkThread(); | ||
495 | m_config = config; | ||
496 | |||
497 | string ode_config = d.GetConfiguration(); | ||
498 | if (ode_config != null && ode_config != "") | ||
499 | { | ||
500 | m_log.WarnFormat("ODE configuration: {0}", ode_config); | ||
501 | |||
502 | if (ode_config.Contains("ODE_Ubit")) | ||
503 | { | ||
504 | OdeUbitLib = true; | ||
505 | } | ||
506 | } | ||
507 | |||
508 | // Defaults | ||
509 | |||
510 | int contactsPerCollision = 80; | ||
511 | |||
512 | IConfig physicsconfig = null; | ||
513 | |||
514 | if (m_config != null) | ||
515 | { | ||
516 | physicsconfig = m_config.Configs["ODEPhysicsSettings"]; | ||
517 | if (physicsconfig != null) | ||
518 | { | ||
519 | gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); | ||
520 | gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); | ||
521 | gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); | ||
522 | |||
523 | metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); | ||
524 | |||
525 | // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); | ||
526 | |||
527 | ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); | ||
528 | |||
529 | avDensity = physicsconfig.GetFloat("av_density", avDensity); | ||
530 | avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); | ||
531 | avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); | ||
532 | |||
533 | contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); | ||
534 | |||
535 | geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); | ||
536 | bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); | ||
537 | |||
538 | physics_logging = physicsconfig.GetBoolean("physics_logging", false); | ||
539 | physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); | ||
540 | physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); | ||
541 | |||
542 | minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); | ||
543 | maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); | ||
544 | } | ||
545 | } | ||
546 | |||
547 | |||
548 | d.WorldSetCFM(world, comumContactCFM); | ||
549 | d.WorldSetERP(world, comumContactERP); | ||
550 | |||
551 | d.WorldSetGravity(world, gravityx, gravityy, gravityz); | ||
552 | |||
553 | d.WorldSetLinearDamping(world, 0.002f); | ||
554 | d.WorldSetAngularDamping(world, 0.002f); | ||
555 | d.WorldSetAngularDampingThreshold(world, 0f); | ||
556 | d.WorldSetLinearDampingThreshold(world, 0f); | ||
557 | d.WorldSetMaxAngularSpeed(world, 100f); | ||
558 | |||
559 | d.WorldSetQuickStepNumIterations(world, m_physicsiterations); | ||
560 | |||
561 | d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); | ||
562 | d.WorldSetContactMaxCorrectingVel(world, 60.0f); | ||
563 | |||
564 | m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); | ||
565 | |||
566 | HalfOdeStep = ODE_STEPSIZE * 0.5f; | ||
567 | odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); | ||
568 | |||
569 | ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); | ||
570 | GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); | ||
571 | |||
572 | SharedTmpcontact.geom.g1 = IntPtr.Zero; | ||
573 | SharedTmpcontact.geom.g2 = IntPtr.Zero; | ||
574 | |||
575 | SharedTmpcontact.geom.side1 = -1; | ||
576 | SharedTmpcontact.geom.side2 = -1; | ||
577 | |||
578 | SharedTmpcontact.surface.mode = comumContactFlags; | ||
579 | SharedTmpcontact.surface.mu = 0; | ||
580 | SharedTmpcontact.surface.bounce = 0; | ||
581 | SharedTmpcontact.surface.soft_cfm = comumContactCFM; | ||
582 | SharedTmpcontact.surface.soft_erp = comumContactERP; | ||
583 | SharedTmpcontact.surface.slip1 = comumContactSLIP; | ||
584 | SharedTmpcontact.surface.slip2 = comumContactSLIP; | ||
585 | |||
586 | m_materialContactsData[(int)Material.Stone].mu = 0.8f; | ||
587 | m_materialContactsData[(int)Material.Stone].bounce = 0.4f; | ||
588 | |||
589 | m_materialContactsData[(int)Material.Metal].mu = 0.3f; | ||
590 | m_materialContactsData[(int)Material.Metal].bounce = 0.4f; | ||
591 | |||
592 | m_materialContactsData[(int)Material.Glass].mu = 0.2f; | ||
593 | m_materialContactsData[(int)Material.Glass].bounce = 0.7f; | ||
594 | |||
595 | m_materialContactsData[(int)Material.Wood].mu = 0.6f; | ||
596 | m_materialContactsData[(int)Material.Wood].bounce = 0.5f; | ||
597 | |||
598 | m_materialContactsData[(int)Material.Flesh].mu = 0.9f; | ||
599 | m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; | ||
600 | |||
601 | m_materialContactsData[(int)Material.Plastic].mu = 0.4f; | ||
602 | m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; | ||
603 | |||
604 | m_materialContactsData[(int)Material.Rubber].mu = 0.9f; | ||
605 | m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; | ||
606 | |||
607 | m_materialContactsData[(int)Material.light].mu = 0.0f; | ||
608 | m_materialContactsData[(int)Material.light].bounce = 0.0f; | ||
609 | |||
610 | |||
611 | spacesPerMeterX = 1.0f / metersInSpace; | ||
612 | spacesPerMeterY = spacesPerMeterX; | ||
613 | spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); | ||
614 | spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); | ||
615 | |||
616 | if (spaceGridMaxX > 24) | ||
617 | { | ||
618 | spaceGridMaxX = 24; | ||
619 | spacesPerMeterX = spaceGridMaxX / WorldExtents.X ; | ||
620 | } | ||
621 | |||
622 | if (spaceGridMaxY > 24) | ||
623 | { | ||
624 | spaceGridMaxY = 24; | ||
625 | spacesPerMeterY = spaceGridMaxY / WorldExtents.Y ; | ||
626 | } | ||
627 | |||
628 | staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; | ||
629 | |||
630 | // create all spaces now | ||
631 | int i, j; | ||
632 | IntPtr newspace; | ||
633 | |||
634 | for (i = 0; i < spaceGridMaxX; i++) | ||
635 | for (j = 0; j < spaceGridMaxY; j++) | ||
636 | { | ||
637 | newspace = d.HashSpaceCreate(StaticSpace); | ||
638 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
639 | waitForSpaceUnlock(newspace); | ||
640 | d.SpaceSetSublevel(newspace, 2); | ||
641 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
642 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
643 | CollisionCategories.Geom | | ||
644 | CollisionCategories.Land | | ||
645 | CollisionCategories.Water | | ||
646 | CollisionCategories.Phantom | | ||
647 | CollisionCategories.VolumeDtc | ||
648 | )); | ||
649 | d.GeomSetCollideBits(newspace, 0); | ||
650 | |||
651 | staticPrimspace[i, j] = newspace; | ||
652 | } | ||
653 | |||
654 | // let this now be index limit | ||
655 | spaceGridMaxX--; | ||
656 | spaceGridMaxY--; | ||
657 | |||
658 | // create 4 off world spaces (x<0,x>max,y<0,y>max) | ||
659 | staticPrimspaceOffRegion = new IntPtr[4]; | ||
660 | |||
661 | for (i = 0; i < 4; i++) | ||
662 | { | ||
663 | newspace = d.HashSpaceCreate(StaticSpace); | ||
664 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
665 | waitForSpaceUnlock(newspace); | ||
666 | d.SpaceSetSublevel(newspace, 2); | ||
667 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
668 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
669 | CollisionCategories.Geom | | ||
670 | CollisionCategories.Land | | ||
671 | CollisionCategories.Water | | ||
672 | CollisionCategories.Phantom | | ||
673 | CollisionCategories.VolumeDtc | ||
674 | )); | ||
675 | d.GeomSetCollideBits(newspace, 0); | ||
676 | |||
677 | staticPrimspaceOffRegion[i] = newspace; | ||
678 | } | ||
679 | |||
680 | m_lastframe = DateTime.UtcNow; | ||
681 | m_lastMeshExpire = m_lastframe; | ||
682 | } | ||
683 | |||
684 | internal void waitForSpaceUnlock(IntPtr space) | ||
685 | { | ||
686 | //if (space != IntPtr.Zero) | ||
687 | //while (d.SpaceLockQuery(space)) { } // Wait and do nothing | ||
688 | } | ||
689 | |||
690 | #region Collision Detection | ||
691 | |||
692 | // sets a global contact for a joint for contactgeom , and base contact description) | ||
693 | |||
694 | |||
695 | |||
696 | private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom) | ||
697 | { | ||
698 | if (m_global_contactcount >= maxContactsbeforedeath) | ||
699 | return IntPtr.Zero; | ||
700 | |||
701 | m_global_contactcount++; | ||
702 | |||
703 | SharedTmpcontact.geom.depth = contactGeom.depth; | ||
704 | SharedTmpcontact.geom.pos = contactGeom.pos; | ||
705 | SharedTmpcontact.geom.normal = contactGeom.normal; | ||
706 | |||
707 | IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); | ||
708 | Marshal.StructureToPtr(SharedTmpcontact, contact, true); | ||
709 | return d.JointCreateContactPtr(world, contactgroup, contact); | ||
710 | } | ||
711 | |||
712 | private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) | ||
713 | { | ||
714 | if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) | ||
715 | return false; | ||
716 | |||
717 | IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); | ||
718 | newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); | ||
719 | return true; | ||
720 | } | ||
721 | |||
722 | /// <summary> | ||
723 | /// This is our near callback. A geometry is near a body | ||
724 | /// </summary> | ||
725 | /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> | ||
726 | /// <param name="g1">a geometry or space</param> | ||
727 | /// <param name="g2">another geometry or space</param> | ||
728 | /// | ||
729 | |||
730 | private void near(IntPtr space, IntPtr g1, IntPtr g2) | ||
731 | { | ||
732 | // no lock here! It's invoked from within Simulate(), which is thread-locked | ||
733 | |||
734 | if (m_global_contactcount >= maxContactsbeforedeath) | ||
735 | return; | ||
736 | |||
737 | // Test if we're colliding a geom with a space. | ||
738 | // If so we have to drill down into the space recursively | ||
739 | |||
740 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
741 | return; | ||
742 | |||
743 | if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) | ||
744 | { | ||
745 | // We'll be calling near recursivly if one | ||
746 | // of them is a space to find all of the | ||
747 | // contact points in the space | ||
748 | try | ||
749 | { | ||
750 | d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); | ||
751 | } | ||
752 | catch (AccessViolationException) | ||
753 | { | ||
754 | m_log.Warn("[PHYSICS]: Unable to collide test a space"); | ||
755 | return; | ||
756 | } | ||
757 | //here one should check collisions of geoms inside a space | ||
758 | // but on each space we only should have geoms that not colide amoung each other | ||
759 | // so we don't dig inside spaces | ||
760 | return; | ||
761 | } | ||
762 | |||
763 | // get geom bodies to check if we already a joint contact | ||
764 | // guess this shouldn't happen now | ||
765 | IntPtr b1 = d.GeomGetBody(g1); | ||
766 | IntPtr b2 = d.GeomGetBody(g2); | ||
767 | |||
768 | // d.GeomClassID id = d.GeomGetClass(g1); | ||
769 | |||
770 | // Figure out how many contact points we have | ||
771 | int count = 0; | ||
772 | try | ||
773 | { | ||
774 | // Colliding Geom To Geom | ||
775 | // This portion of the function 'was' blatantly ripped off from BoxStack.cs | ||
776 | |||
777 | if (g1 == g2) | ||
778 | return; // Can't collide with yourself | ||
779 | |||
780 | if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) | ||
781 | return; | ||
782 | /* | ||
783 | // debug | ||
784 | PhysicsActor dp2; | ||
785 | if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) | ||
786 | { | ||
787 | d.AABB aabb; | ||
788 | d.GeomGetAABB(g2, out aabb); | ||
789 | float x = aabb.MaxX - aabb.MinX; | ||
790 | float y = aabb.MaxY - aabb.MinY; | ||
791 | float z = aabb.MaxZ - aabb.MinZ; | ||
792 | if (x > 60.0f || y > 60.0f || z > 60.0f) | ||
793 | { | ||
794 | if (!actor_name_map.TryGetValue(g2, out dp2)) | ||
795 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); | ||
796 | else | ||
797 | m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", | ||
798 | dp2.Name, dp2.Size, x, y, z, | ||
799 | dp2.Position.ToString(), | ||
800 | dp2.Orientation.ToString(), | ||
801 | dp2.Orientation.Length()); | ||
802 | return; | ||
803 | } | ||
804 | } | ||
805 | // | ||
806 | */ | ||
807 | |||
808 | |||
809 | if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || | ||
810 | d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) | ||
811 | { | ||
812 | int cflags; | ||
813 | unchecked | ||
814 | { | ||
815 | cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); | ||
816 | } | ||
817 | count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
818 | } | ||
819 | else | ||
820 | count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
821 | } | ||
822 | catch (SEHException) | ||
823 | { | ||
824 | 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."); | ||
825 | // ode.drelease(world); | ||
826 | base.TriggerPhysicsBasedRestart(); | ||
827 | } | ||
828 | catch (Exception e) | ||
829 | { | ||
830 | m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); | ||
831 | return; | ||
832 | } | ||
833 | |||
834 | // contacts done | ||
835 | if (count == 0) | ||
836 | return; | ||
837 | |||
838 | // try get physical actors | ||
839 | PhysicsActor p1; | ||
840 | PhysicsActor p2; | ||
841 | |||
842 | if (!actor_name_map.TryGetValue(g1, out p1)) | ||
843 | { | ||
844 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); | ||
845 | return; | ||
846 | } | ||
847 | |||
848 | if (!actor_name_map.TryGetValue(g2, out p2)) | ||
849 | { | ||
850 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); | ||
851 | return; | ||
852 | } | ||
853 | |||
854 | // update actors collision score | ||
855 | if (p1.CollisionScore >= float.MaxValue - count) | ||
856 | p1.CollisionScore = 0; | ||
857 | p1.CollisionScore += count; | ||
858 | |||
859 | if (p2.CollisionScore >= float.MaxValue - count) | ||
860 | p2.CollisionScore = 0; | ||
861 | p2.CollisionScore += count; | ||
862 | |||
863 | // get first contact | ||
864 | d.ContactGeom curContact = new d.ContactGeom(); | ||
865 | |||
866 | if (!GetCurContactGeom(0, ref curContact)) | ||
867 | return; | ||
868 | |||
869 | ContactPoint maxDepthContact = new ContactPoint(); | ||
870 | |||
871 | // do volume detection case | ||
872 | if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) | ||
873 | { | ||
874 | maxDepthContact = new ContactPoint( | ||
875 | new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), | ||
876 | new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), | ||
877 | curContact.depth, false | ||
878 | ); | ||
879 | |||
880 | collision_accounting_events(p1, p2, maxDepthContact); | ||
881 | return; | ||
882 | } | ||
883 | |||
884 | // big messy collision analises | ||
885 | |||
886 | float mu = 0; | ||
887 | float bounce = 0; | ||
888 | // bool IgnoreNegSides = false; | ||
889 | |||
890 | ContactData contactdata1 = new ContactData(0, 0, false); | ||
891 | ContactData contactdata2 = new ContactData(0, 0, false); | ||
892 | |||
893 | bool dop1ava = false; | ||
894 | bool dop2ava = false; | ||
895 | bool ignore = false; | ||
896 | |||
897 | switch (p1.PhysicsActorType) | ||
898 | { | ||
899 | case (int)ActorTypes.Agent: | ||
900 | { | ||
901 | dop1ava = true; | ||
902 | switch (p2.PhysicsActorType) | ||
903 | { | ||
904 | case (int)ActorTypes.Agent: | ||
905 | case (int)ActorTypes.Prim: | ||
906 | break; | ||
907 | |||
908 | default: | ||
909 | ignore = true; // avatar to terrain and water ignored | ||
910 | break; | ||
911 | } | ||
912 | break; | ||
913 | } | ||
914 | |||
915 | case (int)ActorTypes.Prim: | ||
916 | { | ||
917 | switch (p2.PhysicsActorType) | ||
918 | { | ||
919 | case (int)ActorTypes.Agent: | ||
920 | dop2ava = true; | ||
921 | break; | ||
922 | |||
923 | case (int)ActorTypes.Prim: | ||
924 | Vector3 relV = p1.Velocity - p2.Velocity; | ||
925 | float relVlenSQ = relV.LengthSquared(); | ||
926 | if (relVlenSQ > 0.0001f) | ||
927 | { | ||
928 | p1.CollidingObj = true; | ||
929 | p2.CollidingObj = true; | ||
930 | } | ||
931 | p1.getContactData(ref contactdata1); | ||
932 | p2.getContactData(ref contactdata2); | ||
933 | bounce = contactdata1.bounce * contactdata2.bounce; | ||
934 | mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); | ||
935 | |||
936 | if (relVlenSQ > 0.01f) | ||
937 | mu *= frictionMovementMult; | ||
938 | |||
939 | break; | ||
940 | |||
941 | case (int)ActorTypes.Ground: | ||
942 | p1.getContactData(ref contactdata1); | ||
943 | bounce = contactdata1.bounce * TerrainBounce; | ||
944 | mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); | ||
945 | |||
946 | if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) | ||
947 | mu *= frictionMovementMult; | ||
948 | p1.CollidingGround = true; | ||
949 | /* | ||
950 | if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) | ||
951 | { | ||
952 | if (curContact.side1 > 0) | ||
953 | IgnoreNegSides = true; | ||
954 | } | ||
955 | */ | ||
956 | break; | ||
957 | |||
958 | case (int)ActorTypes.Water: | ||
959 | default: | ||
960 | ignore = true; | ||
961 | break; | ||
962 | } | ||
963 | } | ||
964 | break; | ||
965 | |||
966 | case (int)ActorTypes.Ground: | ||
967 | if (p2.PhysicsActorType == (int)ActorTypes.Prim) | ||
968 | { | ||
969 | p2.CollidingGround = true; | ||
970 | p2.getContactData(ref contactdata2); | ||
971 | bounce = contactdata2.bounce * TerrainBounce; | ||
972 | mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); | ||
973 | |||
974 | // if (curContact.side1 > 0) // should be 2 ? | ||
975 | // IgnoreNegSides = true; | ||
976 | |||
977 | if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) | ||
978 | mu *= frictionMovementMult; | ||
979 | } | ||
980 | else | ||
981 | ignore = true; | ||
982 | break; | ||
983 | |||
984 | case (int)ActorTypes.Water: | ||
985 | default: | ||
986 | break; | ||
987 | } | ||
988 | |||
989 | if (ignore) | ||
990 | return; | ||
991 | |||
992 | IntPtr Joint; | ||
993 | bool FeetCollision = false; | ||
994 | int ncontacts = 0; | ||
995 | |||
996 | int i = 0; | ||
997 | |||
998 | maxDepthContact = new ContactPoint(); | ||
999 | maxDepthContact.PenetrationDepth = float.MinValue; | ||
1000 | ContactPoint minDepthContact = new ContactPoint(); | ||
1001 | minDepthContact.PenetrationDepth = float.MaxValue; | ||
1002 | |||
1003 | SharedTmpcontact.geom.depth = 0; | ||
1004 | SharedTmpcontact.surface.mu = mu; | ||
1005 | SharedTmpcontact.surface.bounce = bounce; | ||
1006 | |||
1007 | d.ContactGeom altContact = new d.ContactGeom(); | ||
1008 | bool useAltcontact = false; | ||
1009 | bool noskip = true; | ||
1010 | |||
1011 | while (true) | ||
1012 | { | ||
1013 | // if (!(IgnoreNegSides && curContact.side1 < 0)) | ||
1014 | { | ||
1015 | noskip = true; | ||
1016 | useAltcontact = false; | ||
1017 | |||
1018 | if (dop1ava) | ||
1019 | { | ||
1020 | if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) | ||
1021 | { | ||
1022 | if (p2.PhysicsActorType == (int)ActorTypes.Agent) | ||
1023 | { | ||
1024 | p1.CollidingObj = true; | ||
1025 | p2.CollidingObj = true; | ||
1026 | } | ||
1027 | else if (p2.Velocity.LengthSquared() > 0.0f) | ||
1028 | p2.CollidingObj = true; | ||
1029 | } | ||
1030 | else | ||
1031 | noskip = false; | ||
1032 | } | ||
1033 | else if (dop2ava) | ||
1034 | { | ||
1035 | if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) | ||
1036 | { | ||
1037 | if (p1.PhysicsActorType == (int)ActorTypes.Agent) | ||
1038 | { | ||
1039 | p1.CollidingObj = true; | ||
1040 | p2.CollidingObj = true; | ||
1041 | } | ||
1042 | else if (p2.Velocity.LengthSquared() > 0.0f) | ||
1043 | p1.CollidingObj = true; | ||
1044 | } | ||
1045 | else | ||
1046 | noskip = false; | ||
1047 | } | ||
1048 | |||
1049 | if (noskip) | ||
1050 | { | ||
1051 | if(useAltcontact) | ||
1052 | Joint = CreateContacJoint(ref altContact); | ||
1053 | else | ||
1054 | Joint = CreateContacJoint(ref curContact); | ||
1055 | |||
1056 | if (Joint == IntPtr.Zero) | ||
1057 | break; | ||
1058 | |||
1059 | d.JointAttach(Joint, b1, b2); | ||
1060 | |||
1061 | ncontacts++; | ||
1062 | |||
1063 | if (curContact.depth > maxDepthContact.PenetrationDepth) | ||
1064 | { | ||
1065 | maxDepthContact.Position.X = curContact.pos.X; | ||
1066 | maxDepthContact.Position.Y = curContact.pos.Y; | ||
1067 | maxDepthContact.Position.Z = curContact.pos.Z; | ||
1068 | maxDepthContact.PenetrationDepth = curContact.depth; | ||
1069 | maxDepthContact.CharacterFeet = FeetCollision; | ||
1070 | } | ||
1071 | |||
1072 | if (curContact.depth < minDepthContact.PenetrationDepth) | ||
1073 | { | ||
1074 | minDepthContact.PenetrationDepth = curContact.depth; | ||
1075 | minDepthContact.SurfaceNormal.X = curContact.normal.X; | ||
1076 | minDepthContact.SurfaceNormal.Y = curContact.normal.Y; | ||
1077 | minDepthContact.SurfaceNormal.Z = curContact.normal.Z; | ||
1078 | } | ||
1079 | } | ||
1080 | } | ||
1081 | if (++i >= count) | ||
1082 | break; | ||
1083 | |||
1084 | if (!GetCurContactGeom(i, ref curContact)) | ||
1085 | break; | ||
1086 | } | ||
1087 | |||
1088 | if (ncontacts > 0) | ||
1089 | { | ||
1090 | maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; | ||
1091 | maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; | ||
1092 | maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; | ||
1093 | |||
1094 | collision_accounting_events(p1, p2, maxDepthContact); | ||
1095 | } | ||
1096 | } | ||
1097 | |||
1098 | private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) | ||
1099 | { | ||
1100 | uint obj2LocalID = 0; | ||
1101 | |||
1102 | bool p1events = p1.SubscribedEvents(); | ||
1103 | bool p2events = p2.SubscribedEvents(); | ||
1104 | |||
1105 | if (p1.IsVolumeDtc) | ||
1106 | p2events = false; | ||
1107 | if (p2.IsVolumeDtc) | ||
1108 | p1events = false; | ||
1109 | |||
1110 | if (!p2events && !p1events) | ||
1111 | return; | ||
1112 | |||
1113 | Vector3 vel = Vector3.Zero; | ||
1114 | if (p2 != null && p2.IsPhysical) | ||
1115 | vel = p2.Velocity; | ||
1116 | |||
1117 | if (p1 != null && p1.IsPhysical) | ||
1118 | vel -= p1.Velocity; | ||
1119 | |||
1120 | contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); | ||
1121 | |||
1122 | switch ((ActorTypes)p1.PhysicsActorType) | ||
1123 | { | ||
1124 | case ActorTypes.Agent: | ||
1125 | case ActorTypes.Prim: | ||
1126 | { | ||
1127 | switch ((ActorTypes)p2.PhysicsActorType) | ||
1128 | { | ||
1129 | case ActorTypes.Agent: | ||
1130 | case ActorTypes.Prim: | ||
1131 | if (p2events) | ||
1132 | { | ||
1133 | AddCollisionEventReporting(p2); | ||
1134 | p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); | ||
1135 | } | ||
1136 | obj2LocalID = p2.ParentActor.LocalID; | ||
1137 | break; | ||
1138 | |||
1139 | case ActorTypes.Ground: | ||
1140 | case ActorTypes.Unknown: | ||
1141 | default: | ||
1142 | obj2LocalID = 0; | ||
1143 | break; | ||
1144 | } | ||
1145 | if (p1events) | ||
1146 | { | ||
1147 | contact.SurfaceNormal = -contact.SurfaceNormal; | ||
1148 | AddCollisionEventReporting(p1); | ||
1149 | p1.AddCollisionEvent(obj2LocalID, contact); | ||
1150 | } | ||
1151 | break; | ||
1152 | } | ||
1153 | case ActorTypes.Ground: | ||
1154 | case ActorTypes.Unknown: | ||
1155 | default: | ||
1156 | { | ||
1157 | if (p2events && !p2.IsVolumeDtc) | ||
1158 | { | ||
1159 | AddCollisionEventReporting(p2); | ||
1160 | p2.AddCollisionEvent(0, contact); | ||
1161 | } | ||
1162 | break; | ||
1163 | } | ||
1164 | } | ||
1165 | } | ||
1166 | |||
1167 | /// <summary> | ||
1168 | /// This is our collision testing routine in ODE | ||
1169 | /// </summary> | ||
1170 | /// <param name="timeStep"></param> | ||
1171 | private void collision_optimized() | ||
1172 | { | ||
1173 | lock (_characters) | ||
1174 | { | ||
1175 | try | ||
1176 | { | ||
1177 | foreach (OdeCharacter chr in _characters) | ||
1178 | { | ||
1179 | if (chr == null || chr.Body == IntPtr.Zero) | ||
1180 | continue; | ||
1181 | |||
1182 | chr.IsColliding = false; | ||
1183 | // chr.CollidingGround = false; not done here | ||
1184 | chr.CollidingObj = false; | ||
1185 | // do colisions with static space | ||
1186 | d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); | ||
1187 | |||
1188 | // no coll with gnd | ||
1189 | } | ||
1190 | // chars with chars | ||
1191 | d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); | ||
1192 | |||
1193 | } | ||
1194 | catch (AccessViolationException) | ||
1195 | { | ||
1196 | m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); | ||
1197 | } | ||
1198 | |||
1199 | } | ||
1200 | |||
1201 | lock (_activeprims) | ||
1202 | { | ||
1203 | foreach (OdePrim aprim in _activeprims) | ||
1204 | { | ||
1205 | aprim.CollisionScore = 0; | ||
1206 | aprim.IsColliding = false; | ||
1207 | } | ||
1208 | } | ||
1209 | |||
1210 | // collide active prims with static enviroment | ||
1211 | lock (_activegroups) | ||
1212 | { | ||
1213 | try | ||
1214 | { | ||
1215 | foreach (OdePrim prm in _activegroups) | ||
1216 | { | ||
1217 | if (!prm.m_outbounds) | ||
1218 | { | ||
1219 | if (d.BodyIsEnabled(prm.Body)) | ||
1220 | { | ||
1221 | d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); | ||
1222 | d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); | ||
1223 | } | ||
1224 | } | ||
1225 | } | ||
1226 | } | ||
1227 | catch (AccessViolationException) | ||
1228 | { | ||
1229 | m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); | ||
1230 | } | ||
1231 | } | ||
1232 | // colide active amoung them | ||
1233 | try | ||
1234 | { | ||
1235 | d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); | ||
1236 | } | ||
1237 | catch (AccessViolationException) | ||
1238 | { | ||
1239 | m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space"); | ||
1240 | } | ||
1241 | // and with chars | ||
1242 | try | ||
1243 | { | ||
1244 | d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); | ||
1245 | } | ||
1246 | catch (AccessViolationException) | ||
1247 | { | ||
1248 | m_log.Warn("[PHYSICS]: Unable to collide in Active space"); | ||
1249 | } | ||
1250 | // _perloopContact.Clear(); | ||
1251 | } | ||
1252 | |||
1253 | #endregion | ||
1254 | /// <summary> | ||
1255 | /// Add actor to the list that should receive collision events in the simulate loop. | ||
1256 | /// </summary> | ||
1257 | /// <param name="obj"></param> | ||
1258 | public void AddCollisionEventReporting(PhysicsActor obj) | ||
1259 | { | ||
1260 | if (!_collisionEventPrim.Contains(obj)) | ||
1261 | _collisionEventPrim.Add(obj); | ||
1262 | } | ||
1263 | |||
1264 | /// <summary> | ||
1265 | /// Remove actor from the list that should receive collision events in the simulate loop. | ||
1266 | /// </summary> | ||
1267 | /// <param name="obj"></param> | ||
1268 | public void RemoveCollisionEventReporting(PhysicsActor obj) | ||
1269 | { | ||
1270 | if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) | ||
1271 | _collisionEventPrimRemove.Add(obj); | ||
1272 | } | ||
1273 | |||
1274 | public override float TimeDilation | ||
1275 | { | ||
1276 | get { return m_timeDilation; } | ||
1277 | } | ||
1278 | |||
1279 | public override bool SupportsNINJAJoints | ||
1280 | { | ||
1281 | get { return false; } | ||
1282 | } | ||
1283 | |||
1284 | #region Add/Remove Entities | ||
1285 | |||
1286 | public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) | ||
1287 | { | ||
1288 | return null; | ||
1289 | } | ||
1290 | |||
1291 | public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) | ||
1292 | { | ||
1293 | OdeCharacter newAv = new OdeCharacter(localID, avName, this, position, | ||
1294 | size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); | ||
1295 | newAv.Flying = isFlying; | ||
1296 | newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; | ||
1297 | |||
1298 | return newAv; | ||
1299 | } | ||
1300 | |||
1301 | public void AddCharacter(OdeCharacter chr) | ||
1302 | { | ||
1303 | lock (_characters) | ||
1304 | { | ||
1305 | if (!_characters.Contains(chr)) | ||
1306 | { | ||
1307 | _characters.Add(chr); | ||
1308 | if (chr.bad) | ||
1309 | m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); | ||
1310 | } | ||
1311 | } | ||
1312 | } | ||
1313 | |||
1314 | public void RemoveCharacter(OdeCharacter chr) | ||
1315 | { | ||
1316 | lock (_characters) | ||
1317 | { | ||
1318 | if (_characters.Contains(chr)) | ||
1319 | { | ||
1320 | _characters.Remove(chr); | ||
1321 | } | ||
1322 | } | ||
1323 | } | ||
1324 | |||
1325 | public void BadCharacter(OdeCharacter chr) | ||
1326 | { | ||
1327 | lock (_badCharacter) | ||
1328 | { | ||
1329 | if (!_badCharacter.Contains(chr)) | ||
1330 | _badCharacter.Add(chr); | ||
1331 | } | ||
1332 | } | ||
1333 | |||
1334 | public override void RemoveAvatar(PhysicsActor actor) | ||
1335 | { | ||
1336 | //m_log.Debug("[PHYSICS]:ODELOCK"); | ||
1337 | ((OdeCharacter) actor).Destroy(); | ||
1338 | } | ||
1339 | |||
1340 | |||
1341 | public void addActivePrim(OdePrim activatePrim) | ||
1342 | { | ||
1343 | // adds active prim.. | ||
1344 | lock (_activeprims) | ||
1345 | { | ||
1346 | if (!_activeprims.Contains(activatePrim)) | ||
1347 | _activeprims.Add(activatePrim); | ||
1348 | } | ||
1349 | } | ||
1350 | |||
1351 | public void addActiveGroups(OdePrim activatePrim) | ||
1352 | { | ||
1353 | lock (_activegroups) | ||
1354 | { | ||
1355 | if (!_activegroups.Contains(activatePrim)) | ||
1356 | _activegroups.Add(activatePrim); | ||
1357 | } | ||
1358 | } | ||
1359 | |||
1360 | private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, | ||
1361 | PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) | ||
1362 | { | ||
1363 | OdePrim newPrim; | ||
1364 | lock (OdeLock) | ||
1365 | { | ||
1366 | newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); | ||
1367 | lock (_prims) | ||
1368 | _prims.Add(newPrim); | ||
1369 | } | ||
1370 | return newPrim; | ||
1371 | } | ||
1372 | |||
1373 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1374 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) | ||
1375 | { | ||
1376 | return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); | ||
1377 | } | ||
1378 | |||
1379 | |||
1380 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1381 | Vector3 size, Quaternion rotation, bool isPhysical, uint localid) | ||
1382 | { | ||
1383 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); | ||
1384 | } | ||
1385 | |||
1386 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1387 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) | ||
1388 | { | ||
1389 | |||
1390 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); | ||
1391 | } | ||
1392 | |||
1393 | public void remActivePrim(OdePrim deactivatePrim) | ||
1394 | { | ||
1395 | lock (_activeprims) | ||
1396 | { | ||
1397 | _activeprims.Remove(deactivatePrim); | ||
1398 | } | ||
1399 | } | ||
1400 | public void remActiveGroup(OdePrim deactivatePrim) | ||
1401 | { | ||
1402 | lock (_activegroups) | ||
1403 | { | ||
1404 | _activegroups.Remove(deactivatePrim); | ||
1405 | } | ||
1406 | } | ||
1407 | |||
1408 | public override void RemovePrim(PhysicsActor prim) | ||
1409 | { | ||
1410 | // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be | ||
1411 | // removed in the next physics simulate pass. | ||
1412 | if (prim is OdePrim) | ||
1413 | { | ||
1414 | // lock (OdeLock) | ||
1415 | { | ||
1416 | |||
1417 | OdePrim p = (OdePrim)prim; | ||
1418 | p.setPrimForRemoval(); | ||
1419 | } | ||
1420 | } | ||
1421 | } | ||
1422 | |||
1423 | public void RemovePrimThreadLocked(OdePrim prim) | ||
1424 | { | ||
1425 | //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); | ||
1426 | lock (prim) | ||
1427 | { | ||
1428 | // RemoveCollisionEventReporting(prim); | ||
1429 | lock (_prims) | ||
1430 | _prims.Remove(prim); | ||
1431 | } | ||
1432 | |||
1433 | } | ||
1434 | |||
1435 | public bool havePrim(OdePrim prm) | ||
1436 | { | ||
1437 | lock (_prims) | ||
1438 | return _prims.Contains(prm); | ||
1439 | } | ||
1440 | |||
1441 | public bool haveActor(PhysicsActor actor) | ||
1442 | { | ||
1443 | if (actor is OdePrim) | ||
1444 | { | ||
1445 | lock (_prims) | ||
1446 | return _prims.Contains((OdePrim)actor); | ||
1447 | } | ||
1448 | else if (actor is OdeCharacter) | ||
1449 | { | ||
1450 | lock (_characters) | ||
1451 | return _characters.Contains((OdeCharacter)actor); | ||
1452 | } | ||
1453 | return false; | ||
1454 | } | ||
1455 | |||
1456 | #endregion | ||
1457 | |||
1458 | #region Space Separation Calculation | ||
1459 | |||
1460 | /// <summary> | ||
1461 | /// Called when a static prim moves or becomes static | ||
1462 | /// Places the prim in a space one the static sub-spaces grid | ||
1463 | /// </summary> | ||
1464 | /// <param name="geom">the pointer to the geom that moved</param> | ||
1465 | /// <param name="pos">the position that the geom moved to</param> | ||
1466 | /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> | ||
1467 | /// <returns>a pointer to the new space it's in</returns> | ||
1468 | public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) | ||
1469 | { | ||
1470 | // moves a prim into another static sub-space or from another space into a static sub-space | ||
1471 | |||
1472 | // Called ODEPrim so | ||
1473 | // it's already in locked space. | ||
1474 | |||
1475 | if (geom == IntPtr.Zero) // shouldn't happen | ||
1476 | return IntPtr.Zero; | ||
1477 | |||
1478 | // get the static sub-space for current position | ||
1479 | IntPtr newspace = calculateSpaceForGeom(pos); | ||
1480 | |||
1481 | if (newspace == currentspace) // if we are there all done | ||
1482 | return newspace; | ||
1483 | |||
1484 | // else remove it from its current space | ||
1485 | if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) | ||
1486 | { | ||
1487 | if (d.GeomIsSpace(currentspace)) | ||
1488 | { | ||
1489 | waitForSpaceUnlock(currentspace); | ||
1490 | d.SpaceRemove(currentspace, geom); | ||
1491 | |||
1492 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) | ||
1493 | { | ||
1494 | d.SpaceDestroy(currentspace); | ||
1495 | } | ||
1496 | } | ||
1497 | else | ||
1498 | { | ||
1499 | m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + | ||
1500 | " Geom:" + geom); | ||
1501 | } | ||
1502 | } | ||
1503 | else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space | ||
1504 | { | ||
1505 | currentspace = d.GeomGetSpace(geom); | ||
1506 | if (currentspace != IntPtr.Zero) | ||
1507 | { | ||
1508 | if (d.GeomIsSpace(currentspace)) | ||
1509 | { | ||
1510 | waitForSpaceUnlock(currentspace); | ||
1511 | d.SpaceRemove(currentspace, geom); | ||
1512 | |||
1513 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) | ||
1514 | { | ||
1515 | d.SpaceDestroy(currentspace); | ||
1516 | } | ||
1517 | |||
1518 | } | ||
1519 | } | ||
1520 | } | ||
1521 | |||
1522 | // put the geom in the newspace | ||
1523 | waitForSpaceUnlock(newspace); | ||
1524 | d.SpaceAdd(newspace, geom); | ||
1525 | |||
1526 | // let caller know this newspace | ||
1527 | return newspace; | ||
1528 | } | ||
1529 | |||
1530 | /// <summary> | ||
1531 | /// Calculates the space the prim should be in by its position | ||
1532 | /// </summary> | ||
1533 | /// <param name="pos"></param> | ||
1534 | /// <returns>a pointer to the space. This could be a new space or reused space.</returns> | ||
1535 | public IntPtr calculateSpaceForGeom(Vector3 pos) | ||
1536 | { | ||
1537 | int x, y; | ||
1538 | |||
1539 | if (pos.X < 0) | ||
1540 | return staticPrimspaceOffRegion[0]; | ||
1541 | |||
1542 | if (pos.Y < 0) | ||
1543 | return staticPrimspaceOffRegion[2]; | ||
1544 | |||
1545 | x = (int)(pos.X * spacesPerMeterX); | ||
1546 | if (x > spaceGridMaxX) | ||
1547 | return staticPrimspaceOffRegion[1]; | ||
1548 | |||
1549 | y = (int)(pos.Y * spacesPerMeterY); | ||
1550 | if (y > spaceGridMaxY) | ||
1551 | return staticPrimspaceOffRegion[3]; | ||
1552 | |||
1553 | return staticPrimspace[x, y]; | ||
1554 | } | ||
1555 | |||
1556 | #endregion | ||
1557 | |||
1558 | |||
1559 | /// <summary> | ||
1560 | /// Called to queue a change to a actor | ||
1561 | /// to use in place of old taint mechanism so changes do have a time sequence | ||
1562 | /// </summary> | ||
1563 | |||
1564 | public void AddChange(PhysicsActor actor, changes what, Object arg) | ||
1565 | { | ||
1566 | ODEchangeitem item = new ODEchangeitem(); | ||
1567 | item.actor = actor; | ||
1568 | item.what = what; | ||
1569 | item.arg = arg; | ||
1570 | ChangesQueue.Enqueue(item); | ||
1571 | } | ||
1572 | |||
1573 | /// <summary> | ||
1574 | /// Called after our prim properties are set Scale, position etc. | ||
1575 | /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex | ||
1576 | /// This assures us that we have no race conditions | ||
1577 | /// </summary> | ||
1578 | /// <param name="prim"></param> | ||
1579 | public override void AddPhysicsActorTaint(PhysicsActor prim) | ||
1580 | { | ||
1581 | } | ||
1582 | |||
1583 | // does all pending changes generated during region load process | ||
1584 | public override void PrepareSimulation() | ||
1585 | { | ||
1586 | lock (OdeLock) | ||
1587 | { | ||
1588 | if (world == IntPtr.Zero) | ||
1589 | { | ||
1590 | ChangesQueue.Clear(); | ||
1591 | return; | ||
1592 | } | ||
1593 | |||
1594 | ODEchangeitem item; | ||
1595 | |||
1596 | int donechanges = 0; | ||
1597 | if (ChangesQueue.Count > 0) | ||
1598 | { | ||
1599 | m_log.InfoFormat("[ODE] start processing pending actor operations"); | ||
1600 | int tstart = Util.EnvironmentTickCount(); | ||
1601 | |||
1602 | while (ChangesQueue.Dequeue(out item)) | ||
1603 | { | ||
1604 | if (item.actor != null) | ||
1605 | { | ||
1606 | try | ||
1607 | { | ||
1608 | if (item.actor is OdeCharacter) | ||
1609 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); | ||
1610 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) | ||
1611 | RemovePrimThreadLocked((OdePrim)item.actor); | ||
1612 | } | ||
1613 | catch | ||
1614 | { | ||
1615 | m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", | ||
1616 | item.actor.Name, item.what.ToString()); | ||
1617 | } | ||
1618 | } | ||
1619 | donechanges++; | ||
1620 | } | ||
1621 | int time = Util.EnvironmentTickCountSubtract(tstart); | ||
1622 | m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); | ||
1623 | } | ||
1624 | } | ||
1625 | } | ||
1626 | |||
1627 | /// <summary> | ||
1628 | /// This is our main simulate loop | ||
1629 | /// It's thread locked by a Mutex in the scene. | ||
1630 | /// It holds Collisions, it instructs ODE to step through the physical reactions | ||
1631 | /// It moves the objects around in memory | ||
1632 | /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) | ||
1633 | /// </summary> | ||
1634 | /// <param name="timeStep"></param> | ||
1635 | /// <returns></returns> | ||
1636 | public override float Simulate(float timeStep) | ||
1637 | { | ||
1638 | DateTime now = DateTime.UtcNow; | ||
1639 | TimeSpan timedif = now - m_lastframe; | ||
1640 | timeStep = (float)timedif.TotalSeconds; | ||
1641 | m_lastframe = now; | ||
1642 | |||
1643 | // acumulate time so we can reduce error | ||
1644 | step_time += timeStep; | ||
1645 | |||
1646 | if (step_time < HalfOdeStep) | ||
1647 | return 0; | ||
1648 | |||
1649 | if (framecount < 0) | ||
1650 | framecount = 0; | ||
1651 | |||
1652 | framecount++; | ||
1653 | |||
1654 | // int curphysiteractions; | ||
1655 | |||
1656 | // if in trouble reduce step resolution | ||
1657 | // if (step_time >= m_SkipFramesAtms) | ||
1658 | // curphysiteractions = m_physicsiterations / 2; | ||
1659 | // else | ||
1660 | // curphysiteractions = m_physicsiterations; | ||
1661 | |||
1662 | // checkThread(); | ||
1663 | int nodeframes = 0; | ||
1664 | |||
1665 | lock (SimulationLock) | ||
1666 | lock(OdeLock) | ||
1667 | { | ||
1668 | if (world == IntPtr.Zero) | ||
1669 | { | ||
1670 | ChangesQueue.Clear(); | ||
1671 | return 0; | ||
1672 | } | ||
1673 | |||
1674 | ODEchangeitem item; | ||
1675 | |||
1676 | // d.WorldSetQuickStepNumIterations(world, curphysiteractions); | ||
1677 | |||
1678 | int loopstartMS = Util.EnvironmentTickCount(); | ||
1679 | int looptimeMS = 0; | ||
1680 | |||
1681 | |||
1682 | while (step_time > HalfOdeStep) | ||
1683 | { | ||
1684 | try | ||
1685 | { | ||
1686 | // clear pointer/counter to contacts to pass into joints | ||
1687 | m_global_contactcount = 0; | ||
1688 | |||
1689 | if (ChangesQueue.Count > 0) | ||
1690 | { | ||
1691 | int changestartMS = Util.EnvironmentTickCount(); | ||
1692 | int ttmp; | ||
1693 | while (ChangesQueue.Dequeue(out item)) | ||
1694 | { | ||
1695 | if (item.actor != null) | ||
1696 | { | ||
1697 | try | ||
1698 | { | ||
1699 | if (item.actor is OdeCharacter) | ||
1700 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); | ||
1701 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) | ||
1702 | RemovePrimThreadLocked((OdePrim)item.actor); | ||
1703 | } | ||
1704 | catch | ||
1705 | { | ||
1706 | m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", | ||
1707 | item.actor.Name, item.what.ToString()); | ||
1708 | } | ||
1709 | } | ||
1710 | ttmp = Util.EnvironmentTickCountSubtract(changestartMS); | ||
1711 | if (ttmp > 20) | ||
1712 | break; | ||
1713 | } | ||
1714 | } | ||
1715 | |||
1716 | // Move characters | ||
1717 | lock (_characters) | ||
1718 | { | ||
1719 | List<OdeCharacter> defects = new List<OdeCharacter>(); | ||
1720 | foreach (OdeCharacter actor in _characters) | ||
1721 | { | ||
1722 | if (actor != null) | ||
1723 | actor.Move(defects); | ||
1724 | } | ||
1725 | if (defects.Count != 0) | ||
1726 | { | ||
1727 | foreach (OdeCharacter defect in defects) | ||
1728 | { | ||
1729 | RemoveCharacter(defect); | ||
1730 | } | ||
1731 | defects.Clear(); | ||
1732 | } | ||
1733 | } | ||
1734 | |||
1735 | // Move other active objects | ||
1736 | lock (_activegroups) | ||
1737 | { | ||
1738 | foreach (OdePrim aprim in _activegroups) | ||
1739 | { | ||
1740 | aprim.Move(); | ||
1741 | } | ||
1742 | } | ||
1743 | |||
1744 | m_rayCastManager.ProcessQueuedRequests(); | ||
1745 | |||
1746 | collision_optimized(); | ||
1747 | |||
1748 | foreach (PhysicsActor obj in _collisionEventPrim) | ||
1749 | { | ||
1750 | if (obj == null) | ||
1751 | continue; | ||
1752 | |||
1753 | switch ((ActorTypes)obj.PhysicsActorType) | ||
1754 | { | ||
1755 | case ActorTypes.Agent: | ||
1756 | OdeCharacter cobj = (OdeCharacter)obj; | ||
1757 | cobj.AddCollisionFrameTime((int)(odetimestepMS)); | ||
1758 | cobj.SendCollisions(); | ||
1759 | break; | ||
1760 | |||
1761 | case ActorTypes.Prim: | ||
1762 | OdePrim pobj = (OdePrim)obj; | ||
1763 | if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) | ||
1764 | if (!pobj.m_outbounds) | ||
1765 | { | ||
1766 | pobj.AddCollisionFrameTime((int)(odetimestepMS)); | ||
1767 | pobj.SendCollisions(); | ||
1768 | } | ||
1769 | break; | ||
1770 | } | ||
1771 | } | ||
1772 | |||
1773 | foreach (PhysicsActor obj in _collisionEventPrimRemove) | ||
1774 | _collisionEventPrim.Remove(obj); | ||
1775 | |||
1776 | _collisionEventPrimRemove.Clear(); | ||
1777 | |||
1778 | // do a ode simulation step | ||
1779 | d.WorldQuickStep(world, ODE_STEPSIZE); | ||
1780 | // d.WorldStep(world, ODE_STEPSIZE); | ||
1781 | d.JointGroupEmpty(contactgroup); | ||
1782 | |||
1783 | // update managed ideia of physical data and do updates to core | ||
1784 | /* | ||
1785 | lock (_characters) | ||
1786 | { | ||
1787 | foreach (OdeCharacter actor in _characters) | ||
1788 | { | ||
1789 | if (actor != null) | ||
1790 | { | ||
1791 | if (actor.bad) | ||
1792 | m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); | ||
1793 | |||
1794 | actor.UpdatePositionAndVelocity(); | ||
1795 | } | ||
1796 | } | ||
1797 | } | ||
1798 | */ | ||
1799 | |||
1800 | lock (_activegroups) | ||
1801 | { | ||
1802 | { | ||
1803 | foreach (OdePrim actor in _activegroups) | ||
1804 | { | ||
1805 | if (actor.IsPhysical) | ||
1806 | { | ||
1807 | actor.UpdatePositionAndVelocity(framecount); | ||
1808 | } | ||
1809 | } | ||
1810 | } | ||
1811 | } | ||
1812 | } | ||
1813 | catch (Exception e) | ||
1814 | { | ||
1815 | m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); | ||
1816 | // ode.dunlock(world); | ||
1817 | } | ||
1818 | |||
1819 | step_time -= ODE_STEPSIZE; | ||
1820 | nodeframes++; | ||
1821 | |||
1822 | looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); | ||
1823 | if (looptimeMS > 100) | ||
1824 | break; | ||
1825 | } | ||
1826 | |||
1827 | lock (_badCharacter) | ||
1828 | { | ||
1829 | if (_badCharacter.Count > 0) | ||
1830 | { | ||
1831 | foreach (OdeCharacter chr in _badCharacter) | ||
1832 | { | ||
1833 | RemoveCharacter(chr); | ||
1834 | } | ||
1835 | |||
1836 | _badCharacter.Clear(); | ||
1837 | } | ||
1838 | } | ||
1839 | |||
1840 | timedif = now - m_lastMeshExpire; | ||
1841 | |||
1842 | if (timedif.Seconds > 10) | ||
1843 | { | ||
1844 | mesher.ExpireReleaseMeshs(); | ||
1845 | m_lastMeshExpire = now; | ||
1846 | } | ||
1847 | |||
1848 | // information block running in debug only | ||
1849 | /* | ||
1850 | int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); | ||
1851 | int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); | ||
1852 | int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); | ||
1853 | |||
1854 | int nactivegeoms = 0; | ||
1855 | int nactivespaces = 0; | ||
1856 | |||
1857 | int nstaticgeoms = 0; | ||
1858 | int nstaticspaces = 0; | ||
1859 | IntPtr sp; | ||
1860 | |||
1861 | for (int i = 0; i < ntopactivegeoms; i++) | ||
1862 | { | ||
1863 | sp = d.SpaceGetGeom(ActiveSpace, i); | ||
1864 | if (d.GeomIsSpace(sp)) | ||
1865 | { | ||
1866 | nactivespaces++; | ||
1867 | nactivegeoms += d.SpaceGetNumGeoms(sp); | ||
1868 | } | ||
1869 | else | ||
1870 | nactivegeoms++; | ||
1871 | } | ||
1872 | |||
1873 | for (int i = 0; i < ntopstaticgeoms; i++) | ||
1874 | { | ||
1875 | sp = d.SpaceGetGeom(StaticSpace, i); | ||
1876 | if (d.GeomIsSpace(sp)) | ||
1877 | { | ||
1878 | nstaticspaces++; | ||
1879 | nstaticgeoms += d.SpaceGetNumGeoms(sp); | ||
1880 | } | ||
1881 | else | ||
1882 | nstaticgeoms++; | ||
1883 | } | ||
1884 | |||
1885 | int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); | ||
1886 | |||
1887 | int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray | ||
1888 | int nbodies = d.NTotalBodies; | ||
1889 | int ngeoms = d.NTotalGeoms; | ||
1890 | */ | ||
1891 | // Finished with all sim stepping. If requested, dump world state to file for debugging. | ||
1892 | // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? | ||
1893 | // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? | ||
1894 | if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) | ||
1895 | { | ||
1896 | string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename | ||
1897 | string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file | ||
1898 | |||
1899 | if (physics_logging_append_existing_logfile) | ||
1900 | { | ||
1901 | string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; | ||
1902 | TextWriter fwriter = File.AppendText(fname); | ||
1903 | fwriter.WriteLine(header); | ||
1904 | fwriter.Close(); | ||
1905 | } | ||
1906 | |||
1907 | d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); | ||
1908 | } | ||
1909 | |||
1910 | // think time dilation as to do with dinamic step size that we dont' have | ||
1911 | // even so tell something to world | ||
1912 | if (looptimeMS < 100) // we did the requested loops | ||
1913 | m_timeDilation = 1.0f; | ||
1914 | else if (step_time > 0) | ||
1915 | { | ||
1916 | m_timeDilation = timeStep / step_time; | ||
1917 | if (m_timeDilation > 1) | ||
1918 | m_timeDilation = 1; | ||
1919 | if (step_time > m_SkipFramesAtms) | ||
1920 | step_time = 0; | ||
1921 | m_lastframe = DateTime.UtcNow; // skip also the time lost | ||
1922 | } | ||
1923 | } | ||
1924 | return (float)nodeframes * ODE_STEPSIZE / timeStep; | ||
1925 | } | ||
1926 | |||
1927 | /// <summary> | ||
1928 | public override void GetResults() | ||
1929 | { | ||
1930 | } | ||
1931 | |||
1932 | public override bool IsThreaded | ||
1933 | { | ||
1934 | // for now we won't be multithreaded | ||
1935 | get { return (false); } | ||
1936 | } | ||
1937 | |||
1938 | public float GetTerrainHeightAtXY(float x, float y) | ||
1939 | { | ||
1940 | |||
1941 | int offsetX = 0; | ||
1942 | int offsetY = 0; | ||
1943 | |||
1944 | if (m_suportCombine) | ||
1945 | { | ||
1946 | offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1947 | offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1948 | } | ||
1949 | |||
1950 | // get region map | ||
1951 | IntPtr heightFieldGeom = IntPtr.Zero; | ||
1952 | if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) | ||
1953 | return 0f; | ||
1954 | |||
1955 | if (heightFieldGeom == IntPtr.Zero) | ||
1956 | return 0f; | ||
1957 | |||
1958 | if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) | ||
1959 | return 0f; | ||
1960 | |||
1961 | // TerrainHeightField for ODE as offset 1m | ||
1962 | x += 1f - offsetX; | ||
1963 | y += 1f - offsetY; | ||
1964 | |||
1965 | // make position fit into array | ||
1966 | if (x < 0) | ||
1967 | x = 0; | ||
1968 | if (y < 0) | ||
1969 | y = 0; | ||
1970 | |||
1971 | // integer indexs | ||
1972 | int ix; | ||
1973 | int iy; | ||
1974 | // interpolators offset | ||
1975 | float dx; | ||
1976 | float dy; | ||
1977 | |||
1978 | int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples | ||
1979 | int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples | ||
1980 | int regsize = regsizeX; | ||
1981 | |||
1982 | if (OdeUbitLib) | ||
1983 | { | ||
1984 | if (x < regsizeX - 1) | ||
1985 | { | ||
1986 | ix = (int)x; | ||
1987 | dx = x - (float)ix; | ||
1988 | } | ||
1989 | else // out world use external height | ||
1990 | { | ||
1991 | ix = regsizeX - 2; | ||
1992 | dx = 0; | ||
1993 | } | ||
1994 | if (y < regsizeY - 1) | ||
1995 | { | ||
1996 | iy = (int)y; | ||
1997 | dy = y - (float)iy; | ||
1998 | } | ||
1999 | else | ||
2000 | { | ||
2001 | iy = regsizeY - 2; | ||
2002 | dy = 0; | ||
2003 | } | ||
2004 | } | ||
2005 | else | ||
2006 | { | ||
2007 | // we still have square fixed size regions | ||
2008 | // also flip x and y because of how map is done for ODE fliped axis | ||
2009 | // so ix,iy,dx and dy are inter exchanged | ||
2010 | |||
2011 | regsize = regsizeY; | ||
2012 | |||
2013 | if (x < regsizeX - 1) | ||
2014 | { | ||
2015 | iy = (int)x; | ||
2016 | dy = x - (float)iy; | ||
2017 | } | ||
2018 | else // out world use external height | ||
2019 | { | ||
2020 | iy = regsizeX - 2; | ||
2021 | dy = 0; | ||
2022 | } | ||
2023 | if (y < regsizeY - 1) | ||
2024 | { | ||
2025 | ix = (int)y; | ||
2026 | dx = y - (float)ix; | ||
2027 | } | ||
2028 | else | ||
2029 | { | ||
2030 | ix = regsizeY - 2; | ||
2031 | dx = 0; | ||
2032 | } | ||
2033 | } | ||
2034 | |||
2035 | float h0; | ||
2036 | float h1; | ||
2037 | float h2; | ||
2038 | |||
2039 | iy *= regsize; | ||
2040 | iy += ix; // all indexes have iy + ix | ||
2041 | |||
2042 | float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; | ||
2043 | /* | ||
2044 | if ((dx + dy) <= 1.0f) | ||
2045 | { | ||
2046 | h0 = ((float)heights[iy]); // 0,0 vertice | ||
2047 | h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 | ||
2048 | h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 | ||
2049 | } | ||
2050 | else | ||
2051 | { | ||
2052 | h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice | ||
2053 | h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 | ||
2054 | h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 | ||
2055 | } | ||
2056 | */ | ||
2057 | h0 = ((float)heights[iy]); // 0,0 vertice | ||
2058 | |||
2059 | if (dy>dx) | ||
2060 | { | ||
2061 | iy += regsize; | ||
2062 | h2 = (float)heights[iy]; // 0,1 vertice | ||
2063 | h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 | ||
2064 | h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 | ||
2065 | } | ||
2066 | else | ||
2067 | { | ||
2068 | iy++; | ||
2069 | h2 = (float)heights[iy]; // vertice 1,0 | ||
2070 | h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 | ||
2071 | h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 | ||
2072 | } | ||
2073 | |||
2074 | return h0 + h1 + h2; | ||
2075 | } | ||
2076 | |||
2077 | public Vector3 GetTerrainNormalAtXY(float x, float y) | ||
2078 | { | ||
2079 | int offsetX = 0; | ||
2080 | int offsetY = 0; | ||
2081 | |||
2082 | if (m_suportCombine) | ||
2083 | { | ||
2084 | offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
2085 | offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
2086 | } | ||
2087 | |||
2088 | // get region map | ||
2089 | IntPtr heightFieldGeom = IntPtr.Zero; | ||
2090 | Vector3 norm = new Vector3(0, 0, 1); | ||
2091 | |||
2092 | if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) | ||
2093 | return norm; ; | ||
2094 | |||
2095 | if (heightFieldGeom == IntPtr.Zero) | ||
2096 | return norm; | ||
2097 | |||
2098 | if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) | ||
2099 | return norm; | ||
2100 | |||
2101 | // TerrainHeightField for ODE as offset 1m | ||
2102 | x += 1f - offsetX; | ||
2103 | y += 1f - offsetY; | ||
2104 | |||
2105 | // make position fit into array | ||
2106 | if (x < 0) | ||
2107 | x = 0; | ||
2108 | if (y < 0) | ||
2109 | y = 0; | ||
2110 | |||
2111 | // integer indexs | ||
2112 | int ix; | ||
2113 | int iy; | ||
2114 | // interpolators offset | ||
2115 | float dx; | ||
2116 | float dy; | ||
2117 | |||
2118 | int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples | ||
2119 | int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples | ||
2120 | int regsize = regsizeX; | ||
2121 | |||
2122 | int xstep = 1; | ||
2123 | int ystep = regsizeX; | ||
2124 | bool firstTri = false; | ||
2125 | |||
2126 | if (OdeUbitLib) | ||
2127 | { | ||
2128 | if (x < regsizeX - 1) | ||
2129 | { | ||
2130 | ix = (int)x; | ||
2131 | dx = x - (float)ix; | ||
2132 | } | ||
2133 | else // out world use external height | ||
2134 | { | ||
2135 | ix = regsizeX - 2; | ||
2136 | dx = 0; | ||
2137 | } | ||
2138 | if (y < regsizeY - 1) | ||
2139 | { | ||
2140 | iy = (int)y; | ||
2141 | dy = y - (float)iy; | ||
2142 | } | ||
2143 | else | ||
2144 | { | ||
2145 | iy = regsizeY - 2; | ||
2146 | dy = 0; | ||
2147 | } | ||
2148 | firstTri = dy > dx; | ||
2149 | } | ||
2150 | |||
2151 | else | ||
2152 | { | ||
2153 | xstep = regsizeY; | ||
2154 | ystep = 1; | ||
2155 | regsize = regsizeY; | ||
2156 | |||
2157 | // we still have square fixed size regions | ||
2158 | // also flip x and y because of how map is done for ODE fliped axis | ||
2159 | // so ix,iy,dx and dy are inter exchanged | ||
2160 | if (x < regsizeX - 1) | ||
2161 | { | ||
2162 | iy = (int)x; | ||
2163 | dy = x - (float)iy; | ||
2164 | } | ||
2165 | else // out world use external height | ||
2166 | { | ||
2167 | iy = regsizeX - 2; | ||
2168 | dy = 0; | ||
2169 | } | ||
2170 | if (y < regsizeY - 1) | ||
2171 | { | ||
2172 | ix = (int)y; | ||
2173 | dx = y - (float)ix; | ||
2174 | } | ||
2175 | else | ||
2176 | { | ||
2177 | ix = regsizeY - 2; | ||
2178 | dx = 0; | ||
2179 | } | ||
2180 | firstTri = dx > dy; | ||
2181 | } | ||
2182 | |||
2183 | float h0; | ||
2184 | float h1; | ||
2185 | float h2; | ||
2186 | |||
2187 | iy *= regsize; | ||
2188 | iy += ix; // all indexes have iy + ix | ||
2189 | |||
2190 | float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; | ||
2191 | |||
2192 | if (firstTri) | ||
2193 | { | ||
2194 | h1 = ((float)heights[iy]); // 0,0 vertice | ||
2195 | iy += ystep; | ||
2196 | h0 = (float)heights[iy]; // 0,1 | ||
2197 | h2 = (float)heights[iy+xstep]; // 1,1 vertice | ||
2198 | norm.X = h0 - h2; | ||
2199 | norm.Y = h1 - h0; | ||
2200 | } | ||
2201 | else | ||
2202 | { | ||
2203 | h2 = ((float)heights[iy]); // 0,0 vertice | ||
2204 | iy += xstep; | ||
2205 | h0 = ((float)heights[iy]); // 1,0 vertice | ||
2206 | h1 = (float)heights[iy+ystep]; // vertice 1,1 | ||
2207 | norm.X = h2 - h0; | ||
2208 | norm.Y = h0 - h1; | ||
2209 | } | ||
2210 | norm.Z = 1; | ||
2211 | norm.Normalize(); | ||
2212 | return norm; | ||
2213 | } | ||
2214 | |||
2215 | public override void SetTerrain(float[] heightMap) | ||
2216 | { | ||
2217 | if (m_worldOffset != Vector3.Zero && m_parentScene != null) | ||
2218 | { | ||
2219 | if (m_parentScene is OdeScene) | ||
2220 | { | ||
2221 | ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); | ||
2222 | } | ||
2223 | } | ||
2224 | else | ||
2225 | { | ||
2226 | SetTerrain(heightMap, m_worldOffset); | ||
2227 | } | ||
2228 | } | ||
2229 | |||
2230 | public override void CombineTerrain(float[] heightMap, Vector3 pOffset) | ||
2231 | { | ||
2232 | if(m_suportCombine) | ||
2233 | SetTerrain(heightMap, pOffset); | ||
2234 | } | ||
2235 | |||
2236 | public void SetTerrain(float[] heightMap, Vector3 pOffset) | ||
2237 | { | ||
2238 | if (OdeUbitLib) | ||
2239 | UbitSetTerrain(heightMap, pOffset); | ||
2240 | else | ||
2241 | OriSetTerrain(heightMap, pOffset); | ||
2242 | } | ||
2243 | |||
2244 | public void OriSetTerrain(float[] heightMap, Vector3 pOffset) | ||
2245 | { | ||
2246 | // assumes 1m size grid and constante size square regions | ||
2247 | // needs to know about sims around in future | ||
2248 | |||
2249 | float[] _heightmap; | ||
2250 | |||
2251 | uint regionsizeX = m_regionWidth; | ||
2252 | uint regionsizeY = m_regionHeight; | ||
2253 | |||
2254 | // map is rotated | ||
2255 | uint heightmapWidth = regionsizeY + 2; | ||
2256 | uint heightmapHeight = regionsizeX + 2; | ||
2257 | |||
2258 | uint heightmapWidthSamples = heightmapWidth + 1; | ||
2259 | uint heightmapHeightSamples = heightmapHeight + 1; | ||
2260 | |||
2261 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; | ||
2262 | |||
2263 | const float scale = 1.0f; | ||
2264 | const float offset = 0.0f; | ||
2265 | const float thickness = 10f; | ||
2266 | const int wrap = 0; | ||
2267 | |||
2268 | |||
2269 | float hfmin = float.MaxValue; | ||
2270 | float hfmax = float.MinValue; | ||
2271 | float val; | ||
2272 | uint xx; | ||
2273 | uint yy; | ||
2274 | |||
2275 | uint maxXX = regionsizeX - 1; | ||
2276 | uint maxYY = regionsizeY - 1; | ||
2277 | // flipping map adding one margin all around so things don't fall in edges | ||
2278 | |||
2279 | uint xt = 0; | ||
2280 | xx = 0; | ||
2281 | |||
2282 | for (uint x = 0; x < heightmapWidthSamples; x++) | ||
2283 | { | ||
2284 | if (x > 1 && xx < maxXX) | ||
2285 | xx++; | ||
2286 | yy = 0; | ||
2287 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
2288 | { | ||
2289 | if (y > 1 && y < maxYY) | ||
2290 | yy += regionsizeX; | ||
2291 | |||
2292 | val = heightMap[yy + xx]; | ||
2293 | if (val < 0.0f) | ||
2294 | val = 0.0f; // no neg terrain as in chode | ||
2295 | _heightmap[xt + y] = val; | ||
2296 | |||
2297 | if (hfmin > val) | ||
2298 | hfmin = val; | ||
2299 | if (hfmax < val) | ||
2300 | hfmax = val; | ||
2301 | } | ||
2302 | xt += heightmapHeightSamples; | ||
2303 | } | ||
2304 | |||
2305 | lock (OdeLock) | ||
2306 | { | ||
2307 | IntPtr GroundGeom = IntPtr.Zero; | ||
2308 | if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) | ||
2309 | { | ||
2310 | RegionTerrain.Remove(pOffset); | ||
2311 | if (GroundGeom != IntPtr.Zero) | ||
2312 | { | ||
2313 | actor_name_map.Remove(GroundGeom); | ||
2314 | d.GeomDestroy(GroundGeom); | ||
2315 | |||
2316 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | ||
2317 | { | ||
2318 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2319 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
2320 | TerrainHeightFieldHeights.Remove(GroundGeom); | ||
2321 | } | ||
2322 | } | ||
2323 | } | ||
2324 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); | ||
2325 | |||
2326 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); | ||
2327 | |||
2328 | d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, | ||
2329 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, | ||
2330 | offset, thickness, wrap); | ||
2331 | |||
2332 | d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | ||
2333 | |||
2334 | GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); | ||
2335 | |||
2336 | if (GroundGeom != IntPtr.Zero) | ||
2337 | { | ||
2338 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); | ||
2339 | d.GeomSetCollideBits(GroundGeom, 0); | ||
2340 | |||
2341 | PhysicsActor pa = new NullPhysicsActor(); | ||
2342 | pa.Name = "Terrain"; | ||
2343 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2344 | actor_name_map[GroundGeom] = pa; | ||
2345 | |||
2346 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2347 | |||
2348 | d.Matrix3 R = new d.Matrix3(); | ||
2349 | |||
2350 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); | ||
2351 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); | ||
2352 | |||
2353 | |||
2354 | q1 = q1 * q2; | ||
2355 | |||
2356 | Vector3 v3; | ||
2357 | float angle; | ||
2358 | q1.GetAxisAngle(out v3, out angle); | ||
2359 | |||
2360 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | ||
2361 | d.GeomSetRotation(GroundGeom, ref R); | ||
2362 | d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); | ||
2363 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2364 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2365 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
2366 | } | ||
2367 | } | ||
2368 | } | ||
2369 | |||
2370 | public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) | ||
2371 | { | ||
2372 | // assumes 1m size grid and constante size square regions | ||
2373 | // needs to know about sims around in future | ||
2374 | |||
2375 | float[] _heightmap; | ||
2376 | |||
2377 | uint regionsizeX = m_regionWidth; | ||
2378 | uint regionsizeY = m_regionHeight; | ||
2379 | |||
2380 | uint heightmapWidth = regionsizeX + 2; | ||
2381 | uint heightmapHeight = regionsizeY + 2; | ||
2382 | |||
2383 | uint heightmapWidthSamples = heightmapWidth + 1; | ||
2384 | uint heightmapHeightSamples = heightmapHeight + 1; | ||
2385 | |||
2386 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; | ||
2387 | |||
2388 | |||
2389 | float hfmin = float.MaxValue; | ||
2390 | // float hfmax = float.MinValue; | ||
2391 | float val; | ||
2392 | |||
2393 | |||
2394 | uint maxXX = regionsizeX - 1; | ||
2395 | uint maxYY = regionsizeY - 1; | ||
2396 | // adding one margin all around so things don't fall in edges | ||
2397 | |||
2398 | uint xx; | ||
2399 | uint yy = 0; | ||
2400 | uint yt = 0; | ||
2401 | |||
2402 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
2403 | { | ||
2404 | if (y > 1 && y < maxYY) | ||
2405 | yy += regionsizeX; | ||
2406 | xx = 0; | ||
2407 | for (uint x = 0; x < heightmapWidthSamples; x++) | ||
2408 | { | ||
2409 | if (x > 1 && x < maxXX) | ||
2410 | xx++; | ||
2411 | |||
2412 | val = heightMap[yy + xx]; | ||
2413 | if (val < 0.0f) | ||
2414 | val = 0.0f; // no neg terrain as in chode | ||
2415 | _heightmap[yt + x] = val; | ||
2416 | |||
2417 | if (hfmin > val) | ||
2418 | hfmin = val; | ||
2419 | // if (hfmax < val) | ||
2420 | // hfmax = val; | ||
2421 | } | ||
2422 | yt += heightmapWidthSamples; | ||
2423 | } | ||
2424 | lock (OdeLock) | ||
2425 | { | ||
2426 | IntPtr GroundGeom = IntPtr.Zero; | ||
2427 | if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) | ||
2428 | { | ||
2429 | RegionTerrain.Remove(pOffset); | ||
2430 | if (GroundGeom != IntPtr.Zero) | ||
2431 | { | ||
2432 | actor_name_map.Remove(GroundGeom); | ||
2433 | d.GeomDestroy(GroundGeom); | ||
2434 | |||
2435 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | ||
2436 | { | ||
2437 | if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) | ||
2438 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2439 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
2440 | TerrainHeightFieldHeights.Remove(GroundGeom); | ||
2441 | } | ||
2442 | } | ||
2443 | } | ||
2444 | IntPtr HeightmapData = d.GeomUbitTerrainDataCreate(); | ||
2445 | |||
2446 | const int wrap = 0; | ||
2447 | float thickness = hfmin; | ||
2448 | if (thickness < 0) | ||
2449 | thickness = 1; | ||
2450 | |||
2451 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); | ||
2452 | |||
2453 | d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, | ||
2454 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, | ||
2455 | thickness, wrap); | ||
2456 | |||
2457 | // d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | ||
2458 | GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); | ||
2459 | if (GroundGeom != IntPtr.Zero) | ||
2460 | { | ||
2461 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); | ||
2462 | d.GeomSetCollideBits(GroundGeom, 0); | ||
2463 | |||
2464 | |||
2465 | PhysicsActor pa = new NullPhysicsActor(); | ||
2466 | pa.Name = "Terrain"; | ||
2467 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2468 | actor_name_map[GroundGeom] = pa; | ||
2469 | |||
2470 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2471 | |||
2472 | d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); | ||
2473 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2474 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2475 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
2476 | } | ||
2477 | } | ||
2478 | } | ||
2479 | |||
2480 | |||
2481 | public override void DeleteTerrain() | ||
2482 | { | ||
2483 | } | ||
2484 | |||
2485 | public float GetWaterLevel() | ||
2486 | { | ||
2487 | return waterlevel; | ||
2488 | } | ||
2489 | |||
2490 | public override bool SupportsCombining() | ||
2491 | { | ||
2492 | return m_suportCombine; | ||
2493 | } | ||
2494 | /* | ||
2495 | public override void UnCombine(PhysicsScene pScene) | ||
2496 | { | ||
2497 | IntPtr localGround = IntPtr.Zero; | ||
2498 | // float[] localHeightfield; | ||
2499 | bool proceed = false; | ||
2500 | List<IntPtr> geomDestroyList = new List<IntPtr>(); | ||
2501 | |||
2502 | lock (OdeLock) | ||
2503 | { | ||
2504 | if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) | ||
2505 | { | ||
2506 | foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) | ||
2507 | { | ||
2508 | if (geom == localGround) | ||
2509 | { | ||
2510 | // localHeightfield = TerrainHeightFieldHeights[geom]; | ||
2511 | proceed = true; | ||
2512 | } | ||
2513 | else | ||
2514 | { | ||
2515 | geomDestroyList.Add(geom); | ||
2516 | } | ||
2517 | } | ||
2518 | |||
2519 | if (proceed) | ||
2520 | { | ||
2521 | m_worldOffset = Vector3.Zero; | ||
2522 | WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | ||
2523 | m_parentScene = null; | ||
2524 | |||
2525 | foreach (IntPtr g in geomDestroyList) | ||
2526 | { | ||
2527 | // removingHeightField needs to be done or the garbage collector will | ||
2528 | // collect the terrain data before we tell ODE to destroy it causing | ||
2529 | // memory corruption | ||
2530 | if (TerrainHeightFieldHeights.ContainsKey(g)) | ||
2531 | { | ||
2532 | // float[] removingHeightField = TerrainHeightFieldHeights[g]; | ||
2533 | TerrainHeightFieldHeights.Remove(g); | ||
2534 | |||
2535 | if (RegionTerrain.ContainsKey(g)) | ||
2536 | { | ||
2537 | RegionTerrain.Remove(g); | ||
2538 | } | ||
2539 | |||
2540 | d.GeomDestroy(g); | ||
2541 | //removingHeightField = new float[0]; | ||
2542 | } | ||
2543 | } | ||
2544 | |||
2545 | } | ||
2546 | else | ||
2547 | { | ||
2548 | m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); | ||
2549 | } | ||
2550 | } | ||
2551 | } | ||
2552 | } | ||
2553 | */ | ||
2554 | public override void SetWaterLevel(float baseheight) | ||
2555 | { | ||
2556 | waterlevel = baseheight; | ||
2557 | } | ||
2558 | |||
2559 | public override void Dispose() | ||
2560 | { | ||
2561 | if (m_meshWorker != null) | ||
2562 | m_meshWorker.Stop(); | ||
2563 | |||
2564 | lock (OdeLock) | ||
2565 | { | ||
2566 | m_rayCastManager.Dispose(); | ||
2567 | m_rayCastManager = null; | ||
2568 | |||
2569 | lock (_prims) | ||
2570 | { | ||
2571 | ChangesQueue.Clear(); | ||
2572 | foreach (OdePrim prm in _prims) | ||
2573 | { | ||
2574 | prm.DoAChange(changes.Remove, null); | ||
2575 | _collisionEventPrim.Remove(prm); | ||
2576 | } | ||
2577 | _prims.Clear(); | ||
2578 | } | ||
2579 | |||
2580 | OdeCharacter[] chtorem; | ||
2581 | lock (_characters) | ||
2582 | { | ||
2583 | chtorem = new OdeCharacter[_characters.Count]; | ||
2584 | _characters.CopyTo(chtorem); | ||
2585 | } | ||
2586 | |||
2587 | ChangesQueue.Clear(); | ||
2588 | foreach (OdeCharacter ch in chtorem) | ||
2589 | ch.DoAChange(changes.Remove, null); | ||
2590 | |||
2591 | |||
2592 | foreach (IntPtr GroundGeom in RegionTerrain.Values) | ||
2593 | { | ||
2594 | if (GroundGeom != IntPtr.Zero) | ||
2595 | d.GeomDestroy(GroundGeom); | ||
2596 | } | ||
2597 | |||
2598 | |||
2599 | RegionTerrain.Clear(); | ||
2600 | |||
2601 | if (TerrainHeightFieldHeightsHandlers.Count > 0) | ||
2602 | { | ||
2603 | foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) | ||
2604 | { | ||
2605 | if (gch.IsAllocated) | ||
2606 | gch.Free(); | ||
2607 | } | ||
2608 | } | ||
2609 | |||
2610 | TerrainHeightFieldHeightsHandlers.Clear(); | ||
2611 | TerrainHeightFieldHeights.Clear(); | ||
2612 | |||
2613 | if (ContactgeomsArray != IntPtr.Zero) | ||
2614 | Marshal.FreeHGlobal(ContactgeomsArray); | ||
2615 | if (GlobalContactsArray != IntPtr.Zero) | ||
2616 | Marshal.FreeHGlobal(GlobalContactsArray); | ||
2617 | |||
2618 | |||
2619 | d.WorldDestroy(world); | ||
2620 | world = IntPtr.Zero; | ||
2621 | //d.CloseODE(); | ||
2622 | } | ||
2623 | } | ||
2624 | |||
2625 | public override Dictionary<uint, float> GetTopColliders() | ||
2626 | { | ||
2627 | Dictionary<uint, float> returncolliders = new Dictionary<uint, float>(); | ||
2628 | int cnt = 0; | ||
2629 | lock (_prims) | ||
2630 | { | ||
2631 | foreach (OdePrim prm in _prims) | ||
2632 | { | ||
2633 | if (prm.CollisionScore > 0) | ||
2634 | { | ||
2635 | returncolliders.Add(prm.LocalID, prm.CollisionScore); | ||
2636 | cnt++; | ||
2637 | prm.CollisionScore = 0f; | ||
2638 | if (cnt > 25) | ||
2639 | { | ||
2640 | break; | ||
2641 | } | ||
2642 | } | ||
2643 | } | ||
2644 | } | ||
2645 | return returncolliders; | ||
2646 | } | ||
2647 | |||
2648 | public override bool SupportsRayCast() | ||
2649 | { | ||
2650 | return true; | ||
2651 | } | ||
2652 | |||
2653 | public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) | ||
2654 | { | ||
2655 | if (retMethod != null) | ||
2656 | { | ||
2657 | ODERayRequest req = new ODERayRequest(); | ||
2658 | req.actor = null; | ||
2659 | req.callbackMethod = retMethod; | ||
2660 | req.length = length; | ||
2661 | req.Normal = direction; | ||
2662 | req.Origin = position; | ||
2663 | req.Count = 0; | ||
2664 | req.filter = RayFilterFlags.AllPrims; | ||
2665 | |||
2666 | m_rayCastManager.QueueRequest(req); | ||
2667 | } | ||
2668 | } | ||
2669 | |||
2670 | public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) | ||
2671 | { | ||
2672 | if (retMethod != null) | ||
2673 | { | ||
2674 | ODERayRequest req = new ODERayRequest(); | ||
2675 | req.actor = null; | ||
2676 | req.callbackMethod = retMethod; | ||
2677 | req.length = length; | ||
2678 | req.Normal = direction; | ||
2679 | req.Origin = position; | ||
2680 | req.Count = Count; | ||
2681 | req.filter = RayFilterFlags.AllPrims; | ||
2682 | |||
2683 | m_rayCastManager.QueueRequest(req); | ||
2684 | } | ||
2685 | } | ||
2686 | |||
2687 | |||
2688 | public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) | ||
2689 | { | ||
2690 | List<ContactResult> ourresults = new List<ContactResult>(); | ||
2691 | object SyncObject = new object(); | ||
2692 | |||
2693 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2694 | { | ||
2695 | lock (SyncObject) | ||
2696 | { | ||
2697 | ourresults = results; | ||
2698 | Monitor.PulseAll(SyncObject); | ||
2699 | } | ||
2700 | }; | ||
2701 | |||
2702 | ODERayRequest req = new ODERayRequest(); | ||
2703 | req.actor = null; | ||
2704 | req.callbackMethod = retMethod; | ||
2705 | req.length = length; | ||
2706 | req.Normal = direction; | ||
2707 | req.Origin = position; | ||
2708 | req.Count = Count; | ||
2709 | req.filter = RayFilterFlags.AllPrims; | ||
2710 | |||
2711 | lock (SyncObject) | ||
2712 | { | ||
2713 | m_rayCastManager.QueueRequest(req); | ||
2714 | if (!Monitor.Wait(SyncObject, 500)) | ||
2715 | return null; | ||
2716 | else | ||
2717 | return ourresults; | ||
2718 | } | ||
2719 | } | ||
2720 | |||
2721 | public override bool SupportsRaycastWorldFiltered() | ||
2722 | { | ||
2723 | return true; | ||
2724 | } | ||
2725 | |||
2726 | public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) | ||
2727 | { | ||
2728 | object SyncObject = new object(); | ||
2729 | List<ContactResult> ourresults = new List<ContactResult>(); | ||
2730 | |||
2731 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2732 | { | ||
2733 | lock (SyncObject) | ||
2734 | { | ||
2735 | ourresults = results; | ||
2736 | Monitor.PulseAll(SyncObject); | ||
2737 | } | ||
2738 | }; | ||
2739 | |||
2740 | ODERayRequest req = new ODERayRequest(); | ||
2741 | req.actor = null; | ||
2742 | req.callbackMethod = retMethod; | ||
2743 | req.length = length; | ||
2744 | req.Normal = direction; | ||
2745 | req.Origin = position; | ||
2746 | req.Count = Count; | ||
2747 | req.filter = filter; | ||
2748 | |||
2749 | lock (SyncObject) | ||
2750 | { | ||
2751 | m_rayCastManager.QueueRequest(req); | ||
2752 | if (!Monitor.Wait(SyncObject, 500)) | ||
2753 | return null; | ||
2754 | else | ||
2755 | return ourresults; | ||
2756 | } | ||
2757 | } | ||
2758 | |||
2759 | public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) | ||
2760 | { | ||
2761 | if (actor == null) | ||
2762 | return new List<ContactResult>(); | ||
2763 | |||
2764 | IntPtr geom; | ||
2765 | if (actor is OdePrim) | ||
2766 | geom = ((OdePrim)actor).prim_geom; | ||
2767 | else if (actor is OdeCharacter) | ||
2768 | geom = ((OdePrim)actor).prim_geom; | ||
2769 | else | ||
2770 | return new List<ContactResult>(); | ||
2771 | |||
2772 | if (geom == IntPtr.Zero) | ||
2773 | return new List<ContactResult>(); | ||
2774 | |||
2775 | List<ContactResult> ourResults = null; | ||
2776 | object SyncObject = new object(); | ||
2777 | |||
2778 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2779 | { | ||
2780 | lock (SyncObject) | ||
2781 | { | ||
2782 | ourResults = results; | ||
2783 | Monitor.PulseAll(SyncObject); | ||
2784 | } | ||
2785 | }; | ||
2786 | |||
2787 | ODERayRequest req = new ODERayRequest(); | ||
2788 | req.actor = actor; | ||
2789 | req.callbackMethod = retMethod; | ||
2790 | req.length = length; | ||
2791 | req.Normal = direction; | ||
2792 | req.Origin = position; | ||
2793 | req.Count = Count; | ||
2794 | req.filter = flags; | ||
2795 | |||
2796 | lock (SyncObject) | ||
2797 | { | ||
2798 | m_rayCastManager.QueueRequest(req); | ||
2799 | if (!Monitor.Wait(SyncObject, 500)) | ||
2800 | return new List<ContactResult>(); | ||
2801 | } | ||
2802 | |||
2803 | if (ourResults == null) | ||
2804 | return new List<ContactResult>(); | ||
2805 | return ourResults; | ||
2806 | } | ||
2807 | |||
2808 | public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) | ||
2809 | { | ||
2810 | List<ContactResult> ourResults = null; | ||
2811 | object SyncObject = new object(); | ||
2812 | |||
2813 | ProbeBoxCallback retMethod = delegate(List<ContactResult> results) | ||
2814 | { | ||
2815 | lock (SyncObject) | ||
2816 | { | ||
2817 | ourResults = results; | ||
2818 | Monitor.PulseAll(SyncObject); | ||
2819 | } | ||
2820 | }; | ||
2821 | |||
2822 | ODERayRequest req = new ODERayRequest(); | ||
2823 | req.actor = null; | ||
2824 | req.callbackMethod = retMethod; | ||
2825 | req.Normal = size; | ||
2826 | req.Origin = position; | ||
2827 | req.orientation = orientation; | ||
2828 | req.Count = Count; | ||
2829 | req.filter = flags; | ||
2830 | |||
2831 | lock (SyncObject) | ||
2832 | { | ||
2833 | m_rayCastManager.QueueRequest(req); | ||
2834 | if (!Monitor.Wait(SyncObject, 500)) | ||
2835 | return new List<ContactResult>(); | ||
2836 | } | ||
2837 | |||
2838 | if (ourResults == null) | ||
2839 | return new List<ContactResult>(); | ||
2840 | return ourResults; | ||
2841 | } | ||
2842 | |||
2843 | public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) | ||
2844 | { | ||
2845 | List<ContactResult> ourResults = null; | ||
2846 | object SyncObject = new object(); | ||
2847 | |||
2848 | ProbeSphereCallback retMethod = delegate(List<ContactResult> results) | ||
2849 | { | ||
2850 | ourResults = results; | ||
2851 | Monitor.PulseAll(SyncObject); | ||
2852 | }; | ||
2853 | |||
2854 | ODERayRequest req = new ODERayRequest(); | ||
2855 | req.actor = null; | ||
2856 | req.callbackMethod = retMethod; | ||
2857 | req.length = radius; | ||
2858 | req.Origin = position; | ||
2859 | req.Count = Count; | ||
2860 | req.filter = flags; | ||
2861 | |||
2862 | |||
2863 | lock (SyncObject) | ||
2864 | { | ||
2865 | m_rayCastManager.QueueRequest(req); | ||
2866 | if (!Monitor.Wait(SyncObject, 500)) | ||
2867 | return new List<ContactResult>(); | ||
2868 | } | ||
2869 | |||
2870 | if (ourResults == null) | ||
2871 | return new List<ContactResult>(); | ||
2872 | return ourResults; | ||
2873 | } | ||
2874 | |||
2875 | public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) | ||
2876 | { | ||
2877 | IntPtr geom = IntPtr.Zero;; | ||
2878 | |||
2879 | if (actor != null) | ||
2880 | { | ||
2881 | if (actor is OdePrim) | ||
2882 | geom = ((OdePrim)actor).prim_geom; | ||
2883 | else if (actor is OdeCharacter) | ||
2884 | geom = ((OdePrim)actor).prim_geom; | ||
2885 | } | ||
2886 | |||
2887 | List<ContactResult> ourResults = null; | ||
2888 | object SyncObject = new object(); | ||
2889 | |||
2890 | ProbePlaneCallback retMethod = delegate(List<ContactResult> results) | ||
2891 | { | ||
2892 | ourResults = results; | ||
2893 | Monitor.PulseAll(SyncObject); | ||
2894 | }; | ||
2895 | |||
2896 | ODERayRequest req = new ODERayRequest(); | ||
2897 | req.actor = null; | ||
2898 | req.callbackMethod = retMethod; | ||
2899 | req.length = plane.W; | ||
2900 | req.Normal.X = plane.X; | ||
2901 | req.Normal.Y = plane.Y; | ||
2902 | req.Normal.Z = plane.Z; | ||
2903 | req.Count = Count; | ||
2904 | req.filter = flags; | ||
2905 | |||
2906 | lock (SyncObject) | ||
2907 | { | ||
2908 | m_rayCastManager.QueueRequest(req); | ||
2909 | if (!Monitor.Wait(SyncObject, 500)) | ||
2910 | return new List<ContactResult>(); | ||
2911 | } | ||
2912 | |||
2913 | if (ourResults == null) | ||
2914 | return new List<ContactResult>(); | ||
2915 | return ourResults; | ||
2916 | } | ||
2917 | |||
2918 | public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) | ||
2919 | { | ||
2920 | Util.FireAndForget( delegate | ||
2921 | { | ||
2922 | ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); | ||
2923 | if(sitAvatar != null) | ||
2924 | sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); | ||
2925 | }); | ||
2926 | return 1; | ||
2927 | } | ||
2928 | |||
2929 | } | ||
2930 | } | ||