diff options
author | David Walter Seikel | 2016-11-03 21:44:39 +1000 |
---|---|---|
committer | David Walter Seikel | 2016-11-03 21:44:39 +1000 |
commit | 134f86e8d5c414409631b25b8c6f0ee45fbd8631 (patch) | |
tree | 216b89d3fb89acfb81be1e440c25c41ab09fa96d /OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | |
parent | More changing to production grid. Double oops. (diff) | |
download | opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.zip opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.gz opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.bz2 opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.xz |
Initial update to OpenSim 0.8.2.1 source code.
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODECharacter.cs')
-rw-r--r-- | OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 1411 |
1 files changed, 0 insertions, 1411 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs deleted file mode 100644 index 319f6ab..0000000 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ /dev/null | |||
@@ -1,1411 +0,0 @@ | |||
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 OpenMetaverse; | ||
32 | using Ode.NET; | ||
33 | using OpenSim.Framework; | ||
34 | using OpenSim.Region.Physics.Manager; | ||
35 | using log4net; | ||
36 | |||
37 | namespace OpenSim.Region.Physics.OdePlugin | ||
38 | { | ||
39 | /// <summary> | ||
40 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
41 | /// </summary> | ||
42 | public enum dParam : int | ||
43 | { | ||
44 | LowStop = 0, | ||
45 | HiStop = 1, | ||
46 | Vel = 2, | ||
47 | FMax = 3, | ||
48 | FudgeFactor = 4, | ||
49 | Bounce = 5, | ||
50 | CFM = 6, | ||
51 | StopERP = 7, | ||
52 | StopCFM = 8, | ||
53 | LoStop2 = 256, | ||
54 | HiStop2 = 257, | ||
55 | Vel2 = 258, | ||
56 | FMax2 = 259, | ||
57 | StopERP2 = 7 + 256, | ||
58 | StopCFM2 = 8 + 256, | ||
59 | LoStop3 = 512, | ||
60 | HiStop3 = 513, | ||
61 | Vel3 = 514, | ||
62 | FMax3 = 515, | ||
63 | StopERP3 = 7 + 512, | ||
64 | StopCFM3 = 8 + 512 | ||
65 | } | ||
66 | |||
67 | public class OdeCharacter : PhysicsActor | ||
68 | { | ||
69 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
70 | |||
71 | private Vector3 _position; | ||
72 | private d.Vector3 _zeroPosition; | ||
73 | private bool _zeroFlag = false; | ||
74 | private bool m_lastUpdateSent = false; | ||
75 | private Vector3 _velocity; | ||
76 | private Vector3 m_taintTargetVelocity; | ||
77 | private Vector3 _target_velocity; | ||
78 | private Vector3 _acceleration; | ||
79 | private Vector3 m_rotationalVelocity; | ||
80 | private float m_mass = 80f; | ||
81 | private float m_density = 60f; | ||
82 | private bool m_pidControllerActive = true; | ||
83 | private float PID_D = 800.0f; | ||
84 | private float PID_P = 900.0f; | ||
85 | //private static float POSTURE_SERVO = 10000.0f; | ||
86 | private float CAPSULE_RADIUS = 0.37f; | ||
87 | private float CAPSULE_LENGTH = 2.140599f; | ||
88 | private float m_tensor = 3800000f; | ||
89 | // private float heightFudgeFactor = 0.52f; | ||
90 | private float walkDivisor = 1.3f; | ||
91 | private float runDivisor = 0.8f; | ||
92 | private bool flying = false; | ||
93 | private bool m_iscolliding = false; | ||
94 | private bool m_iscollidingGround = false; | ||
95 | private bool m_wascolliding = false; | ||
96 | private bool m_wascollidingGround = false; | ||
97 | private bool m_iscollidingObj = false; | ||
98 | private bool m_alwaysRun = false; | ||
99 | private bool m_hackSentFall = false; | ||
100 | private bool m_hackSentFly = false; | ||
101 | private int m_requestedUpdateFrequency = 0; | ||
102 | private Vector3 m_taintPosition; | ||
103 | internal bool m_avatarplanted = false; | ||
104 | /// <summary> | ||
105 | /// Hold set forces so we can process them outside physics calculations. This prevents race conditions if we set force | ||
106 | /// while calculatios are going on | ||
107 | /// </summary> | ||
108 | private Vector3 m_taintForce; | ||
109 | |||
110 | // taints and their non-tainted counterparts | ||
111 | private bool m_isPhysical = false; // the current physical status | ||
112 | private bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) | ||
113 | internal float MinimumGroundFlightOffset = 3f; | ||
114 | |||
115 | private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. | ||
116 | |||
117 | /// <summary> | ||
118 | /// Base movement for calculating tilt. | ||
119 | /// </summary> | ||
120 | private float m_tiltBaseMovement = (float)Math.Sqrt(2); | ||
121 | |||
122 | /// <summary> | ||
123 | /// Used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider | ||
124 | /// </summary> | ||
125 | private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; | ||
126 | |||
127 | private float m_buoyancy = 0f; | ||
128 | |||
129 | // private CollisionLocker ode; | ||
130 | private bool[] m_colliderarr = new bool[11]; | ||
131 | private bool[] m_colliderGroundarr = new bool[11]; | ||
132 | |||
133 | // Default we're a Character | ||
134 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); | ||
135 | |||
136 | // Default, Collide with Other Geometries, spaces, bodies and characters. | ||
137 | private CollisionCategories m_collisionFlags = (CollisionCategories.Geom | ||
138 | | CollisionCategories.Space | ||
139 | | CollisionCategories.Body | ||
140 | | CollisionCategories.Character | ||
141 | | CollisionCategories.Land); | ||
142 | /// <summary> | ||
143 | /// Body for dynamics simulation | ||
144 | /// </summary> | ||
145 | internal IntPtr Body { get; private set; } | ||
146 | |||
147 | private OdeScene _parent_scene; | ||
148 | |||
149 | /// <summary> | ||
150 | /// Collision geometry | ||
151 | /// </summary> | ||
152 | internal IntPtr Shell { get; private set; } | ||
153 | |||
154 | private IntPtr Amotor = IntPtr.Zero; | ||
155 | private d.Mass ShellMass; | ||
156 | |||
157 | private int m_eventsubscription = 0; | ||
158 | private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
159 | |||
160 | // unique UUID of this character object | ||
161 | internal UUID m_uuid { get; private set; } | ||
162 | internal bool bad = false; | ||
163 | |||
164 | /// <summary> | ||
165 | /// ODE Avatar. | ||
166 | /// </summary> | ||
167 | /// <param name="avName"></param> | ||
168 | /// <param name="parent_scene"></param> | ||
169 | /// <param name="pos"></param> | ||
170 | /// <param name="size"></param> | ||
171 | /// <param name="pid_d"></param> | ||
172 | /// <param name="pid_p"></param> | ||
173 | /// <param name="capsule_radius"></param> | ||
174 | /// <param name="tensor"></param> | ||
175 | /// <param name="density"> | ||
176 | /// Only used right now to return information to LSL. Not actually used to set mass in ODE! | ||
177 | /// </param> | ||
178 | /// <param name="walk_divisor"></param> | ||
179 | /// <param name="rundivisor"></param> | ||
180 | public OdeCharacter( | ||
181 | String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, | ||
182 | float capsule_radius, float tensor, float density, | ||
183 | float walk_divisor, float rundivisor) | ||
184 | { | ||
185 | m_uuid = UUID.Random(); | ||
186 | |||
187 | if (pos.IsFinite()) | ||
188 | { | ||
189 | if (pos.Z > 9999999f) | ||
190 | { | ||
191 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
192 | } | ||
193 | if (pos.Z < -90000f) | ||
194 | { | ||
195 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
196 | } | ||
197 | |||
198 | _position = pos; | ||
199 | m_taintPosition = pos; | ||
200 | } | ||
201 | else | ||
202 | { | ||
203 | _position | ||
204 | = new Vector3( | ||
205 | (float)_parent_scene.WorldExtents.X * 0.5f, | ||
206 | (float)_parent_scene.WorldExtents.Y * 0.5f, | ||
207 | parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); | ||
208 | m_taintPosition = _position; | ||
209 | |||
210 | m_log.WarnFormat("[ODE CHARACTER]: Got NaN Position on Character Create for {0}", avName); | ||
211 | } | ||
212 | |||
213 | _parent_scene = parent_scene; | ||
214 | |||
215 | PID_D = pid_d; | ||
216 | PID_P = pid_p; | ||
217 | CAPSULE_RADIUS = capsule_radius; | ||
218 | m_tensor = tensor; | ||
219 | m_density = density; | ||
220 | // heightFudgeFactor = height_fudge_factor; | ||
221 | walkDivisor = walk_divisor; | ||
222 | runDivisor = rundivisor; | ||
223 | |||
224 | // m_StandUpRotation = | ||
225 | // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f, | ||
226 | // 0.5f); | ||
227 | |||
228 | // We can set taint and actual to be the same here, since the entire character will be set up when the | ||
229 | // m_tainted_isPhysical is processed. | ||
230 | SetTaintedCapsuleLength(size); | ||
231 | CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; | ||
232 | |||
233 | m_isPhysical = false; // current status: no ODE information exists | ||
234 | m_tainted_isPhysical = true; // new tainted status: need to create ODE information | ||
235 | |||
236 | _parent_scene.AddPhysicsActorTaint(this); | ||
237 | |||
238 | Name = avName; | ||
239 | } | ||
240 | |||
241 | public override int PhysicsActorType | ||
242 | { | ||
243 | get { return (int) ActorTypes.Agent; } | ||
244 | set { return; } | ||
245 | } | ||
246 | |||
247 | /// <summary> | ||
248 | /// If this is set, the avatar will move faster | ||
249 | /// </summary> | ||
250 | public override bool SetAlwaysRun | ||
251 | { | ||
252 | get { return m_alwaysRun; } | ||
253 | set { m_alwaysRun = value; } | ||
254 | } | ||
255 | |||
256 | public override bool Grabbed | ||
257 | { | ||
258 | set { return; } | ||
259 | } | ||
260 | |||
261 | public override bool Selected | ||
262 | { | ||
263 | set { return; } | ||
264 | } | ||
265 | |||
266 | public override float Buoyancy | ||
267 | { | ||
268 | get { return m_buoyancy; } | ||
269 | set { m_buoyancy = value; } | ||
270 | } | ||
271 | |||
272 | public override bool FloatOnWater | ||
273 | { | ||
274 | set { return; } | ||
275 | } | ||
276 | |||
277 | public override bool IsPhysical | ||
278 | { | ||
279 | get { return false; } | ||
280 | set { return; } | ||
281 | } | ||
282 | |||
283 | public override bool ThrottleUpdates | ||
284 | { | ||
285 | get { return false; } | ||
286 | set { return; } | ||
287 | } | ||
288 | |||
289 | public override bool Flying | ||
290 | { | ||
291 | get { return flying; } | ||
292 | set | ||
293 | { | ||
294 | flying = value; | ||
295 | // m_log.DebugFormat("[ODE CHARACTER]: Set OdeCharacter Flying to {0}", flying); | ||
296 | } | ||
297 | } | ||
298 | |||
299 | /// <summary> | ||
300 | /// Returns if the avatar is colliding in general. | ||
301 | /// This includes the ground and objects and avatar. | ||
302 | /// </summary> | ||
303 | public override bool IsColliding | ||
304 | { | ||
305 | get { return m_iscolliding; } | ||
306 | set | ||
307 | { | ||
308 | int i; | ||
309 | int truecount = 0; | ||
310 | int falsecount = 0; | ||
311 | |||
312 | if (m_colliderarr.Length >= 10) | ||
313 | { | ||
314 | for (i = 0; i < 10; i++) | ||
315 | { | ||
316 | m_colliderarr[i] = m_colliderarr[i + 1]; | ||
317 | } | ||
318 | } | ||
319 | m_colliderarr[10] = value; | ||
320 | |||
321 | for (i = 0; i < 11; i++) | ||
322 | { | ||
323 | if (m_colliderarr[i]) | ||
324 | { | ||
325 | truecount++; | ||
326 | } | ||
327 | else | ||
328 | { | ||
329 | falsecount++; | ||
330 | } | ||
331 | } | ||
332 | |||
333 | // Equal truecounts and false counts means we're colliding with something. | ||
334 | |||
335 | if (falsecount > 1.2*truecount) | ||
336 | { | ||
337 | m_iscolliding = false; | ||
338 | } | ||
339 | else | ||
340 | { | ||
341 | m_iscolliding = true; | ||
342 | } | ||
343 | |||
344 | if (m_wascolliding != m_iscolliding) | ||
345 | { | ||
346 | //base.SendCollisionUpdate(new CollisionEventUpdate()); | ||
347 | } | ||
348 | |||
349 | m_wascolliding = m_iscolliding; | ||
350 | } | ||
351 | } | ||
352 | |||
353 | /// <summary> | ||
354 | /// Returns if an avatar is colliding with the ground | ||
355 | /// </summary> | ||
356 | public override bool CollidingGround | ||
357 | { | ||
358 | get { return m_iscollidingGround; } | ||
359 | set | ||
360 | { | ||
361 | // Collisions against the ground are not really reliable | ||
362 | // So, to get a consistant value we have to average the current result over time | ||
363 | // Currently we use 1 second = 10 calls to this. | ||
364 | int i; | ||
365 | int truecount = 0; | ||
366 | int falsecount = 0; | ||
367 | |||
368 | if (m_colliderGroundarr.Length >= 10) | ||
369 | { | ||
370 | for (i = 0; i < 10; i++) | ||
371 | { | ||
372 | m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; | ||
373 | } | ||
374 | } | ||
375 | m_colliderGroundarr[10] = value; | ||
376 | |||
377 | for (i = 0; i < 11; i++) | ||
378 | { | ||
379 | if (m_colliderGroundarr[i]) | ||
380 | { | ||
381 | truecount++; | ||
382 | } | ||
383 | else | ||
384 | { | ||
385 | falsecount++; | ||
386 | } | ||
387 | } | ||
388 | |||
389 | // Equal truecounts and false counts means we're colliding with something. | ||
390 | |||
391 | if (falsecount > 1.2*truecount) | ||
392 | { | ||
393 | m_iscollidingGround = false; | ||
394 | } | ||
395 | else | ||
396 | { | ||
397 | m_iscollidingGround = true; | ||
398 | } | ||
399 | if (m_wascollidingGround != m_iscollidingGround) | ||
400 | { | ||
401 | //base.SendCollisionUpdate(new CollisionEventUpdate()); | ||
402 | } | ||
403 | m_wascollidingGround = m_iscollidingGround; | ||
404 | } | ||
405 | } | ||
406 | |||
407 | /// <summary> | ||
408 | /// Returns if the avatar is colliding with an object | ||
409 | /// </summary> | ||
410 | public override bool CollidingObj | ||
411 | { | ||
412 | get { return m_iscollidingObj; } | ||
413 | set | ||
414 | { | ||
415 | m_iscollidingObj = value; | ||
416 | if (value && !m_avatarplanted) | ||
417 | m_pidControllerActive = false; | ||
418 | else | ||
419 | m_pidControllerActive = true; | ||
420 | } | ||
421 | } | ||
422 | |||
423 | /// <summary> | ||
424 | /// turn the PID controller on or off. | ||
425 | /// The PID Controller will turn on all by itself in many situations | ||
426 | /// </summary> | ||
427 | /// <param name="status"></param> | ||
428 | public void SetPidStatus(bool status) | ||
429 | { | ||
430 | m_pidControllerActive = status; | ||
431 | } | ||
432 | |||
433 | public override bool Stopped | ||
434 | { | ||
435 | get { return _zeroFlag; } | ||
436 | } | ||
437 | |||
438 | /// <summary> | ||
439 | /// This 'puts' an avatar somewhere in the physics space. | ||
440 | /// Not really a good choice unless you 'know' it's a good | ||
441 | /// spot otherwise you're likely to orbit the avatar. | ||
442 | /// </summary> | ||
443 | public override Vector3 Position | ||
444 | { | ||
445 | get { return _position; } | ||
446 | set | ||
447 | { | ||
448 | if (Body == IntPtr.Zero || Shell == IntPtr.Zero) | ||
449 | { | ||
450 | if (value.IsFinite()) | ||
451 | { | ||
452 | if (value.Z > 9999999f) | ||
453 | { | ||
454 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
455 | } | ||
456 | if (value.Z < -90000f) | ||
457 | { | ||
458 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
459 | } | ||
460 | |||
461 | m_taintPosition = value; | ||
462 | _parent_scene.AddPhysicsActorTaint(this); | ||
463 | } | ||
464 | else | ||
465 | { | ||
466 | m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Position from Scene on character {0}", Name); | ||
467 | } | ||
468 | } | ||
469 | } | ||
470 | } | ||
471 | |||
472 | public override Vector3 RotationalVelocity | ||
473 | { | ||
474 | get { return m_rotationalVelocity; } | ||
475 | set { m_rotationalVelocity = value; } | ||
476 | } | ||
477 | |||
478 | /// <summary> | ||
479 | /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight | ||
480 | /// and use it to offset landings properly | ||
481 | /// </summary> | ||
482 | public override Vector3 Size | ||
483 | { | ||
484 | get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } | ||
485 | set | ||
486 | { | ||
487 | SetTaintedCapsuleLength(value); | ||
488 | |||
489 | // If we reset velocity here, then an avatar stalls when it crosses a border for the first time | ||
490 | // (as the height of the new root agent is set). | ||
491 | // Velocity = Vector3.Zero; | ||
492 | |||
493 | _parent_scene.AddPhysicsActorTaint(this); | ||
494 | } | ||
495 | } | ||
496 | |||
497 | private void SetTaintedCapsuleLength(Vector3 size) | ||
498 | { | ||
499 | if (size.IsFinite()) | ||
500 | { | ||
501 | m_pidControllerActive = true; | ||
502 | |||
503 | m_tainted_CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; | ||
504 | // m_log.Info("[ODE CHARACTER]: " + CAPSULE_LENGTH); | ||
505 | } | ||
506 | else | ||
507 | { | ||
508 | m_log.WarnFormat("[ODE CHARACTER]: Got a NaN Size for {0} in {1}", Name, _parent_scene.Name); | ||
509 | } | ||
510 | } | ||
511 | |||
512 | private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector) | ||
513 | { | ||
514 | movementVector.Z = 0f; | ||
515 | float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y)); | ||
516 | if (magnitude < 0.1f) return; | ||
517 | |||
518 | // normalize the velocity vector | ||
519 | float invMagnitude = 1.0f / magnitude; | ||
520 | movementVector.X *= invMagnitude; | ||
521 | movementVector.Y *= invMagnitude; | ||
522 | |||
523 | // if we change the capsule heading too often, the capsule can fall down | ||
524 | // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw), | ||
525 | // meaning only 4 possible capsule tilt orientations | ||
526 | if (movementVector.X > 0) | ||
527 | { | ||
528 | // east | ||
529 | if (movementVector.Y > 0) | ||
530 | { | ||
531 | // northeast | ||
532 | movementVector.X = m_tiltBaseMovement; | ||
533 | movementVector.Y = m_tiltBaseMovement; | ||
534 | } | ||
535 | else | ||
536 | { | ||
537 | // southeast | ||
538 | movementVector.X = m_tiltBaseMovement; | ||
539 | movementVector.Y = -m_tiltBaseMovement; | ||
540 | } | ||
541 | } | ||
542 | else | ||
543 | { | ||
544 | // west | ||
545 | if (movementVector.Y > 0) | ||
546 | { | ||
547 | // northwest | ||
548 | movementVector.X = -m_tiltBaseMovement; | ||
549 | movementVector.Y = m_tiltBaseMovement; | ||
550 | } | ||
551 | else | ||
552 | { | ||
553 | // southwest | ||
554 | movementVector.X = -m_tiltBaseMovement; | ||
555 | movementVector.Y = -m_tiltBaseMovement; | ||
556 | } | ||
557 | } | ||
558 | |||
559 | // movementVector.Z is zero | ||
560 | |||
561 | // calculate tilt components based on desired amount of tilt and current (snapped) heading. | ||
562 | // the "-" sign is to force the tilt to be OPPOSITE the direction of movement. | ||
563 | float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane; | ||
564 | float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane; | ||
565 | |||
566 | //m_log.Debug("[ODE CHARACTER]: changing avatar tilt"); | ||
567 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent); | ||
568 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced | ||
569 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent); | ||
570 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop | ||
571 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); | ||
572 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop | ||
573 | } | ||
574 | |||
575 | /// <summary> | ||
576 | /// Uses the capped cyllinder volume formula to calculate the avatar's mass. | ||
577 | /// This may be used in calculations in the scene/scenepresence | ||
578 | /// </summary> | ||
579 | public override float Mass | ||
580 | { | ||
581 | get | ||
582 | { | ||
583 | float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH); | ||
584 | return m_density * AVvolume; | ||
585 | } | ||
586 | } | ||
587 | |||
588 | public override void link(PhysicsActor obj) {} | ||
589 | |||
590 | public override void delink() {} | ||
591 | |||
592 | public override void LockAngularMotion(Vector3 axis) {} | ||
593 | |||
594 | // This code is very useful. Written by DanX0r. We're just not using it right now. | ||
595 | // Commented out to prevent a warning. | ||
596 | // | ||
597 | // private void standupStraight() | ||
598 | // { | ||
599 | // // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air. | ||
600 | // // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you | ||
601 | // // change appearance and when you enter the simulator | ||
602 | // // After this routine is done, the amotor stabilizes much quicker | ||
603 | // d.Vector3 feet; | ||
604 | // d.Vector3 head; | ||
605 | // d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet); | ||
606 | // d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head); | ||
607 | // float posture = head.Z - feet.Z; | ||
608 | |||
609 | // // restoring force proportional to lack of posture: | ||
610 | // float servo = (2.5f - posture) * POSTURE_SERVO; | ||
611 | // d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); | ||
612 | // d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); | ||
613 | // //d.Matrix3 bodyrotation = d.BodyGetRotation(Body); | ||
614 | // //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyFArotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22); | ||
615 | // } | ||
616 | |||
617 | public override Vector3 Force | ||
618 | { | ||
619 | get { return _target_velocity; } | ||
620 | set { return; } | ||
621 | } | ||
622 | |||
623 | public override int VehicleType | ||
624 | { | ||
625 | get { return 0; } | ||
626 | set { return; } | ||
627 | } | ||
628 | |||
629 | public override void VehicleFloatParam(int param, float value) | ||
630 | { | ||
631 | } | ||
632 | |||
633 | public override void VehicleVectorParam(int param, Vector3 value) | ||
634 | { | ||
635 | } | ||
636 | |||
637 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
638 | { | ||
639 | } | ||
640 | |||
641 | public override void VehicleFlags(int param, bool remove) | ||
642 | { | ||
643 | } | ||
644 | |||
645 | public override void SetVolumeDetect(int param) | ||
646 | { | ||
647 | } | ||
648 | |||
649 | public override Vector3 CenterOfMass | ||
650 | { | ||
651 | get { return Vector3.Zero; } | ||
652 | } | ||
653 | |||
654 | public override Vector3 GeometricCenter | ||
655 | { | ||
656 | get { return Vector3.Zero; } | ||
657 | } | ||
658 | |||
659 | public override PrimitiveBaseShape Shape | ||
660 | { | ||
661 | set { return; } | ||
662 | } | ||
663 | |||
664 | public override Vector3 TargetVelocity | ||
665 | { | ||
666 | get | ||
667 | { | ||
668 | return m_taintTargetVelocity; | ||
669 | } | ||
670 | |||
671 | set | ||
672 | { | ||
673 | Velocity = value; | ||
674 | } | ||
675 | } | ||
676 | |||
677 | |||
678 | public override Vector3 Velocity | ||
679 | { | ||
680 | get | ||
681 | { | ||
682 | // There's a problem with Vector3.Zero! Don't Use it Here! | ||
683 | if (_zeroFlag) | ||
684 | return Vector3.Zero; | ||
685 | m_lastUpdateSent = false; | ||
686 | return _velocity; | ||
687 | } | ||
688 | |||
689 | set | ||
690 | { | ||
691 | if (value.IsFinite()) | ||
692 | { | ||
693 | m_pidControllerActive = true; | ||
694 | m_taintTargetVelocity = value; | ||
695 | _parent_scene.AddPhysicsActorTaint(this); | ||
696 | } | ||
697 | else | ||
698 | { | ||
699 | m_log.WarnFormat("[ODE CHARACTER]: Got a NaN velocity from Scene for {0}", Name); | ||
700 | } | ||
701 | |||
702 | // m_log.DebugFormat("[PHYSICS]: Set target velocity of {0}", m_taintTargetVelocity); | ||
703 | } | ||
704 | } | ||
705 | |||
706 | public override Vector3 Torque | ||
707 | { | ||
708 | get { return Vector3.Zero; } | ||
709 | set { return; } | ||
710 | } | ||
711 | |||
712 | public override float CollisionScore | ||
713 | { | ||
714 | get { return 0f; } | ||
715 | set { } | ||
716 | } | ||
717 | |||
718 | public override bool Kinematic | ||
719 | { | ||
720 | get { return false; } | ||
721 | set { } | ||
722 | } | ||
723 | |||
724 | public override Quaternion Orientation | ||
725 | { | ||
726 | get { return Quaternion.Identity; } | ||
727 | set { | ||
728 | //Matrix3 or = Orientation.ToRotationMatrix(); | ||
729 | //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22); | ||
730 | //d.BodySetRotation(Body, ref ord); | ||
731 | } | ||
732 | } | ||
733 | |||
734 | public override Vector3 Acceleration | ||
735 | { | ||
736 | get { return _acceleration; } | ||
737 | set { _acceleration = value; } | ||
738 | } | ||
739 | |||
740 | /// <summary> | ||
741 | /// Adds the force supplied to the Target Velocity | ||
742 | /// The PID controller takes this target velocity and tries to make it a reality | ||
743 | /// </summary> | ||
744 | /// <param name="force"></param> | ||
745 | public override void AddForce(Vector3 force, bool pushforce) | ||
746 | { | ||
747 | if (force.IsFinite()) | ||
748 | { | ||
749 | if (pushforce) | ||
750 | { | ||
751 | m_pidControllerActive = false; | ||
752 | force *= 100f; | ||
753 | m_taintForce += force; | ||
754 | _parent_scene.AddPhysicsActorTaint(this); | ||
755 | |||
756 | // If uncommented, things get pushed off world | ||
757 | // | ||
758 | // m_log.Debug("Push!"); | ||
759 | // m_taintTargetVelocity.X += force.X; | ||
760 | // m_taintTargetVelocity.Y += force.Y; | ||
761 | // m_taintTargetVelocity.Z += force.Z; | ||
762 | } | ||
763 | else | ||
764 | { | ||
765 | m_pidControllerActive = true; | ||
766 | m_taintTargetVelocity += force; | ||
767 | } | ||
768 | } | ||
769 | else | ||
770 | { | ||
771 | m_log.WarnFormat("[ODE CHARACTER]: Got a NaN force applied to {0}", Name); | ||
772 | } | ||
773 | //m_lastUpdateSent = false; | ||
774 | } | ||
775 | |||
776 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
777 | { | ||
778 | } | ||
779 | |||
780 | public override void SetMomentum(Vector3 momentum) | ||
781 | { | ||
782 | } | ||
783 | |||
784 | /// <summary> | ||
785 | /// Called from Simulate | ||
786 | /// This is the avatar's movement control + PID Controller | ||
787 | /// </summary> | ||
788 | /// <param name="defects">The character will be added to this list if there is something wrong (non-finite | ||
789 | /// position or velocity). | ||
790 | /// </param> | ||
791 | internal void Move(List<OdeCharacter> defects) | ||
792 | { | ||
793 | // no lock; for now it's only called from within Simulate() | ||
794 | |||
795 | // If the PID Controller isn't active then we set our force | ||
796 | // calculating base velocity to the current position | ||
797 | |||
798 | if (Body == IntPtr.Zero) | ||
799 | return; | ||
800 | |||
801 | if (m_pidControllerActive == false) | ||
802 | { | ||
803 | _zeroPosition = d.BodyGetPosition(Body); | ||
804 | } | ||
805 | //PidStatus = true; | ||
806 | |||
807 | d.Vector3 localpos = d.BodyGetPosition(Body); | ||
808 | Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z); | ||
809 | |||
810 | if (!localPos.IsFinite()) | ||
811 | { | ||
812 | m_log.WarnFormat( | ||
813 | "[ODE CHARACTER]: Avatar position of {0} for {1} is non-finite! Removing from physics scene.", | ||
814 | localPos, Name); | ||
815 | |||
816 | defects.Add(this); | ||
817 | |||
818 | return; | ||
819 | } | ||
820 | |||
821 | Vector3 vec = Vector3.Zero; | ||
822 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
823 | |||
824 | // m_log.DebugFormat( | ||
825 | // "[ODE CHARACTER]: Current velocity in Move() is <{0},{1},{2}>, target {3} for {4}", | ||
826 | // vel.X, vel.Y, vel.Z, _target_velocity, Name); | ||
827 | |||
828 | float movementdivisor = 1f; | ||
829 | |||
830 | if (!m_alwaysRun) | ||
831 | { | ||
832 | movementdivisor = walkDivisor; | ||
833 | } | ||
834 | else | ||
835 | { | ||
836 | movementdivisor = runDivisor; | ||
837 | } | ||
838 | |||
839 | // if velocity is zero, use position control; otherwise, velocity control | ||
840 | if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding) | ||
841 | { | ||
842 | // keep track of where we stopped. No more slippin' & slidin' | ||
843 | if (!_zeroFlag) | ||
844 | { | ||
845 | _zeroFlag = true; | ||
846 | _zeroPosition = d.BodyGetPosition(Body); | ||
847 | } | ||
848 | |||
849 | if (m_pidControllerActive) | ||
850 | { | ||
851 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
852 | // react to the physics scene by moving it's position. | ||
853 | // Avatar to Avatar collisions | ||
854 | // Prim to avatar collisions | ||
855 | |||
856 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
857 | vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); | ||
858 | vec.Y = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2); | ||
859 | if (flying) | ||
860 | { | ||
861 | vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; | ||
862 | } | ||
863 | } | ||
864 | //PidStatus = true; | ||
865 | } | ||
866 | else | ||
867 | { | ||
868 | m_pidControllerActive = true; | ||
869 | _zeroFlag = false; | ||
870 | if (m_iscolliding && !flying) | ||
871 | { | ||
872 | // We're standing on something | ||
873 | vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D); | ||
874 | vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); | ||
875 | } | ||
876 | else if (m_iscolliding && flying) | ||
877 | { | ||
878 | // We're flying and colliding with something | ||
879 | vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16); | ||
880 | vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16); | ||
881 | } | ||
882 | else if (!m_iscolliding && flying) | ||
883 | { | ||
884 | // we're in mid air suspended | ||
885 | vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6); | ||
886 | vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6); | ||
887 | |||
888 | // m_log.DebugFormat( | ||
889 | // "[ODE CHARACTER]: !m_iscolliding && flying, vec {0}, _target_velocity {1}, movementdivisor {2}, vel {3}", | ||
890 | // vec, _target_velocity, movementdivisor, vel); | ||
891 | } | ||
892 | |||
893 | if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) | ||
894 | { | ||
895 | // We're colliding with something and we're not flying but we're moving | ||
896 | // This means we're walking or running. | ||
897 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
898 | vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P; | ||
899 | if (_target_velocity.X > 0) | ||
900 | { | ||
901 | vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; | ||
902 | } | ||
903 | if (_target_velocity.Y > 0) | ||
904 | { | ||
905 | vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; | ||
906 | } | ||
907 | } | ||
908 | else if (!m_iscolliding && !flying) | ||
909 | { | ||
910 | // we're not colliding and we're not flying so that means we're falling! | ||
911 | // m_iscolliding includes collisions with the ground. | ||
912 | |||
913 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
914 | if (_target_velocity.X > 0) | ||
915 | { | ||
916 | vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; | ||
917 | } | ||
918 | if (_target_velocity.Y > 0) | ||
919 | { | ||
920 | vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; | ||
921 | } | ||
922 | } | ||
923 | |||
924 | if (flying) | ||
925 | { | ||
926 | // This also acts as anti-gravity so that we hover when flying rather than fall. | ||
927 | vec.Z = (_target_velocity.Z - vel.Z) * (PID_D); | ||
928 | } | ||
929 | } | ||
930 | |||
931 | if (flying) | ||
932 | { | ||
933 | // Anti-gravity so that we hover when flying rather than fall. | ||
934 | vec.Z += ((-1 * _parent_scene.gravityz) * m_mass); | ||
935 | |||
936 | //Added for auto fly height. Kitto Flora | ||
937 | //d.Vector3 pos = d.BodyGetPosition(Body); | ||
938 | float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset; | ||
939 | |||
940 | if (_position.Z < target_altitude) | ||
941 | { | ||
942 | vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f; | ||
943 | } | ||
944 | // end add Kitto Flora | ||
945 | } | ||
946 | |||
947 | if (vec.IsFinite()) | ||
948 | { | ||
949 | // Apply the total force acting on this avatar | ||
950 | d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); | ||
951 | |||
952 | if (!_zeroFlag) | ||
953 | AlignAvatarTiltWithCurrentDirectionOfMovement(vec); | ||
954 | } | ||
955 | else | ||
956 | { | ||
957 | m_log.WarnFormat( | ||
958 | "[ODE CHARACTER]: Got a NaN force vector {0} in Move() for {1}. Removing character from physics scene.", | ||
959 | vec, Name); | ||
960 | |||
961 | defects.Add(this); | ||
962 | |||
963 | return; | ||
964 | } | ||
965 | |||
966 | d.Vector3 newVel = d.BodyGetLinearVel(Body); | ||
967 | if (newVel.X >= 256 || newVel.X <= 256 || newVel.Y >= 256 || newVel.Y <= 256 || newVel.Z >= 256 || newVel.Z <= 256) | ||
968 | { | ||
969 | // m_log.DebugFormat( | ||
970 | // "[ODE CHARACTER]: Limiting falling velocity from {0} to {1} for {2}", newVel.Z, -9.8, Name); | ||
971 | |||
972 | newVel.X = Util.Clamp<float>(newVel.X, -255f, 255f); | ||
973 | newVel.Y = Util.Clamp<float>(newVel.Y, -255f, 255f); | ||
974 | |||
975 | if (!flying) | ||
976 | newVel.Z | ||
977 | = Util.Clamp<float>( | ||
978 | newVel.Z, -_parent_scene.AvatarTerminalVelocity, _parent_scene.AvatarTerminalVelocity); | ||
979 | else | ||
980 | newVel.Z = Util.Clamp<float>(newVel.Z, -255f, 255f); | ||
981 | |||
982 | d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); | ||
983 | } | ||
984 | } | ||
985 | |||
986 | /// <summary> | ||
987 | /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. | ||
988 | /// </summary> | ||
989 | /// <param name="defects">The character will be added to this list if there is something wrong (non-finite | ||
990 | /// position or velocity). | ||
991 | /// </param> | ||
992 | internal void UpdatePositionAndVelocity(List<OdeCharacter> defects) | ||
993 | { | ||
994 | // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! | ||
995 | d.Vector3 newPos; | ||
996 | try | ||
997 | { | ||
998 | newPos = d.BodyGetPosition(Body); | ||
999 | } | ||
1000 | catch (NullReferenceException) | ||
1001 | { | ||
1002 | bad = true; | ||
1003 | defects.Add(this); | ||
1004 | newPos = new d.Vector3(_position.X, _position.Y, _position.Z); | ||
1005 | base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! | ||
1006 | m_log.WarnFormat("[ODE CHARACTER]: Avatar Null reference for Avatar {0}, physical actor {1}", Name, m_uuid); | ||
1007 | |||
1008 | return; | ||
1009 | } | ||
1010 | |||
1011 | // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) | ||
1012 | if (newPos.X < 0.0f) newPos.X = 0.0f; | ||
1013 | if (newPos.Y < 0.0f) newPos.Y = 0.0f; | ||
1014 | if (newPos.X > (int)_parent_scene.WorldExtents.X - 0.05f) newPos.X = (int)_parent_scene.WorldExtents.X - 0.05f; | ||
1015 | if (newPos.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) newPos.Y = (int)_parent_scene.WorldExtents.Y - 0.05f; | ||
1016 | |||
1017 | _position.X = newPos.X; | ||
1018 | _position.Y = newPos.Y; | ||
1019 | _position.Z = newPos.Z; | ||
1020 | |||
1021 | // I think we need to update the taintPosition too -- Diva 12/24/10 | ||
1022 | m_taintPosition = _position; | ||
1023 | |||
1024 | // Did we move last? = zeroflag | ||
1025 | // This helps keep us from sliding all over | ||
1026 | |||
1027 | if (_zeroFlag) | ||
1028 | { | ||
1029 | _velocity = Vector3.Zero; | ||
1030 | |||
1031 | // Did we send out the 'stopped' message? | ||
1032 | if (!m_lastUpdateSent) | ||
1033 | { | ||
1034 | m_lastUpdateSent = true; | ||
1035 | //base.RequestPhysicsterseUpdate(); | ||
1036 | } | ||
1037 | } | ||
1038 | else | ||
1039 | { | ||
1040 | m_lastUpdateSent = false; | ||
1041 | d.Vector3 newVelocity; | ||
1042 | |||
1043 | try | ||
1044 | { | ||
1045 | newVelocity = d.BodyGetLinearVel(Body); | ||
1046 | } | ||
1047 | catch (NullReferenceException) | ||
1048 | { | ||
1049 | newVelocity.X = _velocity.X; | ||
1050 | newVelocity.Y = _velocity.Y; | ||
1051 | newVelocity.Z = _velocity.Z; | ||
1052 | } | ||
1053 | |||
1054 | _velocity.X = newVelocity.X; | ||
1055 | _velocity.Y = newVelocity.Y; | ||
1056 | _velocity.Z = newVelocity.Z; | ||
1057 | |||
1058 | if (_velocity.Z < -6 && !m_hackSentFall) | ||
1059 | { | ||
1060 | m_hackSentFall = true; | ||
1061 | m_pidControllerActive = false; | ||
1062 | } | ||
1063 | else if (flying && !m_hackSentFly) | ||
1064 | { | ||
1065 | //m_hackSentFly = true; | ||
1066 | //base.SendCollisionUpdate(new CollisionEventUpdate()); | ||
1067 | } | ||
1068 | else | ||
1069 | { | ||
1070 | m_hackSentFly = false; | ||
1071 | m_hackSentFall = false; | ||
1072 | } | ||
1073 | } | ||
1074 | } | ||
1075 | |||
1076 | /// <summary> | ||
1077 | /// This creates the Avatar's physical Surrogate in ODE at the position supplied | ||
1078 | /// </summary> | ||
1079 | /// <remarks> | ||
1080 | /// WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access | ||
1081 | /// to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only | ||
1082 | /// place that is safe to call this routine AvatarGeomAndBodyCreation. | ||
1083 | /// </remarks> | ||
1084 | /// <param name="npositionX"></param> | ||
1085 | /// <param name="npositionY"></param> | ||
1086 | /// <param name="npositionZ"></param> | ||
1087 | /// <param name="tensor"></param> | ||
1088 | private void CreateOdeStructures(float npositionX, float npositionY, float npositionZ, float tensor) | ||
1089 | { | ||
1090 | if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero)) | ||
1091 | { | ||
1092 | m_log.ErrorFormat( | ||
1093 | "[ODE CHARACTER]: Creating ODE structures for {0} even though some already exist. Shell = {1}, Body = {2}, Amotor = {3}", | ||
1094 | Name, Shell, Body, Amotor); | ||
1095 | } | ||
1096 | |||
1097 | int dAMotorEuler = 1; | ||
1098 | // _parent_scene.waitForSpaceUnlock(_parent_scene.space); | ||
1099 | if (CAPSULE_LENGTH <= 0) | ||
1100 | { | ||
1101 | m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); | ||
1102 | CAPSULE_LENGTH = 0.01f; | ||
1103 | } | ||
1104 | |||
1105 | if (CAPSULE_RADIUS <= 0) | ||
1106 | { | ||
1107 | m_log.Warn("[ODE CHARACTER]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); | ||
1108 | CAPSULE_RADIUS = 0.01f; | ||
1109 | } | ||
1110 | |||
1111 | // lock (OdeScene.UniversalColliderSyncObject) | ||
1112 | Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); | ||
1113 | |||
1114 | d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); | ||
1115 | d.GeomSetCollideBits(Shell, (int)m_collisionFlags); | ||
1116 | |||
1117 | d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH); | ||
1118 | Body = d.BodyCreate(_parent_scene.world); | ||
1119 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); | ||
1120 | |||
1121 | _position.X = npositionX; | ||
1122 | _position.Y = npositionY; | ||
1123 | _position.Z = npositionZ; | ||
1124 | |||
1125 | m_taintPosition = _position; | ||
1126 | |||
1127 | d.BodySetMass(Body, ref ShellMass); | ||
1128 | d.Matrix3 m_caprot; | ||
1129 | // 90 Stand up on the cap of the capped cyllinder | ||
1130 | if (_parent_scene.IsAvCapsuleTilted) | ||
1131 | { | ||
1132 | d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2)); | ||
1133 | } | ||
1134 | else | ||
1135 | { | ||
1136 | d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2)); | ||
1137 | } | ||
1138 | |||
1139 | d.GeomSetRotation(Shell, ref m_caprot); | ||
1140 | d.BodySetRotation(Body, ref m_caprot); | ||
1141 | |||
1142 | d.GeomSetBody(Shell, Body); | ||
1143 | |||
1144 | // The purpose of the AMotor here is to keep the avatar's physical | ||
1145 | // surrogate from rotating while moving | ||
1146 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
1147 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
1148 | d.JointSetAMotorMode(Amotor, dAMotorEuler); | ||
1149 | d.JointSetAMotorNumAxes(Amotor, 3); | ||
1150 | d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); | ||
1151 | d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); | ||
1152 | d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); | ||
1153 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
1154 | d.JointSetAMotorAngle(Amotor, 1, 0); | ||
1155 | d.JointSetAMotorAngle(Amotor, 2, 0); | ||
1156 | |||
1157 | // These lowstops and high stops are effectively (no wiggle room) | ||
1158 | if (_parent_scene.IsAvCapsuleTilted) | ||
1159 | { | ||
1160 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f); | ||
1161 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f); | ||
1162 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f); | ||
1163 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f); | ||
1164 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f); | ||
1165 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f); | ||
1166 | } | ||
1167 | else | ||
1168 | { | ||
1169 | #region Documentation of capsule motor LowStop and HighStop parameters | ||
1170 | // Intentionally introduce some tilt into the capsule by setting | ||
1171 | // the motor stops to small epsilon values. This small tilt prevents | ||
1172 | // the capsule from falling into the terrain; a straight-up capsule | ||
1173 | // (with -0..0 motor stops) falls into the terrain for reasons yet | ||
1174 | // to be comprehended in their entirety. | ||
1175 | #endregion | ||
1176 | AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero); | ||
1177 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f); | ||
1178 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); | ||
1179 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f); | ||
1180 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced | ||
1181 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop | ||
1182 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop | ||
1183 | } | ||
1184 | |||
1185 | // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the | ||
1186 | // capped cyllinder will fall over | ||
1187 | d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); | ||
1188 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor); | ||
1189 | |||
1190 | //d.Matrix3 bodyrotation = d.BodyGetRotation(Body); | ||
1191 | //d.QfromR( | ||
1192 | //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068, | ||
1193 | // | ||
1194 | //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22); | ||
1195 | //standupStraight(); | ||
1196 | |||
1197 | _parent_scene.geom_name_map[Shell] = Name; | ||
1198 | _parent_scene.actor_name_map[Shell] = this; | ||
1199 | } | ||
1200 | |||
1201 | /// <summary> | ||
1202 | /// Cleanup the things we use in the scene. | ||
1203 | /// </summary> | ||
1204 | internal void Destroy() | ||
1205 | { | ||
1206 | m_tainted_isPhysical = false; | ||
1207 | _parent_scene.AddPhysicsActorTaint(this); | ||
1208 | } | ||
1209 | |||
1210 | /// <summary> | ||
1211 | /// Used internally to destroy the ODE structures associated with this character. | ||
1212 | /// </summary> | ||
1213 | internal void DestroyOdeStructures() | ||
1214 | { | ||
1215 | // Create avatar capsule and related ODE data | ||
1216 | if (Shell == IntPtr.Zero || Body == IntPtr.Zero || Amotor == IntPtr.Zero) | ||
1217 | { | ||
1218 | m_log.ErrorFormat( | ||
1219 | "[ODE CHARACTER]: Destroying ODE structures for {0} even though some are already null. Shell = {1}, Body = {2}, Amotor = {3}", | ||
1220 | Name, Shell, Body, Amotor); | ||
1221 | } | ||
1222 | |||
1223 | // destroy avatar capsule and related ODE data | ||
1224 | if (Amotor != IntPtr.Zero) | ||
1225 | { | ||
1226 | // Kill the Amotor | ||
1227 | d.JointDestroy(Amotor); | ||
1228 | Amotor = IntPtr.Zero; | ||
1229 | } | ||
1230 | |||
1231 | //kill the Geometry | ||
1232 | // _parent_scene.waitForSpaceUnlock(_parent_scene.space); | ||
1233 | |||
1234 | if (Body != IntPtr.Zero) | ||
1235 | { | ||
1236 | //kill the body | ||
1237 | d.BodyDestroy(Body); | ||
1238 | Body = IntPtr.Zero; | ||
1239 | } | ||
1240 | |||
1241 | if (Shell != IntPtr.Zero) | ||
1242 | { | ||
1243 | // lock (OdeScene.UniversalColliderSyncObject) | ||
1244 | d.GeomDestroy(Shell); | ||
1245 | |||
1246 | _parent_scene.geom_name_map.Remove(Shell); | ||
1247 | _parent_scene.actor_name_map.Remove(Shell); | ||
1248 | |||
1249 | Shell = IntPtr.Zero; | ||
1250 | } | ||
1251 | } | ||
1252 | |||
1253 | public override void CrossingFailure() | ||
1254 | { | ||
1255 | } | ||
1256 | |||
1257 | public override Vector3 PIDTarget { set { return; } } | ||
1258 | public override bool PIDActive { set { return; } } | ||
1259 | public override float PIDTau { set { return; } } | ||
1260 | |||
1261 | public override float PIDHoverHeight { set { return; } } | ||
1262 | public override bool PIDHoverActive { set { return; } } | ||
1263 | public override PIDHoverType PIDHoverType { set { return; } } | ||
1264 | public override float PIDHoverTau { set { return; } } | ||
1265 | |||
1266 | public override Quaternion APIDTarget{ set { return; } } | ||
1267 | |||
1268 | public override bool APIDActive{ set { return; } } | ||
1269 | |||
1270 | public override float APIDStrength{ set { return; } } | ||
1271 | |||
1272 | public override float APIDDamping{ set { return; } } | ||
1273 | |||
1274 | public override void SubscribeEvents(int ms) | ||
1275 | { | ||
1276 | m_requestedUpdateFrequency = ms; | ||
1277 | m_eventsubscription = ms; | ||
1278 | |||
1279 | // Don't clear collision event reporting here. This is called directly from scene code and so can lead | ||
1280 | // to a race condition with the simulate loop | ||
1281 | |||
1282 | _parent_scene.AddCollisionEventReporting(this); | ||
1283 | } | ||
1284 | |||
1285 | public override void UnSubscribeEvents() | ||
1286 | { | ||
1287 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1288 | |||
1289 | // Don't clear collision event reporting here. This is called directly from scene code and so can lead | ||
1290 | // to a race condition with the simulate loop | ||
1291 | |||
1292 | m_requestedUpdateFrequency = 0; | ||
1293 | m_eventsubscription = 0; | ||
1294 | } | ||
1295 | |||
1296 | internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1297 | { | ||
1298 | if (m_eventsubscription > 0) | ||
1299 | { | ||
1300 | // m_log.DebugFormat( | ||
1301 | // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); | ||
1302 | |||
1303 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1304 | } | ||
1305 | } | ||
1306 | |||
1307 | internal void SendCollisions() | ||
1308 | { | ||
1309 | if (m_eventsubscription > m_requestedUpdateFrequency) | ||
1310 | { | ||
1311 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1312 | |||
1313 | CollisionEventsThisFrame.Clear(); | ||
1314 | m_eventsubscription = 0; | ||
1315 | } | ||
1316 | } | ||
1317 | |||
1318 | public override bool SubscribedEvents() | ||
1319 | { | ||
1320 | if (m_eventsubscription > 0) | ||
1321 | return true; | ||
1322 | return false; | ||
1323 | } | ||
1324 | |||
1325 | internal void ProcessTaints() | ||
1326 | { | ||
1327 | if (m_taintPosition != _position) | ||
1328 | { | ||
1329 | if (Body != IntPtr.Zero) | ||
1330 | { | ||
1331 | d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); | ||
1332 | _position = m_taintPosition; | ||
1333 | } | ||
1334 | } | ||
1335 | |||
1336 | if (m_taintForce != Vector3.Zero) | ||
1337 | { | ||
1338 | if (Body != IntPtr.Zero) | ||
1339 | { | ||
1340 | // FIXME: This is not a good solution since it's subject to a race condition if a force is another | ||
1341 | // thread sets a new force while we're in this loop (since it could be obliterated by | ||
1342 | // m_taintForce = Vector3.Zero. Need to lock ProcessTaints() when we set a new tainted force. | ||
1343 | d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z); | ||
1344 | } | ||
1345 | |||
1346 | m_taintForce = Vector3.Zero; | ||
1347 | } | ||
1348 | |||
1349 | if (m_taintTargetVelocity != _target_velocity) | ||
1350 | _target_velocity = m_taintTargetVelocity; | ||
1351 | |||
1352 | if (m_tainted_isPhysical != m_isPhysical) | ||
1353 | { | ||
1354 | if (m_tainted_isPhysical) | ||
1355 | { | ||
1356 | CreateOdeStructures(_position.X, _position.Y, _position.Z, m_tensor); | ||
1357 | _parent_scene.AddCharacter(this); | ||
1358 | } | ||
1359 | else | ||
1360 | { | ||
1361 | _parent_scene.RemoveCharacter(this); | ||
1362 | DestroyOdeStructures(); | ||
1363 | } | ||
1364 | |||
1365 | m_isPhysical = m_tainted_isPhysical; | ||
1366 | } | ||
1367 | |||
1368 | if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) | ||
1369 | { | ||
1370 | if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) | ||
1371 | { | ||
1372 | // m_log.DebugFormat( | ||
1373 | // "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}", | ||
1374 | // CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name); | ||
1375 | |||
1376 | m_pidControllerActive = true; | ||
1377 | |||
1378 | // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() | ||
1379 | DestroyOdeStructures(); | ||
1380 | |||
1381 | float prevCapsule = CAPSULE_LENGTH; | ||
1382 | CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; | ||
1383 | |||
1384 | CreateOdeStructures( | ||
1385 | _position.X, | ||
1386 | _position.Y, | ||
1387 | _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor); | ||
1388 | |||
1389 | // As with Size, we reset velocity. However, this isn't strictly necessary since it doesn't | ||
1390 | // appear to stall initial region crossings when done here. Being done for consistency. | ||
1391 | // Velocity = Vector3.Zero; | ||
1392 | } | ||
1393 | else | ||
1394 | { | ||
1395 | m_log.Warn("[ODE CHARACTER]: trying to change capsule size for " + Name + ", but the following ODE data is missing - " | ||
1396 | + (Shell==IntPtr.Zero ? "Shell ":"") | ||
1397 | + (Body==IntPtr.Zero ? "Body ":"") | ||
1398 | + (Amotor==IntPtr.Zero ? "Amotor ":"")); | ||
1399 | } | ||
1400 | } | ||
1401 | } | ||
1402 | |||
1403 | internal void AddCollisionFrameTime(int p) | ||
1404 | { | ||
1405 | // protect it from overflow crashing | ||
1406 | if (m_eventsubscription + p >= int.MaxValue) | ||
1407 | m_eventsubscription = 0; | ||
1408 | m_eventsubscription += p; | ||
1409 | } | ||
1410 | } | ||
1411 | } | ||