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