diff options
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 1470 |
1 files changed, 1470 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs new file mode 100644 index 0000000..326fe97 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | |||
@@ -0,0 +1,1470 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | |||
28 | |||
29 | // Revision by Ubit 2011/12 | ||
30 | |||
31 | using System; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using OpenMetaverse; | ||
35 | using OdeAPI; | ||
36 | using OpenSim.Framework; | ||
37 | using OpenSim.Region.Physics.Manager; | ||
38 | using log4net; | ||
39 | |||
40 | namespace OpenSim.Region.Physics.OdePlugin | ||
41 | { | ||
42 | /// <summary> | ||
43 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
44 | /// </summary> | ||
45 | |||
46 | public enum dParam : int | ||
47 | { | ||
48 | LowStop = 0, | ||
49 | HiStop = 1, | ||
50 | Vel = 2, | ||
51 | FMax = 3, | ||
52 | FudgeFactor = 4, | ||
53 | Bounce = 5, | ||
54 | CFM = 6, | ||
55 | StopERP = 7, | ||
56 | StopCFM = 8, | ||
57 | LoStop2 = 256, | ||
58 | HiStop2 = 257, | ||
59 | Vel2 = 258, | ||
60 | FMax2 = 259, | ||
61 | StopERP2 = 7 + 256, | ||
62 | StopCFM2 = 8 + 256, | ||
63 | LoStop3 = 512, | ||
64 | HiStop3 = 513, | ||
65 | Vel3 = 514, | ||
66 | FMax3 = 515, | ||
67 | StopERP3 = 7 + 512, | ||
68 | StopCFM3 = 8 + 512 | ||
69 | } | ||
70 | |||
71 | public class OdeCharacter : PhysicsActor | ||
72 | { | ||
73 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
74 | |||
75 | private Vector3 _position; | ||
76 | private Vector3 _zeroPosition; | ||
77 | private bool _zeroFlag = false; | ||
78 | private Vector3 _velocity; | ||
79 | private Vector3 _target_velocity; | ||
80 | private Vector3 _acceleration; | ||
81 | private Vector3 m_rotationalVelocity; | ||
82 | private float m_mass = 80f; | ||
83 | public float m_density = 60f; | ||
84 | private bool m_pidControllerActive = true; | ||
85 | public float PID_D = 800.0f; | ||
86 | public float PID_P = 900.0f; | ||
87 | //private static float POSTURE_SERVO = 10000.0f; | ||
88 | public float CAPSULE_RADIUS = 0.37f; | ||
89 | public float CAPSULE_LENGTH = 2.140599f; | ||
90 | public float walkDivisor = 1.3f; | ||
91 | public float runDivisor = 0.8f; | ||
92 | private bool flying = false; | ||
93 | private bool m_iscolliding = false; | ||
94 | private bool m_iscollidingGround = false; | ||
95 | private bool m_iscollidingObj = false; | ||
96 | private bool m_alwaysRun = false; | ||
97 | private int m_requestedUpdateFrequency = 0; | ||
98 | public uint m_localID = 0; | ||
99 | public bool m_returnCollisions = false; | ||
100 | // taints and their non-tainted counterparts | ||
101 | public bool m_isPhysical = false; // the current physical status | ||
102 | public float MinimumGroundFlightOffset = 3f; | ||
103 | |||
104 | private float m_buoyancy = 0f; | ||
105 | |||
106 | private bool m_freemove = false; | ||
107 | // private CollisionLocker ode; | ||
108 | |||
109 | private string m_name = String.Empty; | ||
110 | // other filter control | ||
111 | int m_colliderfilter = 0; | ||
112 | int m_colliderGroundfilter = 0; | ||
113 | int m_colliderObjectfilter = 0; | ||
114 | |||
115 | // Default we're a Character | ||
116 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); | ||
117 | |||
118 | // Default, Collide with Other Geometries, spaces, bodies and characters. | ||
119 | private CollisionCategories m_collisionFlags = (CollisionCategories.Character | ||
120 | | CollisionCategories.Geom | ||
121 | | CollisionCategories.VolumeDtc | ||
122 | ); | ||
123 | // we do land collisions not ode | CollisionCategories.Land); | ||
124 | public IntPtr Body = IntPtr.Zero; | ||
125 | private OdeScene _parent_scene; | ||
126 | public IntPtr Shell = IntPtr.Zero; | ||
127 | public IntPtr Amotor = IntPtr.Zero; | ||
128 | public d.Mass ShellMass; | ||
129 | // public bool collidelock = false; | ||
130 | |||
131 | private bool m_haseventsubscription = false; | ||
132 | public int m_eventsubscription = 0; | ||
133 | private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
134 | |||
135 | // unique UUID of this character object | ||
136 | public UUID m_uuid; | ||
137 | public bool bad = false; | ||
138 | |||
139 | float mu; | ||
140 | |||
141 | public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) | ||
142 | { | ||
143 | m_uuid = UUID.Random(); | ||
144 | |||
145 | if (pos.IsFinite()) | ||
146 | { | ||
147 | if (pos.Z > 99999f) | ||
148 | { | ||
149 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
150 | } | ||
151 | if (pos.Z < -100f) // shouldn't this be 0 ? | ||
152 | { | ||
153 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
154 | } | ||
155 | _position = pos; | ||
156 | } | ||
157 | else | ||
158 | { | ||
159 | _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); | ||
160 | m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); | ||
161 | } | ||
162 | |||
163 | _parent_scene = parent_scene; | ||
164 | |||
165 | PID_D = pid_d; | ||
166 | PID_P = pid_p; | ||
167 | CAPSULE_RADIUS = capsule_radius; | ||
168 | m_density = density; | ||
169 | m_mass = 80f; // sure we have a default | ||
170 | |||
171 | mu = parent_scene.AvatarFriction; | ||
172 | |||
173 | walkDivisor = walk_divisor; | ||
174 | runDivisor = rundivisor; | ||
175 | |||
176 | CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; | ||
177 | //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); | ||
178 | |||
179 | m_isPhysical = false; // current status: no ODE information exists | ||
180 | |||
181 | m_name = avName; | ||
182 | |||
183 | AddChange(changes.Add, null); | ||
184 | } | ||
185 | |||
186 | public override int PhysicsActorType | ||
187 | { | ||
188 | get { return (int)ActorTypes.Agent; } | ||
189 | set { return; } | ||
190 | } | ||
191 | |||
192 | public override void getContactData(ref ContactData cdata) | ||
193 | { | ||
194 | cdata.mu = mu; | ||
195 | cdata.bounce = 0; | ||
196 | cdata.softcolide = false; | ||
197 | } | ||
198 | |||
199 | public override bool Building { get; set; } | ||
200 | |||
201 | /// <summary> | ||
202 | /// If this is set, the avatar will move faster | ||
203 | /// </summary> | ||
204 | public override bool SetAlwaysRun | ||
205 | { | ||
206 | get { return m_alwaysRun; } | ||
207 | set { m_alwaysRun = value; } | ||
208 | } | ||
209 | |||
210 | public override uint LocalID | ||
211 | { | ||
212 | set { m_localID = value; } | ||
213 | } | ||
214 | |||
215 | public override bool Grabbed | ||
216 | { | ||
217 | set { return; } | ||
218 | } | ||
219 | |||
220 | public override bool Selected | ||
221 | { | ||
222 | set { return; } | ||
223 | } | ||
224 | |||
225 | public override float Buoyancy | ||
226 | { | ||
227 | get { return m_buoyancy; } | ||
228 | set { m_buoyancy = value; } | ||
229 | } | ||
230 | |||
231 | public override bool FloatOnWater | ||
232 | { | ||
233 | set { return; } | ||
234 | } | ||
235 | |||
236 | public override bool IsPhysical | ||
237 | { | ||
238 | get { return false; } | ||
239 | set { return; } | ||
240 | } | ||
241 | |||
242 | public override bool ThrottleUpdates | ||
243 | { | ||
244 | get { return false; } | ||
245 | set { return; } | ||
246 | } | ||
247 | |||
248 | public override bool Flying | ||
249 | { | ||
250 | get { return flying; } | ||
251 | set | ||
252 | { | ||
253 | flying = value; | ||
254 | // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); | ||
255 | } | ||
256 | } | ||
257 | |||
258 | /// <summary> | ||
259 | /// Returns if the avatar is colliding in general. | ||
260 | /// This includes the ground and objects and avatar. | ||
261 | /// </summary> | ||
262 | public override bool IsColliding | ||
263 | { | ||
264 | get { return (m_iscolliding || m_iscollidingGround); } | ||
265 | set | ||
266 | { | ||
267 | if (value) | ||
268 | { | ||
269 | m_colliderfilter += 2; | ||
270 | if (m_colliderfilter > 2) | ||
271 | m_colliderfilter = 2; | ||
272 | } | ||
273 | else | ||
274 | { | ||
275 | m_colliderfilter--; | ||
276 | if (m_colliderfilter < 0) | ||
277 | m_colliderfilter = 0; | ||
278 | } | ||
279 | |||
280 | if (m_colliderfilter == 0) | ||
281 | m_iscolliding = false; | ||
282 | else | ||
283 | { | ||
284 | m_pidControllerActive = true; | ||
285 | m_iscolliding = true; | ||
286 | } | ||
287 | } | ||
288 | } | ||
289 | |||
290 | /// <summary> | ||
291 | /// Returns if an avatar is colliding with the ground | ||
292 | /// </summary> | ||
293 | public override bool CollidingGround | ||
294 | { | ||
295 | get { return m_iscollidingGround; } | ||
296 | set | ||
297 | { | ||
298 | /* we now control this | ||
299 | if (value) | ||
300 | { | ||
301 | m_colliderGroundfilter += 2; | ||
302 | if (m_colliderGroundfilter > 2) | ||
303 | m_colliderGroundfilter = 2; | ||
304 | } | ||
305 | else | ||
306 | { | ||
307 | m_colliderGroundfilter--; | ||
308 | if (m_colliderGroundfilter < 0) | ||
309 | m_colliderGroundfilter = 0; | ||
310 | } | ||
311 | |||
312 | if (m_colliderGroundfilter == 0) | ||
313 | m_iscollidingGround = false; | ||
314 | else | ||
315 | m_iscollidingGround = true; | ||
316 | */ | ||
317 | } | ||
318 | |||
319 | } | ||
320 | |||
321 | /// <summary> | ||
322 | /// Returns if the avatar is colliding with an object | ||
323 | /// </summary> | ||
324 | public override bool CollidingObj | ||
325 | { | ||
326 | get { return m_iscollidingObj; } | ||
327 | set | ||
328 | { | ||
329 | // Ubit filter this also | ||
330 | if (value) | ||
331 | { | ||
332 | m_colliderObjectfilter += 2; | ||
333 | if (m_colliderObjectfilter > 2) | ||
334 | m_colliderObjectfilter = 2; | ||
335 | } | ||
336 | else | ||
337 | { | ||
338 | m_colliderObjectfilter--; | ||
339 | if (m_colliderObjectfilter < 0) | ||
340 | m_colliderObjectfilter = 0; | ||
341 | } | ||
342 | |||
343 | if (m_colliderObjectfilter == 0) | ||
344 | m_iscollidingObj = false; | ||
345 | else | ||
346 | m_iscollidingObj = true; | ||
347 | |||
348 | // m_iscollidingObj = value; | ||
349 | |||
350 | if (m_iscollidingObj) | ||
351 | m_pidControllerActive = false; | ||
352 | else | ||
353 | m_pidControllerActive = true; | ||
354 | } | ||
355 | } | ||
356 | |||
357 | /// <summary> | ||
358 | /// turn the PID controller on or off. | ||
359 | /// The PID Controller will turn on all by itself in many situations | ||
360 | /// </summary> | ||
361 | /// <param name="status"></param> | ||
362 | public void SetPidStatus(bool status) | ||
363 | { | ||
364 | m_pidControllerActive = status; | ||
365 | } | ||
366 | |||
367 | public override bool Stopped | ||
368 | { | ||
369 | get { return _zeroFlag; } | ||
370 | } | ||
371 | |||
372 | /// <summary> | ||
373 | /// This 'puts' an avatar somewhere in the physics space. | ||
374 | /// Not really a good choice unless you 'know' it's a good | ||
375 | /// spot otherwise you're likely to orbit the avatar. | ||
376 | /// </summary> | ||
377 | public override Vector3 Position | ||
378 | { | ||
379 | get { return _position; } | ||
380 | set | ||
381 | { | ||
382 | if (value.IsFinite()) | ||
383 | { | ||
384 | if (value.Z > 9999999f) | ||
385 | { | ||
386 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
387 | } | ||
388 | if (value.Z < -100f) | ||
389 | { | ||
390 | value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
391 | } | ||
392 | AddChange(changes.Position, value); | ||
393 | } | ||
394 | else | ||
395 | { | ||
396 | m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); | ||
397 | } | ||
398 | } | ||
399 | } | ||
400 | |||
401 | public override Vector3 RotationalVelocity | ||
402 | { | ||
403 | get { return m_rotationalVelocity; } | ||
404 | set { m_rotationalVelocity = value; } | ||
405 | } | ||
406 | |||
407 | /// <summary> | ||
408 | /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight | ||
409 | /// and use it to offset landings properly | ||
410 | /// </summary> | ||
411 | public override Vector3 Size | ||
412 | { | ||
413 | get { | ||
414 | float d = CAPSULE_RADIUS * 2; | ||
415 | return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } | ||
416 | set | ||
417 | { | ||
418 | if (value.IsFinite()) | ||
419 | { | ||
420 | AddChange(changes.Size, value); | ||
421 | } | ||
422 | else | ||
423 | { | ||
424 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
425 | } | ||
426 | } | ||
427 | } | ||
428 | |||
429 | /// <summary> | ||
430 | /// This creates the Avatar's physical Surrogate at the position supplied | ||
431 | /// </summary> | ||
432 | /// <param name="npositionX"></param> | ||
433 | /// <param name="npositionY"></param> | ||
434 | /// <param name="npositionZ"></param> | ||
435 | |||
436 | // | ||
437 | /// <summary> | ||
438 | /// Uses the capped cyllinder volume formula to calculate the avatar's mass. | ||
439 | /// This may be used in calculations in the scene/scenepresence | ||
440 | /// </summary> | ||
441 | public override float Mass | ||
442 | { | ||
443 | get | ||
444 | { | ||
445 | float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); | ||
446 | return m_density * AVvolume; | ||
447 | } | ||
448 | } | ||
449 | public override void link(PhysicsActor obj) | ||
450 | { | ||
451 | |||
452 | } | ||
453 | |||
454 | public override void delink() | ||
455 | { | ||
456 | |||
457 | } | ||
458 | |||
459 | public override void LockAngularMotion(Vector3 axis) | ||
460 | { | ||
461 | |||
462 | } | ||
463 | |||
464 | |||
465 | public override Vector3 Force | ||
466 | { | ||
467 | get { return _target_velocity; } | ||
468 | set { return; } | ||
469 | } | ||
470 | |||
471 | public override int VehicleType | ||
472 | { | ||
473 | get { return 0; } | ||
474 | set { return; } | ||
475 | } | ||
476 | |||
477 | public override void VehicleFloatParam(int param, float value) | ||
478 | { | ||
479 | |||
480 | } | ||
481 | |||
482 | public override void VehicleVectorParam(int param, Vector3 value) | ||
483 | { | ||
484 | |||
485 | } | ||
486 | |||
487 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
488 | { | ||
489 | |||
490 | } | ||
491 | |||
492 | public override void VehicleFlags(int param, bool remove) | ||
493 | { | ||
494 | |||
495 | } | ||
496 | |||
497 | public override void SetVolumeDetect(int param) | ||
498 | { | ||
499 | |||
500 | } | ||
501 | |||
502 | public override Vector3 CenterOfMass | ||
503 | { | ||
504 | get | ||
505 | { | ||
506 | Vector3 pos = _position; | ||
507 | return pos; | ||
508 | } | ||
509 | } | ||
510 | |||
511 | public override Vector3 GeometricCenter | ||
512 | { | ||
513 | get | ||
514 | { | ||
515 | Vector3 pos = _position; | ||
516 | return pos; | ||
517 | } | ||
518 | } | ||
519 | |||
520 | //UBit mess | ||
521 | /* for later use | ||
522 | public override Vector3 PrimOOBsize | ||
523 | { | ||
524 | get | ||
525 | { | ||
526 | Vector3 s=Size; | ||
527 | s.X *=0.5f; | ||
528 | s.Y *=0.5f; | ||
529 | s.Z *=0.5f; | ||
530 | return s; | ||
531 | } | ||
532 | } | ||
533 | |||
534 | public override Vector3 PrimOOBoffset | ||
535 | { | ||
536 | get | ||
537 | { | ||
538 | return Vector3.Zero; | ||
539 | } | ||
540 | } | ||
541 | */ | ||
542 | |||
543 | public override PrimitiveBaseShape Shape | ||
544 | { | ||
545 | set { return; } | ||
546 | } | ||
547 | |||
548 | public override Vector3 Velocity | ||
549 | { | ||
550 | get | ||
551 | { | ||
552 | return _velocity; | ||
553 | } | ||
554 | set | ||
555 | { | ||
556 | if (value.IsFinite()) | ||
557 | { | ||
558 | AddChange(changes.Velocity, value); | ||
559 | } | ||
560 | else | ||
561 | { | ||
562 | m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); | ||
563 | } | ||
564 | } | ||
565 | } | ||
566 | |||
567 | public override Vector3 Torque | ||
568 | { | ||
569 | get { return Vector3.Zero; } | ||
570 | set { return; } | ||
571 | } | ||
572 | |||
573 | public override float CollisionScore | ||
574 | { | ||
575 | get { return 0f; } | ||
576 | set { } | ||
577 | } | ||
578 | |||
579 | public override bool Kinematic | ||
580 | { | ||
581 | get { return false; } | ||
582 | set { } | ||
583 | } | ||
584 | |||
585 | public override Quaternion Orientation | ||
586 | { | ||
587 | get { return Quaternion.Identity; } | ||
588 | set | ||
589 | { | ||
590 | } | ||
591 | } | ||
592 | |||
593 | public override Vector3 Acceleration | ||
594 | { | ||
595 | get { return _acceleration; } | ||
596 | set { } | ||
597 | } | ||
598 | |||
599 | public void SetAcceleration(Vector3 accel) | ||
600 | { | ||
601 | m_pidControllerActive = true; | ||
602 | _acceleration = accel; | ||
603 | } | ||
604 | |||
605 | /// <summary> | ||
606 | /// Adds the force supplied to the Target Velocity | ||
607 | /// The PID controller takes this target velocity and tries to make it a reality | ||
608 | /// </summary> | ||
609 | /// <param name="force"></param> | ||
610 | public override void AddForce(Vector3 force, bool pushforce) | ||
611 | { | ||
612 | if (force.IsFinite()) | ||
613 | { | ||
614 | if (pushforce) | ||
615 | { | ||
616 | AddChange(changes.Force, force * m_density / (_parent_scene.ODE_STEPSIZE * 28f)); | ||
617 | } | ||
618 | else | ||
619 | { | ||
620 | AddChange(changes.Velocity, force); | ||
621 | } | ||
622 | } | ||
623 | else | ||
624 | { | ||
625 | m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); | ||
626 | } | ||
627 | //m_lastUpdateSent = false; | ||
628 | } | ||
629 | |||
630 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
631 | { | ||
632 | |||
633 | } | ||
634 | |||
635 | public override void SetMomentum(Vector3 momentum) | ||
636 | { | ||
637 | if (momentum.IsFinite()) | ||
638 | AddChange(changes.Momentum, momentum); | ||
639 | } | ||
640 | |||
641 | |||
642 | // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access | ||
643 | // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only | ||
644 | // place that is safe to call this routine AvatarGeomAndBodyCreation. | ||
645 | private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) | ||
646 | { | ||
647 | _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); | ||
648 | if (CAPSULE_LENGTH <= 0) | ||
649 | { | ||
650 | m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); | ||
651 | CAPSULE_LENGTH = 0.01f; | ||
652 | |||
653 | } | ||
654 | |||
655 | if (CAPSULE_RADIUS <= 0) | ||
656 | { | ||
657 | m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); | ||
658 | CAPSULE_RADIUS = 0.01f; | ||
659 | |||
660 | } | ||
661 | Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); | ||
662 | |||
663 | d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories); | ||
664 | d.GeomSetCollideBits(Shell, (uint)m_collisionFlags); | ||
665 | |||
666 | d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); | ||
667 | |||
668 | m_mass = ShellMass.mass; // update mass | ||
669 | |||
670 | // rescale PID parameters | ||
671 | PID_D = _parent_scene.avPIDD; | ||
672 | PID_P = _parent_scene.avPIDP; | ||
673 | |||
674 | // rescale PID parameters so that this aren't affected by mass | ||
675 | // and so don't get unstable for some masses | ||
676 | // also scale by ode time step so you don't need to refix them | ||
677 | |||
678 | PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps | ||
679 | PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; | ||
680 | PID_P /= 50 * 80; | ||
681 | PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; | ||
682 | |||
683 | Body = d.BodyCreate(_parent_scene.world); | ||
684 | |||
685 | _zeroFlag = false; | ||
686 | m_pidControllerActive = true; | ||
687 | m_freemove = false; | ||
688 | |||
689 | d.BodySetAutoDisableFlag(Body, false); | ||
690 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); | ||
691 | |||
692 | _position.X = npositionX; | ||
693 | _position.Y = npositionY; | ||
694 | _position.Z = npositionZ; | ||
695 | |||
696 | d.BodySetMass(Body, ref ShellMass); | ||
697 | d.GeomSetBody(Shell, Body); | ||
698 | |||
699 | // The purpose of the AMotor here is to keep the avatar's physical | ||
700 | // surrogate from rotating while moving | ||
701 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
702 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
703 | |||
704 | d.JointSetAMotorMode(Amotor, 0); | ||
705 | d.JointSetAMotorNumAxes(Amotor, 3); | ||
706 | d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); | ||
707 | d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); | ||
708 | d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); | ||
709 | |||
710 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
711 | d.JointSetAMotorAngle(Amotor, 1, 0); | ||
712 | d.JointSetAMotorAngle(Amotor, 2, 0); | ||
713 | |||
714 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD | ||
715 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); | ||
716 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); | ||
717 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); | ||
718 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); | ||
719 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); | ||
720 | |||
721 | // These lowstops and high stops are effectively (no wiggle room) | ||
722 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); | ||
723 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); | ||
724 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); | ||
725 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); | ||
726 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); | ||
727 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); | ||
728 | |||
729 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
730 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); | ||
731 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); | ||
732 | |||
733 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); | ||
734 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); | ||
735 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); | ||
736 | } | ||
737 | |||
738 | /// <summary> | ||
739 | /// Destroys the avatar body and geom | ||
740 | |||
741 | private void AvatarGeomAndBodyDestroy() | ||
742 | { | ||
743 | // Kill the Amotor | ||
744 | if (Amotor != IntPtr.Zero) | ||
745 | { | ||
746 | d.JointDestroy(Amotor); | ||
747 | Amotor = IntPtr.Zero; | ||
748 | } | ||
749 | |||
750 | if (Body != IntPtr.Zero) | ||
751 | { | ||
752 | //kill the body | ||
753 | d.BodyDestroy(Body); | ||
754 | Body = IntPtr.Zero; | ||
755 | } | ||
756 | |||
757 | //kill the Geometry | ||
758 | if (Shell != IntPtr.Zero) | ||
759 | { | ||
760 | _parent_scene.geom_name_map.Remove(Shell); | ||
761 | _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); | ||
762 | d.GeomDestroy(Shell); | ||
763 | Shell = IntPtr.Zero; | ||
764 | } | ||
765 | } | ||
766 | |||
767 | /// <summary> | ||
768 | /// Called from Simulate | ||
769 | /// This is the avatar's movement control + PID Controller | ||
770 | /// </summary> | ||
771 | /// <param name="timeStep"></param> | ||
772 | public void Move(float timeStep, List<OdeCharacter> defects) | ||
773 | { | ||
774 | if (Body == IntPtr.Zero) | ||
775 | return; | ||
776 | |||
777 | d.Vector3 dtmp = d.BodyGetPosition(Body); | ||
778 | Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
779 | |||
780 | // the Amotor still lets avatar rotation to drift during colisions | ||
781 | // so force it back to identity | ||
782 | |||
783 | d.Quaternion qtmp; | ||
784 | qtmp.W = 1; | ||
785 | qtmp.X = 0; | ||
786 | qtmp.Y = 0; | ||
787 | qtmp.Z = 0; | ||
788 | d.BodySetQuaternion(Body, ref qtmp); | ||
789 | |||
790 | if (m_pidControllerActive == false) | ||
791 | { | ||
792 | _zeroPosition = localpos; | ||
793 | } | ||
794 | |||
795 | if (!localpos.IsFinite()) | ||
796 | { | ||
797 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
798 | defects.Add(this); | ||
799 | // _parent_scene.RemoveCharacter(this); | ||
800 | |||
801 | // destroy avatar capsule and related ODE data | ||
802 | AvatarGeomAndBodyDestroy(); | ||
803 | return; | ||
804 | } | ||
805 | |||
806 | // check outbounds forcing to be in world | ||
807 | bool fixbody = false; | ||
808 | if (localpos.X < 0.0f) | ||
809 | { | ||
810 | fixbody = true; | ||
811 | localpos.X = 0.1f; | ||
812 | } | ||
813 | else if (localpos.X > _parent_scene.WorldExtents.X - 0.1f) | ||
814 | { | ||
815 | fixbody = true; | ||
816 | localpos.X = _parent_scene.WorldExtents.X - 0.1f; | ||
817 | } | ||
818 | if (localpos.Y < 0.0f) | ||
819 | { | ||
820 | fixbody = true; | ||
821 | localpos.Y = 0.1f; | ||
822 | } | ||
823 | else if (localpos.Y > _parent_scene.WorldExtents.Y - 0.1) | ||
824 | { | ||
825 | fixbody = true; | ||
826 | localpos.Y = _parent_scene.WorldExtents.Y - 0.1f; | ||
827 | } | ||
828 | if (fixbody) | ||
829 | { | ||
830 | m_freemove = false; | ||
831 | d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); | ||
832 | } | ||
833 | |||
834 | float breakfactor; | ||
835 | |||
836 | Vector3 vec = Vector3.Zero; | ||
837 | dtmp = d.BodyGetLinearVel(Body); | ||
838 | Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
839 | float velLengthSquared = vel.LengthSquared(); | ||
840 | |||
841 | float movementdivisor = 1f; | ||
842 | //Ubit change divisions into multiplications below | ||
843 | if (!m_alwaysRun) | ||
844 | movementdivisor = 1 / walkDivisor; | ||
845 | else | ||
846 | movementdivisor = 1 / runDivisor; | ||
847 | |||
848 | //****************************************** | ||
849 | // colide with land | ||
850 | d.AABB aabb; | ||
851 | d.GeomGetAABB(Shell, out aabb); | ||
852 | float chrminZ = aabb.MinZ; | ||
853 | |||
854 | Vector3 posch = localpos; | ||
855 | |||
856 | float ftmp; | ||
857 | |||
858 | if (flying) | ||
859 | { | ||
860 | ftmp = timeStep; | ||
861 | posch.X += vel.X * ftmp; | ||
862 | posch.Y += vel.Y * ftmp; | ||
863 | } | ||
864 | |||
865 | float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); | ||
866 | if (chrminZ < terrainheight) | ||
867 | { | ||
868 | float depth = terrainheight - chrminZ; | ||
869 | if (!flying) | ||
870 | { | ||
871 | vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; | ||
872 | } | ||
873 | else | ||
874 | vec.Z = depth * PID_P * 50; | ||
875 | |||
876 | if (depth < 0.1f) | ||
877 | { | ||
878 | m_colliderGroundfilter++; | ||
879 | if (m_colliderGroundfilter > 2) | ||
880 | { | ||
881 | m_iscolliding = true; | ||
882 | m_colliderfilter = 2; | ||
883 | |||
884 | if (m_colliderGroundfilter > 10) | ||
885 | { | ||
886 | m_colliderGroundfilter = 10; | ||
887 | m_freemove = false; | ||
888 | } | ||
889 | |||
890 | m_iscollidingGround = true; | ||
891 | |||
892 | ContactPoint contact = new ContactPoint(); | ||
893 | contact.PenetrationDepth = depth; | ||
894 | contact.Position.X = localpos.X; | ||
895 | contact.Position.Y = localpos.Y; | ||
896 | contact.Position.Z = chrminZ; | ||
897 | contact.SurfaceNormal.X = 0f; | ||
898 | contact.SurfaceNormal.Y = 0f; | ||
899 | contact.SurfaceNormal.Z = -1f; | ||
900 | AddCollisionEvent(0, contact); | ||
901 | |||
902 | vec.Z *= 0.5f; | ||
903 | } | ||
904 | } | ||
905 | |||
906 | else | ||
907 | { | ||
908 | m_colliderGroundfilter = 0; | ||
909 | m_iscollidingGround = false; | ||
910 | } | ||
911 | } | ||
912 | else | ||
913 | { | ||
914 | m_colliderGroundfilter = 0; | ||
915 | m_iscollidingGround = false; | ||
916 | } | ||
917 | |||
918 | //****************************************** | ||
919 | |||
920 | bool tviszero = (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f); | ||
921 | |||
922 | // if (!tviszero || m_iscolliding || velLengthSquared <0.01) | ||
923 | if (!tviszero) | ||
924 | m_freemove = false; | ||
925 | |||
926 | if (!m_freemove) | ||
927 | { | ||
928 | |||
929 | // if velocity is zero, use position control; otherwise, velocity control | ||
930 | if (tviszero && m_iscolliding) | ||
931 | { | ||
932 | // keep track of where we stopped. No more slippin' & slidin' | ||
933 | if (!_zeroFlag) | ||
934 | { | ||
935 | _zeroFlag = true; | ||
936 | _zeroPosition = localpos; | ||
937 | } | ||
938 | if (m_pidControllerActive) | ||
939 | { | ||
940 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
941 | // react to the physics scene by moving it's position. | ||
942 | // Avatar to Avatar collisions | ||
943 | // Prim to avatar collisions | ||
944 | |||
945 | vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); | ||
946 | vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); | ||
947 | if (flying) | ||
948 | { | ||
949 | vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; | ||
950 | } | ||
951 | } | ||
952 | //PidStatus = true; | ||
953 | } | ||
954 | else | ||
955 | { | ||
956 | m_pidControllerActive = true; | ||
957 | _zeroFlag = false; | ||
958 | |||
959 | if (m_iscolliding) | ||
960 | { | ||
961 | if (!flying) | ||
962 | { | ||
963 | if (_target_velocity.Z > 0.0f) | ||
964 | { | ||
965 | // We're colliding with something and we're not flying but we're moving | ||
966 | // This means we're walking or running. JUMPING | ||
967 | vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; | ||
968 | } | ||
969 | // We're standing on something | ||
970 | vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); | ||
971 | vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); | ||
972 | } | ||
973 | else | ||
974 | { | ||
975 | // We're flying and colliding with something | ||
976 | vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); | ||
977 | vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); | ||
978 | vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); | ||
979 | } | ||
980 | } | ||
981 | else // ie not colliding | ||
982 | { | ||
983 | if (flying) //(!m_iscolliding && flying) | ||
984 | { | ||
985 | // we're in mid air suspended | ||
986 | vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); | ||
987 | vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); | ||
988 | vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); | ||
989 | } | ||
990 | |||
991 | else | ||
992 | { | ||
993 | // we're not colliding and we're not flying so that means we're falling! | ||
994 | // m_iscolliding includes collisions with the ground. | ||
995 | |||
996 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
997 | vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; | ||
998 | vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; | ||
999 | } | ||
1000 | } | ||
1001 | } | ||
1002 | |||
1003 | if (velLengthSquared > 2500.0f) // 50m/s apply breaks | ||
1004 | { | ||
1005 | breakfactor = 0.16f * m_mass; | ||
1006 | vec.X -= breakfactor * vel.X; | ||
1007 | vec.Y -= breakfactor * vel.Y; | ||
1008 | vec.Z -= breakfactor * vel.Z; | ||
1009 | } | ||
1010 | } | ||
1011 | else | ||
1012 | { | ||
1013 | breakfactor = m_mass; | ||
1014 | vec.X -= breakfactor * vel.X; | ||
1015 | vec.Y -= breakfactor * vel.Y; | ||
1016 | if (flying) | ||
1017 | vec.Z -= breakfactor * vel.Z; | ||
1018 | else | ||
1019 | vec.Z -= .5f* m_mass * vel.Z; | ||
1020 | } | ||
1021 | |||
1022 | if (flying) | ||
1023 | { | ||
1024 | vec.Z -= _parent_scene.gravityz * m_mass; | ||
1025 | |||
1026 | //Added for auto fly height. Kitto Flora | ||
1027 | float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; | ||
1028 | |||
1029 | if (localpos.Z < target_altitude) | ||
1030 | { | ||
1031 | vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; | ||
1032 | } | ||
1033 | // end add Kitto Flora | ||
1034 | } | ||
1035 | |||
1036 | if (vec.IsFinite()) | ||
1037 | { | ||
1038 | if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) | ||
1039 | d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); | ||
1040 | } | ||
1041 | else | ||
1042 | { | ||
1043 | m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); | ||
1044 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
1045 | defects.Add(this); | ||
1046 | // _parent_scene.RemoveCharacter(this); | ||
1047 | // destroy avatar capsule and related ODE data | ||
1048 | AvatarGeomAndBodyDestroy(); | ||
1049 | return; | ||
1050 | } | ||
1051 | |||
1052 | // update our local ideia of position velocity and aceleration | ||
1053 | _position = localpos; | ||
1054 | if (_zeroFlag) | ||
1055 | { | ||
1056 | _velocity = Vector3.Zero; | ||
1057 | _acceleration = Vector3.Zero; | ||
1058 | } | ||
1059 | else | ||
1060 | { | ||
1061 | _acceleration = _velocity; // previus velocity | ||
1062 | _velocity = vel; | ||
1063 | _acceleration = (vel - _acceleration) / timeStep; | ||
1064 | } | ||
1065 | |||
1066 | } | ||
1067 | |||
1068 | /// <summary> | ||
1069 | /// Updates the reported position and velocity. | ||
1070 | /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording | ||
1071 | /// also outbounds checking | ||
1072 | /// copy and outbounds now done in move(..) at ode rate | ||
1073 | /// | ||
1074 | /// </summary> | ||
1075 | public void UpdatePositionAndVelocity() | ||
1076 | { | ||
1077 | return; | ||
1078 | |||
1079 | // if (Body == IntPtr.Zero) | ||
1080 | // return; | ||
1081 | |||
1082 | } | ||
1083 | |||
1084 | /// <summary> | ||
1085 | /// Cleanup the things we use in the scene. | ||
1086 | /// </summary> | ||
1087 | public void Destroy() | ||
1088 | { | ||
1089 | AddChange(changes.Remove, null); | ||
1090 | } | ||
1091 | |||
1092 | public override void CrossingFailure() | ||
1093 | { | ||
1094 | } | ||
1095 | |||
1096 | public override Vector3 PIDTarget { set { return; } } | ||
1097 | public override bool PIDActive { set { return; } } | ||
1098 | public override float PIDTau { set { return; } } | ||
1099 | |||
1100 | public override float PIDHoverHeight { set { return; } } | ||
1101 | public override bool PIDHoverActive { set { return; } } | ||
1102 | public override PIDHoverType PIDHoverType { set { return; } } | ||
1103 | public override float PIDHoverTau { set { return; } } | ||
1104 | |||
1105 | public override Quaternion APIDTarget { set { return; } } | ||
1106 | |||
1107 | public override bool APIDActive { set { return; } } | ||
1108 | |||
1109 | public override float APIDStrength { set { return; } } | ||
1110 | |||
1111 | public override float APIDDamping { set { return; } } | ||
1112 | |||
1113 | |||
1114 | public override void SubscribeEvents(int ms) | ||
1115 | { | ||
1116 | m_requestedUpdateFrequency = ms; | ||
1117 | m_eventsubscription = ms; | ||
1118 | _parent_scene.AddCollisionEventReporting(this); | ||
1119 | m_haseventsubscription = true; | ||
1120 | } | ||
1121 | |||
1122 | public override void UnSubscribeEvents() | ||
1123 | { | ||
1124 | m_haseventsubscription = false; | ||
1125 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1126 | m_requestedUpdateFrequency = 0; | ||
1127 | m_eventsubscription = 0; | ||
1128 | } | ||
1129 | |||
1130 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1131 | { | ||
1132 | if (m_haseventsubscription) | ||
1133 | { | ||
1134 | // m_log.DebugFormat( | ||
1135 | // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); | ||
1136 | |||
1137 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1138 | } | ||
1139 | } | ||
1140 | |||
1141 | public void SendCollisions() | ||
1142 | { | ||
1143 | if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency) | ||
1144 | { | ||
1145 | if (CollisionEventsThisFrame != null) | ||
1146 | { | ||
1147 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1148 | } | ||
1149 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1150 | m_eventsubscription = 0; | ||
1151 | } | ||
1152 | } | ||
1153 | |||
1154 | public override bool SubscribedEvents() | ||
1155 | { | ||
1156 | return m_haseventsubscription; | ||
1157 | } | ||
1158 | |||
1159 | private void changePhysicsStatus(bool NewStatus) | ||
1160 | { | ||
1161 | if (NewStatus != m_isPhysical) | ||
1162 | { | ||
1163 | if (NewStatus) | ||
1164 | { | ||
1165 | // Create avatar capsule and related ODE data | ||
1166 | if ((Shell != IntPtr.Zero)) | ||
1167 | { | ||
1168 | // a lost shell ? | ||
1169 | m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " | ||
1170 | + (Shell != IntPtr.Zero ? "Shell " : "") | ||
1171 | + (Body != IntPtr.Zero ? "Body " : "") | ||
1172 | + (Amotor != IntPtr.Zero ? "Amotor " : "")); | ||
1173 | AvatarGeomAndBodyDestroy(); | ||
1174 | } | ||
1175 | |||
1176 | AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); | ||
1177 | _parent_scene.geom_name_map[Shell] = m_name; | ||
1178 | _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; | ||
1179 | _parent_scene.AddCharacter(this); | ||
1180 | } | ||
1181 | else | ||
1182 | { | ||
1183 | _parent_scene.RemoveCharacter(this); | ||
1184 | // destroy avatar capsule and related ODE data | ||
1185 | AvatarGeomAndBodyDestroy(); | ||
1186 | } | ||
1187 | |||
1188 | m_isPhysical = NewStatus; | ||
1189 | } | ||
1190 | } | ||
1191 | |||
1192 | private void changeAdd() | ||
1193 | { | ||
1194 | changePhysicsStatus(true); | ||
1195 | } | ||
1196 | |||
1197 | private void changeRemove() | ||
1198 | { | ||
1199 | changePhysicsStatus(false); | ||
1200 | } | ||
1201 | |||
1202 | private void changeShape(PrimitiveBaseShape arg) | ||
1203 | { | ||
1204 | } | ||
1205 | |||
1206 | private void changeSize(Vector3 Size) | ||
1207 | { | ||
1208 | if (Size.IsFinite()) | ||
1209 | { | ||
1210 | float caplen = Size.Z; | ||
1211 | |||
1212 | caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f; | ||
1213 | |||
1214 | if (caplen != CAPSULE_LENGTH) | ||
1215 | { | ||
1216 | if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) | ||
1217 | { | ||
1218 | AvatarGeomAndBodyDestroy(); | ||
1219 | |||
1220 | float prevCapsule = CAPSULE_LENGTH; | ||
1221 | CAPSULE_LENGTH = caplen; | ||
1222 | |||
1223 | AvatarGeomAndBodyCreation(_position.X, _position.Y, | ||
1224 | _position.Z + (CAPSULE_LENGTH - prevCapsule) * 0.5f); | ||
1225 | |||
1226 | Velocity = Vector3.Zero; | ||
1227 | |||
1228 | _parent_scene.geom_name_map[Shell] = m_name; | ||
1229 | _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; | ||
1230 | } | ||
1231 | else | ||
1232 | { | ||
1233 | m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " | ||
1234 | + (Shell == IntPtr.Zero ? "Shell " : "") | ||
1235 | + (Body == IntPtr.Zero ? "Body " : "") | ||
1236 | + (Amotor == IntPtr.Zero ? "Amotor " : "")); | ||
1237 | } | ||
1238 | } | ||
1239 | |||
1240 | m_pidControllerActive = true; | ||
1241 | } | ||
1242 | else | ||
1243 | { | ||
1244 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
1245 | } | ||
1246 | } | ||
1247 | |||
1248 | private void changePosition( Vector3 newPos) | ||
1249 | { | ||
1250 | if (Body != IntPtr.Zero) | ||
1251 | d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); | ||
1252 | _position = newPos; | ||
1253 | m_pidControllerActive = true; | ||
1254 | } | ||
1255 | |||
1256 | private void changeOrientation(Quaternion newOri) | ||
1257 | { | ||
1258 | } | ||
1259 | |||
1260 | private void changeVelocity(Vector3 newVel) | ||
1261 | { | ||
1262 | m_pidControllerActive = true; | ||
1263 | _target_velocity = newVel; | ||
1264 | } | ||
1265 | |||
1266 | private void changeSetTorque(Vector3 newTorque) | ||
1267 | { | ||
1268 | } | ||
1269 | |||
1270 | private void changeAddForce(Vector3 newForce) | ||
1271 | { | ||
1272 | } | ||
1273 | |||
1274 | private void changeAddAngularForce(Vector3 arg) | ||
1275 | { | ||
1276 | } | ||
1277 | |||
1278 | private void changeAngularLock(Vector3 arg) | ||
1279 | { | ||
1280 | } | ||
1281 | |||
1282 | private void changeFloatOnWater(bool arg) | ||
1283 | { | ||
1284 | } | ||
1285 | |||
1286 | private void changeVolumedetetion(bool arg) | ||
1287 | { | ||
1288 | } | ||
1289 | |||
1290 | private void changeSelectedStatus(bool arg) | ||
1291 | { | ||
1292 | } | ||
1293 | |||
1294 | private void changeDisable(bool arg) | ||
1295 | { | ||
1296 | } | ||
1297 | |||
1298 | private void changeBuilding(bool arg) | ||
1299 | { | ||
1300 | } | ||
1301 | |||
1302 | private void setFreeMove() | ||
1303 | { | ||
1304 | m_pidControllerActive = true; | ||
1305 | _zeroFlag = false; | ||
1306 | _target_velocity = Vector3.Zero; | ||
1307 | m_freemove = true; | ||
1308 | m_colliderfilter = -2; | ||
1309 | m_colliderObjectfilter = -2; | ||
1310 | m_colliderGroundfilter = -2; | ||
1311 | |||
1312 | m_iscolliding = false; | ||
1313 | m_iscollidingGround = false; | ||
1314 | m_iscollidingObj = false; | ||
1315 | |||
1316 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1317 | m_eventsubscription = 0; | ||
1318 | } | ||
1319 | |||
1320 | private void changeForce(Vector3 newForce) | ||
1321 | { | ||
1322 | setFreeMove(); | ||
1323 | |||
1324 | if (Body != IntPtr.Zero) | ||
1325 | { | ||
1326 | if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) | ||
1327 | d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); | ||
1328 | } | ||
1329 | } | ||
1330 | |||
1331 | // for now momentum is actually velocity | ||
1332 | private void changeMomentum(Vector3 newmomentum) | ||
1333 | { | ||
1334 | _velocity = newmomentum; | ||
1335 | setFreeMove(); | ||
1336 | |||
1337 | if (Body != IntPtr.Zero) | ||
1338 | d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); | ||
1339 | } | ||
1340 | |||
1341 | private void donullchange() | ||
1342 | { | ||
1343 | } | ||
1344 | |||
1345 | public bool DoAChange(changes what, object arg) | ||
1346 | { | ||
1347 | if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) | ||
1348 | { | ||
1349 | return false; | ||
1350 | } | ||
1351 | |||
1352 | // nasty switch | ||
1353 | switch (what) | ||
1354 | { | ||
1355 | case changes.Add: | ||
1356 | changeAdd(); | ||
1357 | break; | ||
1358 | case changes.Remove: | ||
1359 | changeRemove(); | ||
1360 | break; | ||
1361 | |||
1362 | case changes.Position: | ||
1363 | changePosition((Vector3)arg); | ||
1364 | break; | ||
1365 | |||
1366 | case changes.Orientation: | ||
1367 | changeOrientation((Quaternion)arg); | ||
1368 | break; | ||
1369 | |||
1370 | case changes.PosOffset: | ||
1371 | donullchange(); | ||
1372 | break; | ||
1373 | |||
1374 | case changes.OriOffset: | ||
1375 | donullchange(); | ||
1376 | break; | ||
1377 | |||
1378 | case changes.Velocity: | ||
1379 | changeVelocity((Vector3)arg); | ||
1380 | break; | ||
1381 | |||
1382 | // case changes.Acceleration: | ||
1383 | // changeacceleration((Vector3)arg); | ||
1384 | // break; | ||
1385 | // case changes.AngVelocity: | ||
1386 | // changeangvelocity((Vector3)arg); | ||
1387 | // break; | ||
1388 | |||
1389 | case changes.Force: | ||
1390 | changeForce((Vector3)arg); | ||
1391 | break; | ||
1392 | |||
1393 | case changes.Torque: | ||
1394 | changeSetTorque((Vector3)arg); | ||
1395 | break; | ||
1396 | |||
1397 | case changes.AddForce: | ||
1398 | changeAddForce((Vector3)arg); | ||
1399 | break; | ||
1400 | |||
1401 | case changes.AddAngForce: | ||
1402 | changeAddAngularForce((Vector3)arg); | ||
1403 | break; | ||
1404 | |||
1405 | case changes.AngLock: | ||
1406 | changeAngularLock((Vector3)arg); | ||
1407 | break; | ||
1408 | |||
1409 | case changes.Size: | ||
1410 | changeSize((Vector3)arg); | ||
1411 | break; | ||
1412 | |||
1413 | case changes.Momentum: | ||
1414 | changeMomentum((Vector3)arg); | ||
1415 | break; | ||
1416 | /* not in use for now | ||
1417 | case changes.Shape: | ||
1418 | changeShape((PrimitiveBaseShape)arg); | ||
1419 | break; | ||
1420 | |||
1421 | case changes.CollidesWater: | ||
1422 | changeFloatOnWater((bool)arg); | ||
1423 | break; | ||
1424 | |||
1425 | case changes.VolumeDtc: | ||
1426 | changeVolumedetetion((bool)arg); | ||
1427 | break; | ||
1428 | |||
1429 | case changes.Physical: | ||
1430 | changePhysicsStatus((bool)arg); | ||
1431 | break; | ||
1432 | |||
1433 | case changes.Selected: | ||
1434 | changeSelectedStatus((bool)arg); | ||
1435 | break; | ||
1436 | |||
1437 | case changes.disabled: | ||
1438 | changeDisable((bool)arg); | ||
1439 | break; | ||
1440 | |||
1441 | case changes.building: | ||
1442 | changeBuilding((bool)arg); | ||
1443 | break; | ||
1444 | */ | ||
1445 | case changes.Null: | ||
1446 | donullchange(); | ||
1447 | break; | ||
1448 | |||
1449 | default: | ||
1450 | donullchange(); | ||
1451 | break; | ||
1452 | } | ||
1453 | return false; | ||
1454 | } | ||
1455 | |||
1456 | public void AddChange(changes what, object arg) | ||
1457 | { | ||
1458 | _parent_scene.AddChange((PhysicsActor)this, what, arg); | ||
1459 | } | ||
1460 | |||
1461 | |||
1462 | internal void AddCollisionFrameTime(int p) | ||
1463 | { | ||
1464 | // protect it from overflow crashing | ||
1465 | if (m_eventsubscription + p >= int.MaxValue) | ||
1466 | m_eventsubscription = 0; | ||
1467 | m_eventsubscription += p; | ||
1468 | } | ||
1469 | } | ||
1470 | } | ||