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