diff options
author | onefang | 2019-09-11 16:36:50 +1000 |
---|---|---|
committer | onefang | 2019-09-11 16:36:50 +1000 |
commit | 50cd1ffd32f69228e566f2b0b89f86ea0d9fe489 (patch) | |
tree | 52f2ab0c04f1a5d7d6ac5dc872981b4b156447e7 /OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs | |
parent | Renamed branch to SledjChisl. (diff) | |
parent | Bump to release flavour, build 0. (diff) | |
download | opensim-SC_OLD-50cd1ffd32f69228e566f2b0b89f86ea0d9fe489.zip opensim-SC_OLD-50cd1ffd32f69228e566f2b0b89f86ea0d9fe489.tar.gz opensim-SC_OLD-50cd1ffd32f69228e566f2b0b89f86ea0d9fe489.tar.bz2 opensim-SC_OLD-50cd1ffd32f69228e566f2b0b89f86ea0d9fe489.tar.xz |
Merge branch 'SledjChisl'
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs | 2036 |
1 files changed, 2036 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs new file mode 100644 index 0000000..0e46471 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs | |||
@@ -0,0 +1,2036 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | |||
28 | |||
29 | // Revision by Ubit 2011/12 | ||
30 | |||
31 | using System; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using OpenMetaverse; | ||
35 | using OdeAPI; | ||
36 | using OpenSim.Framework; | ||
37 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
38 | using log4net; | ||
39 | |||
40 | namespace OpenSim.Region.PhysicsModule.ubOde | ||
41 | { | ||
42 | /// <summary> | ||
43 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
44 | /// </summary> | ||
45 | |||
46 | public enum dParam : int | ||
47 | { | ||
48 | LowStop = 0, | ||
49 | HiStop = 1, | ||
50 | Vel = 2, | ||
51 | FMax = 3, | ||
52 | FudgeFactor = 4, | ||
53 | Bounce = 5, | ||
54 | CFM = 6, | ||
55 | StopERP = 7, | ||
56 | StopCFM = 8, | ||
57 | LoStop2 = 256, | ||
58 | HiStop2 = 257, | ||
59 | Vel2 = 258, | ||
60 | FMax2 = 259, | ||
61 | StopERP2 = 7 + 256, | ||
62 | StopCFM2 = 8 + 256, | ||
63 | LoStop3 = 512, | ||
64 | HiStop3 = 513, | ||
65 | Vel3 = 514, | ||
66 | FMax3 = 515, | ||
67 | StopERP3 = 7 + 512, | ||
68 | StopCFM3 = 8 + 512 | ||
69 | } | ||
70 | |||
71 | public class OdeCharacter : PhysicsActor | ||
72 | { | ||
73 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
74 | |||
75 | private Vector3 _position; | ||
76 | private Vector3 _zeroPosition; | ||
77 | private Vector3 _velocity; | ||
78 | private Vector3 _target_velocity; | ||
79 | private Vector3 _acceleration; | ||
80 | private Vector3 m_rotationalVelocity; | ||
81 | private Vector3 m_size; | ||
82 | private Vector3 m_collideNormal; | ||
83 | private Vector3 m_lastFallVel; | ||
84 | private Quaternion m_orientation; | ||
85 | private Quaternion m_orientation2D; | ||
86 | private float m_mass = 80f; | ||
87 | public float m_density = 60f; | ||
88 | private bool m_pidControllerActive = true; | ||
89 | |||
90 | const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) | ||
91 | const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) | ||
92 | public float PID_D; | ||
93 | public float PID_P; | ||
94 | |||
95 | private float timeStep; | ||
96 | private float invtimeStep; | ||
97 | |||
98 | private float m_feetOffset = 0; | ||
99 | private float feetOff = 0; | ||
100 | private float boneOff = 0; | ||
101 | private float AvaAvaSizeXsq = 0.3f; | ||
102 | private float AvaAvaSizeYsq = 0.2f; | ||
103 | |||
104 | public float walkDivisor = 1.3f; | ||
105 | public float runDivisor = 0.8f; | ||
106 | private bool m_flying = false; | ||
107 | private bool m_iscolliding = false; | ||
108 | private bool m_iscollidingGround = false; | ||
109 | private bool m_iscollidingObj = false; | ||
110 | private bool m_alwaysRun = false; | ||
111 | |||
112 | private bool _zeroFlag = false; | ||
113 | private bool m_haveLastFallVel = false; | ||
114 | |||
115 | private uint m_localID = 0; | ||
116 | public bool m_returnCollisions = false; | ||
117 | // taints and their non-tainted counterparts | ||
118 | public bool m_isPhysical = false; // the current physical status | ||
119 | public float MinimumGroundFlightOffset = 3f; | ||
120 | |||
121 | private float m_buoyancy = 0f; | ||
122 | |||
123 | private bool m_freemove = false; | ||
124 | |||
125 | // private string m_name = String.Empty; | ||
126 | // other filter control | ||
127 | int m_colliderfilter = 0; | ||
128 | int m_colliderGroundfilter = 0; | ||
129 | int m_colliderObjectfilter = 0; | ||
130 | |||
131 | // Default we're a Character | ||
132 | private CollisionCategories m_collisionCategories = (CollisionCategories.Character); | ||
133 | |||
134 | // Default, Collide with Other Geometries, spaces, bodies and characters. | ||
135 | private CollisionCategories m_collisionFlags = (CollisionCategories.Character | ||
136 | | CollisionCategories.Geom | ||
137 | | CollisionCategories.VolumeDtc | ||
138 | ); | ||
139 | // we do land collisions not ode | CollisionCategories.Land); | ||
140 | public IntPtr Body = IntPtr.Zero; | ||
141 | private ODEScene m_parent_scene; | ||
142 | private IntPtr capsule = IntPtr.Zero; | ||
143 | public IntPtr collider = IntPtr.Zero; | ||
144 | |||
145 | public IntPtr Amotor = IntPtr.Zero; | ||
146 | |||
147 | public d.Mass ShellMass; | ||
148 | |||
149 | public int m_eventsubscription = 0; | ||
150 | private int m_cureventsubscription = 0; | ||
151 | private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
152 | private bool SentEmptyCollisionsEvent; | ||
153 | |||
154 | // unique UUID of this character object | ||
155 | public UUID m_uuid; | ||
156 | public bool bad = false; | ||
157 | |||
158 | float mu; | ||
159 | |||
160 | // HoverHeight control | ||
161 | private float m_PIDHoverHeight; | ||
162 | private float m_PIDHoverTau; | ||
163 | private bool m_useHoverPID; | ||
164 | private PIDHoverType m_PIDHoverType; | ||
165 | private float m_targetHoverHeight; | ||
166 | |||
167 | |||
168 | public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) | ||
169 | { | ||
170 | m_uuid = UUID.Random(); | ||
171 | m_localID = localID; | ||
172 | m_parent_scene = parent_scene; | ||
173 | |||
174 | timeStep = parent_scene.ODE_STEPSIZE; | ||
175 | invtimeStep = 1 / timeStep; | ||
176 | |||
177 | if (pos.IsFinite()) | ||
178 | { | ||
179 | if (pos.Z > 99999f) | ||
180 | { | ||
181 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
182 | } | ||
183 | if (pos.Z < -100f) // shouldn't this be 0 ? | ||
184 | { | ||
185 | pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
186 | } | ||
187 | _position = pos; | ||
188 | } | ||
189 | else | ||
190 | { | ||
191 | _position = new Vector3(((float)m_parent_scene.WorldExtents.X * 0.5f), ((float)m_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); | ||
192 | m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); | ||
193 | } | ||
194 | |||
195 | m_size.X = pSize.X; | ||
196 | m_size.Y = pSize.Y; | ||
197 | m_size.Z = pSize.Z; | ||
198 | |||
199 | if(m_size.X <0.01f) | ||
200 | m_size.X = 0.01f; | ||
201 | if(m_size.Y <0.01f) | ||
202 | m_size.Y = 0.01f; | ||
203 | if(m_size.Z <0.01f) | ||
204 | m_size.Z = 0.01f; | ||
205 | |||
206 | m_feetOffset = pfeetOffset; | ||
207 | m_orientation = Quaternion.Identity; | ||
208 | m_orientation2D = Quaternion.Identity; | ||
209 | m_density = density; | ||
210 | |||
211 | // force lower density for testing | ||
212 | m_density = 3.0f; | ||
213 | |||
214 | mu = m_parent_scene.AvatarFriction; | ||
215 | |||
216 | walkDivisor = walk_divisor; | ||
217 | runDivisor = rundivisor; | ||
218 | |||
219 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default | ||
220 | |||
221 | PID_D = basePID_D * m_mass * invtimeStep; | ||
222 | PID_P = basePID_P * m_mass * invtimeStep; | ||
223 | |||
224 | m_isPhysical = false; // current status: no ODE information exists | ||
225 | |||
226 | Name = avName; | ||
227 | |||
228 | AddChange(changes.Add, null); | ||
229 | } | ||
230 | |||
231 | public override int PhysicsActorType | ||
232 | { | ||
233 | get { return (int)ActorTypes.Agent; } | ||
234 | set { return; } | ||
235 | } | ||
236 | |||
237 | public override void getContactData(ref ContactData cdata) | ||
238 | { | ||
239 | cdata.mu = mu; | ||
240 | cdata.bounce = 0; | ||
241 | cdata.softcolide = false; | ||
242 | } | ||
243 | |||
244 | public override bool Building { get; set; } | ||
245 | |||
246 | /// <summary> | ||
247 | /// If this is set, the avatar will move faster | ||
248 | /// </summary> | ||
249 | public override bool SetAlwaysRun | ||
250 | { | ||
251 | get { return m_alwaysRun; } | ||
252 | set { m_alwaysRun = value; } | ||
253 | } | ||
254 | |||
255 | public override uint LocalID | ||
256 | { | ||
257 | get { return m_localID; } | ||
258 | set { m_localID = value; } | ||
259 | } | ||
260 | |||
261 | public override PhysicsActor ParentActor | ||
262 | { | ||
263 | get { return (PhysicsActor)this; } | ||
264 | } | ||
265 | |||
266 | public override bool Grabbed | ||
267 | { | ||
268 | set { return; } | ||
269 | } | ||
270 | |||
271 | public override bool Selected | ||
272 | { | ||
273 | set { return; } | ||
274 | } | ||
275 | |||
276 | public override float Buoyancy | ||
277 | { | ||
278 | get { return m_buoyancy; } | ||
279 | set { m_buoyancy = value; } | ||
280 | } | ||
281 | |||
282 | public override bool FloatOnWater | ||
283 | { | ||
284 | set { return; } | ||
285 | } | ||
286 | |||
287 | public override bool IsPhysical | ||
288 | { | ||
289 | get { return m_isPhysical; } | ||
290 | set { return; } | ||
291 | } | ||
292 | |||
293 | public override bool ThrottleUpdates | ||
294 | { | ||
295 | get { return false; } | ||
296 | set { return; } | ||
297 | } | ||
298 | |||
299 | public override bool Flying | ||
300 | { | ||
301 | get { return m_flying; } | ||
302 | set | ||
303 | { | ||
304 | m_flying = value; | ||
305 | // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); | ||
306 | } | ||
307 | } | ||
308 | |||
309 | /// <summary> | ||
310 | /// Returns if the avatar is colliding in general. | ||
311 | /// This includes the ground and objects and avatar. | ||
312 | /// </summary> | ||
313 | public override bool IsColliding | ||
314 | { | ||
315 | get { return (m_iscolliding || m_iscollidingGround); } | ||
316 | set | ||
317 | { | ||
318 | if (value) | ||
319 | { | ||
320 | m_colliderfilter += 3; | ||
321 | if (m_colliderfilter > 3) | ||
322 | m_colliderfilter = 3; | ||
323 | } | ||
324 | else | ||
325 | { | ||
326 | m_colliderfilter--; | ||
327 | if (m_colliderfilter < 0) | ||
328 | m_colliderfilter = 0; | ||
329 | } | ||
330 | |||
331 | if (m_colliderfilter == 0) | ||
332 | m_iscolliding = false; | ||
333 | else | ||
334 | { | ||
335 | m_pidControllerActive = true; | ||
336 | m_iscolliding = true; | ||
337 | m_freemove = false; | ||
338 | } | ||
339 | } | ||
340 | } | ||
341 | |||
342 | /// <summary> | ||
343 | /// Returns if an avatar is colliding with the ground | ||
344 | /// </summary> | ||
345 | public override bool CollidingGround | ||
346 | { | ||
347 | get { return m_iscollidingGround; } | ||
348 | set | ||
349 | { | ||
350 | /* we now control this | ||
351 | if (value) | ||
352 | { | ||
353 | m_colliderGroundfilter += 2; | ||
354 | if (m_colliderGroundfilter > 2) | ||
355 | m_colliderGroundfilter = 2; | ||
356 | } | ||
357 | else | ||
358 | { | ||
359 | m_colliderGroundfilter--; | ||
360 | if (m_colliderGroundfilter < 0) | ||
361 | m_colliderGroundfilter = 0; | ||
362 | } | ||
363 | |||
364 | if (m_colliderGroundfilter == 0) | ||
365 | m_iscollidingGround = false; | ||
366 | else | ||
367 | m_iscollidingGround = true; | ||
368 | */ | ||
369 | } | ||
370 | |||
371 | } | ||
372 | |||
373 | /// <summary> | ||
374 | /// Returns if the avatar is colliding with an object | ||
375 | /// </summary> | ||
376 | public override bool CollidingObj | ||
377 | { | ||
378 | get { return m_iscollidingObj; } | ||
379 | set | ||
380 | { | ||
381 | // Ubit filter this also | ||
382 | if (value) | ||
383 | { | ||
384 | m_colliderObjectfilter += 2; | ||
385 | if (m_colliderObjectfilter > 2) | ||
386 | m_colliderObjectfilter = 2; | ||
387 | } | ||
388 | else | ||
389 | { | ||
390 | m_colliderObjectfilter--; | ||
391 | if (m_colliderObjectfilter < 0) | ||
392 | m_colliderObjectfilter = 0; | ||
393 | } | ||
394 | |||
395 | if (m_colliderObjectfilter == 0) | ||
396 | m_iscollidingObj = false; | ||
397 | else | ||
398 | m_iscollidingObj = true; | ||
399 | |||
400 | // m_iscollidingObj = value; | ||
401 | |||
402 | if (m_iscollidingObj) | ||
403 | m_pidControllerActive = false; | ||
404 | else | ||
405 | m_pidControllerActive = true; | ||
406 | } | ||
407 | } | ||
408 | |||
409 | /// <summary> | ||
410 | /// turn the PID controller on or off. | ||
411 | /// The PID Controller will turn on all by itself in many situations | ||
412 | /// </summary> | ||
413 | /// <param name="status"></param> | ||
414 | public void SetPidStatus(bool status) | ||
415 | { | ||
416 | m_pidControllerActive = status; | ||
417 | } | ||
418 | |||
419 | public override bool Stopped | ||
420 | { | ||
421 | get { return _zeroFlag; } | ||
422 | } | ||
423 | |||
424 | /// <summary> | ||
425 | /// This 'puts' an avatar somewhere in the physics space. | ||
426 | /// Not really a good choice unless you 'know' it's a good | ||
427 | /// spot otherwise you're likely to orbit the avatar. | ||
428 | /// </summary> | ||
429 | public override Vector3 Position | ||
430 | { | ||
431 | get { return _position; } | ||
432 | set | ||
433 | { | ||
434 | if (value.IsFinite()) | ||
435 | { | ||
436 | if (value.Z > 9999999f) | ||
437 | { | ||
438 | value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
439 | } | ||
440 | if (value.Z < -100f) | ||
441 | { | ||
442 | value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5; | ||
443 | } | ||
444 | AddChange(changes.Position, value); | ||
445 | } | ||
446 | else | ||
447 | { | ||
448 | m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); | ||
449 | } | ||
450 | } | ||
451 | } | ||
452 | |||
453 | public override Vector3 RotationalVelocity | ||
454 | { | ||
455 | get { return m_rotationalVelocity; } | ||
456 | set { m_rotationalVelocity = value; } | ||
457 | } | ||
458 | |||
459 | /// <summary> | ||
460 | /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight | ||
461 | /// and use it to offset landings properly | ||
462 | /// </summary> | ||
463 | public override Vector3 Size | ||
464 | { | ||
465 | get | ||
466 | { | ||
467 | return m_size; | ||
468 | } | ||
469 | set | ||
470 | { | ||
471 | if (value.IsFinite()) | ||
472 | { | ||
473 | if(value.X <0.01f) | ||
474 | value.X = 0.01f; | ||
475 | if(value.Y <0.01f) | ||
476 | value.Y = 0.01f; | ||
477 | if(value.Z <0.01f) | ||
478 | value.Z = 0.01f; | ||
479 | |||
480 | AddChange(changes.Size, value); | ||
481 | } | ||
482 | else | ||
483 | { | ||
484 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
485 | } | ||
486 | } | ||
487 | } | ||
488 | |||
489 | public override void setAvatarSize(Vector3 size, float feetOffset) | ||
490 | { | ||
491 | if (size.IsFinite()) | ||
492 | { | ||
493 | if (size.X < 0.01f) | ||
494 | size.X = 0.01f; | ||
495 | if (size.Y < 0.01f) | ||
496 | size.Y = 0.01f; | ||
497 | if (size.Z < 0.01f) | ||
498 | size.Z = 0.01f; | ||
499 | |||
500 | strAvatarSize st = new strAvatarSize(); | ||
501 | st.size = size; | ||
502 | st.offset = feetOffset; | ||
503 | AddChange(changes.AvatarSize, st); | ||
504 | } | ||
505 | else | ||
506 | { | ||
507 | m_log.Warn("[PHYSICS]: Got a NaN AvatarSize from Scene on a Character"); | ||
508 | } | ||
509 | |||
510 | } | ||
511 | /// <summary> | ||
512 | /// This creates the Avatar's physical Surrogate at the position supplied | ||
513 | /// </summary> | ||
514 | /// <param name="npositionX"></param> | ||
515 | /// <param name="npositionY"></param> | ||
516 | /// <param name="npositionZ"></param> | ||
517 | |||
518 | // | ||
519 | /// <summary> | ||
520 | /// Uses the capped cyllinder volume formula to calculate the avatar's mass. | ||
521 | /// This may be used in calculations in the scene/scenepresence | ||
522 | /// </summary> | ||
523 | public override float Mass | ||
524 | { | ||
525 | get | ||
526 | { | ||
527 | return m_mass; | ||
528 | } | ||
529 | } | ||
530 | public override void link(PhysicsActor obj) | ||
531 | { | ||
532 | |||
533 | } | ||
534 | |||
535 | public override void delink() | ||
536 | { | ||
537 | |||
538 | } | ||
539 | |||
540 | public override void LockAngularMotion(byte axislocks) | ||
541 | { | ||
542 | |||
543 | } | ||
544 | |||
545 | |||
546 | public override Vector3 Force | ||
547 | { | ||
548 | get { return _target_velocity; } | ||
549 | set { return; } | ||
550 | } | ||
551 | |||
552 | public override int VehicleType | ||
553 | { | ||
554 | get { return 0; } | ||
555 | set { return; } | ||
556 | } | ||
557 | |||
558 | public override void VehicleFloatParam(int param, float value) | ||
559 | { | ||
560 | |||
561 | } | ||
562 | |||
563 | public override void VehicleVectorParam(int param, Vector3 value) | ||
564 | { | ||
565 | |||
566 | } | ||
567 | |||
568 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
569 | { | ||
570 | |||
571 | } | ||
572 | |||
573 | public override void VehicleFlags(int param, bool remove) | ||
574 | { | ||
575 | |||
576 | } | ||
577 | |||
578 | public override void SetVolumeDetect(int param) | ||
579 | { | ||
580 | |||
581 | } | ||
582 | |||
583 | public override Vector3 CenterOfMass | ||
584 | { | ||
585 | get | ||
586 | { | ||
587 | Vector3 pos = _position; | ||
588 | return pos; | ||
589 | } | ||
590 | } | ||
591 | |||
592 | public override Vector3 GeometricCenter | ||
593 | { | ||
594 | get | ||
595 | { | ||
596 | Vector3 pos = _position; | ||
597 | return pos; | ||
598 | } | ||
599 | } | ||
600 | |||
601 | public override PrimitiveBaseShape Shape | ||
602 | { | ||
603 | set { return; } | ||
604 | } | ||
605 | |||
606 | public override Vector3 rootVelocity | ||
607 | { | ||
608 | get | ||
609 | { | ||
610 | return _velocity; | ||
611 | } | ||
612 | } | ||
613 | |||
614 | public override Vector3 Velocity | ||
615 | { | ||
616 | get | ||
617 | { | ||
618 | return _velocity; | ||
619 | } | ||
620 | set | ||
621 | { | ||
622 | if (value.IsFinite()) | ||
623 | { | ||
624 | AddChange(changes.Velocity, value); | ||
625 | } | ||
626 | else | ||
627 | { | ||
628 | m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); | ||
629 | } | ||
630 | } | ||
631 | } | ||
632 | |||
633 | public override Vector3 TargetVelocity | ||
634 | { | ||
635 | get | ||
636 | { | ||
637 | return m_targetVelocity; | ||
638 | } | ||
639 | set | ||
640 | { | ||
641 | if (value.IsFinite()) | ||
642 | { | ||
643 | AddChange(changes.TargetVelocity, value); | ||
644 | } | ||
645 | else | ||
646 | { | ||
647 | m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); | ||
648 | } | ||
649 | } | ||
650 | } | ||
651 | |||
652 | public override Vector3 Torque | ||
653 | { | ||
654 | get { return Vector3.Zero; } | ||
655 | set { return; } | ||
656 | } | ||
657 | |||
658 | public override float CollisionScore | ||
659 | { | ||
660 | get { return 0f; } | ||
661 | set { } | ||
662 | } | ||
663 | |||
664 | public override bool Kinematic | ||
665 | { | ||
666 | get { return false; } | ||
667 | set { } | ||
668 | } | ||
669 | |||
670 | public override Quaternion Orientation | ||
671 | { | ||
672 | get { return m_orientation; } | ||
673 | set | ||
674 | { | ||
675 | // fakeori = value; | ||
676 | // givefakeori++; | ||
677 | value.Normalize(); | ||
678 | AddChange(changes.Orientation, value); | ||
679 | } | ||
680 | } | ||
681 | |||
682 | public override Vector3 Acceleration | ||
683 | { | ||
684 | get { return _acceleration; } | ||
685 | set { } | ||
686 | } | ||
687 | |||
688 | public void SetAcceleration(Vector3 accel) | ||
689 | { | ||
690 | m_pidControllerActive = true; | ||
691 | _acceleration = accel; | ||
692 | } | ||
693 | |||
694 | /// <summary> | ||
695 | /// Adds the force supplied to the Target Velocity | ||
696 | /// The PID controller takes this target velocity and tries to make it a reality | ||
697 | /// </summary> | ||
698 | /// <param name="force"></param> | ||
699 | public override void AddForce(Vector3 force, bool pushforce) | ||
700 | { | ||
701 | if (force.IsFinite()) | ||
702 | { | ||
703 | if (pushforce) | ||
704 | { | ||
705 | AddChange(changes.Force, force * m_density / (m_parent_scene.ODE_STEPSIZE * 28f)); | ||
706 | } | ||
707 | else | ||
708 | { | ||
709 | AddChange(changes.TargetVelocity, force); | ||
710 | } | ||
711 | } | ||
712 | else | ||
713 | { | ||
714 | m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); | ||
715 | } | ||
716 | //m_lastUpdateSent = false; | ||
717 | } | ||
718 | |||
719 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
720 | { | ||
721 | |||
722 | } | ||
723 | |||
724 | public override void SetMomentum(Vector3 momentum) | ||
725 | { | ||
726 | if (momentum.IsFinite()) | ||
727 | AddChange(changes.Momentum, momentum); | ||
728 | } | ||
729 | |||
730 | |||
731 | private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) | ||
732 | { | ||
733 | // sizes one day should came from visual parameters | ||
734 | float sx = m_size.X; | ||
735 | float sy = m_size.Y; | ||
736 | float sz = m_size.Z; | ||
737 | |||
738 | float bot = -sz * 0.5f + m_feetOffset; | ||
739 | boneOff = bot + 0.3f; | ||
740 | |||
741 | float feetsz = sz * 0.45f; | ||
742 | if (feetsz > 0.6f) | ||
743 | feetsz = 0.6f; | ||
744 | |||
745 | feetOff = bot + feetsz; | ||
746 | |||
747 | AvaAvaSizeXsq = 0.4f * sx; | ||
748 | AvaAvaSizeXsq *= AvaAvaSizeXsq; | ||
749 | AvaAvaSizeYsq = 0.5f * sy; | ||
750 | AvaAvaSizeYsq *= AvaAvaSizeYsq; | ||
751 | |||
752 | m_parent_scene.waitForSpaceUnlock(m_parent_scene.CharsSpace); | ||
753 | |||
754 | collider = d.HashSpaceCreate(m_parent_scene.CharsSpace); | ||
755 | d.HashSpaceSetLevels(collider, -4, 3); | ||
756 | d.SpaceSetSublevel(collider, 3); | ||
757 | d.SpaceSetCleanup(collider, false); | ||
758 | d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); | ||
759 | d.GeomSetCollideBits(collider, (uint)m_collisionFlags); | ||
760 | |||
761 | float r = m_size.X; | ||
762 | if (m_size.Y > r) | ||
763 | r = m_size.Y; | ||
764 | float l = m_size.Z - r; | ||
765 | r *= 0.5f; | ||
766 | |||
767 | capsule = d.CreateCapsule(collider, r, l); | ||
768 | |||
769 | m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass | ||
770 | |||
771 | d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); | ||
772 | |||
773 | PID_D = basePID_D * m_mass / m_parent_scene.ODE_STEPSIZE; | ||
774 | PID_P = basePID_P * m_mass / m_parent_scene.ODE_STEPSIZE; | ||
775 | |||
776 | Body = d.BodyCreate(m_parent_scene.world); | ||
777 | |||
778 | _zeroFlag = false; | ||
779 | m_pidControllerActive = true; | ||
780 | m_freemove = false; | ||
781 | |||
782 | _velocity = Vector3.Zero; | ||
783 | |||
784 | d.BodySetAutoDisableFlag(Body, false); | ||
785 | d.BodySetPosition(Body, npositionX, npositionY, npositionZ); | ||
786 | |||
787 | _position.X = npositionX; | ||
788 | _position.Y = npositionY; | ||
789 | _position.Z = npositionZ; | ||
790 | |||
791 | d.BodySetMass(Body, ref ShellMass); | ||
792 | d.GeomSetBody(capsule, Body); | ||
793 | |||
794 | // The purpose of the AMotor here is to keep the avatar's physical | ||
795 | // surrogate from rotating while moving | ||
796 | Amotor = d.JointCreateAMotor(m_parent_scene.world, IntPtr.Zero); | ||
797 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
798 | |||
799 | d.JointSetAMotorMode(Amotor, 0); | ||
800 | d.JointSetAMotorNumAxes(Amotor, 3); | ||
801 | d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); | ||
802 | d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); | ||
803 | d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); | ||
804 | |||
805 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
806 | d.JointSetAMotorAngle(Amotor, 1, 0); | ||
807 | d.JointSetAMotorAngle(Amotor, 2, 0); | ||
808 | |||
809 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD | ||
810 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); | ||
811 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); | ||
812 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); | ||
813 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); | ||
814 | d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); | ||
815 | |||
816 | // These lowstops and high stops are effectively (no wiggle room) | ||
817 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); | ||
818 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); | ||
819 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); | ||
820 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); | ||
821 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); | ||
822 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); | ||
823 | |||
824 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
825 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); | ||
826 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); | ||
827 | |||
828 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e8f); | ||
829 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); | ||
830 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); | ||
831 | } | ||
832 | |||
833 | /// <summary> | ||
834 | /// Destroys the avatar body and geom | ||
835 | |||
836 | private void AvatarGeomAndBodyDestroy() | ||
837 | { | ||
838 | // Kill the Amotor | ||
839 | if (Amotor != IntPtr.Zero) | ||
840 | { | ||
841 | d.JointDestroy(Amotor); | ||
842 | Amotor = IntPtr.Zero; | ||
843 | } | ||
844 | |||
845 | if (Body != IntPtr.Zero) | ||
846 | { | ||
847 | //kill the body | ||
848 | d.BodyDestroy(Body); | ||
849 | Body = IntPtr.Zero; | ||
850 | } | ||
851 | |||
852 | //kill the Geoms | ||
853 | if (capsule != IntPtr.Zero) | ||
854 | { | ||
855 | m_parent_scene.actor_name_map.Remove(capsule); | ||
856 | m_parent_scene.waitForSpaceUnlock(collider); | ||
857 | d.GeomDestroy(capsule); | ||
858 | capsule = IntPtr.Zero; | ||
859 | } | ||
860 | |||
861 | if (collider != IntPtr.Zero) | ||
862 | { | ||
863 | d.SpaceDestroy(collider); | ||
864 | collider = IntPtr.Zero; | ||
865 | } | ||
866 | |||
867 | } | ||
868 | |||
869 | //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z | ||
870 | public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
871 | { | ||
872 | float sin = 2.0f * rot.Z * rot.W; | ||
873 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
874 | float tx = x; | ||
875 | |||
876 | x = tx * cos - y * sin; | ||
877 | y = tx * sin + y * cos; | ||
878 | } | ||
879 | public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
880 | { | ||
881 | float tx = x; | ||
882 | x = tx * cos - y * sin; | ||
883 | y = tx * sin + y * cos; | ||
884 | } | ||
885 | public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) | ||
886 | { | ||
887 | float tx = x; | ||
888 | x = tx * cos + y * sin; | ||
889 | y = -tx * sin + y * cos; | ||
890 | } | ||
891 | |||
892 | public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) | ||
893 | { | ||
894 | float sin = - 2.0f * rot.Z * rot.W; | ||
895 | float cos = rot.W * rot.W - rot.Z * rot.Z; | ||
896 | float tx = x; | ||
897 | |||
898 | x = tx * cos - y * sin; | ||
899 | y = tx * sin + y * cos; | ||
900 | } | ||
901 | |||
902 | public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, | ||
903 | ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) | ||
904 | { | ||
905 | feetcollision = false; | ||
906 | useAltcontact = false; | ||
907 | |||
908 | if (me == capsule) | ||
909 | { | ||
910 | Vector3 offset; | ||
911 | |||
912 | float h = contact.pos.Z - _position.Z; | ||
913 | offset.Z = h - feetOff; | ||
914 | |||
915 | offset.X = contact.pos.X - _position.X; | ||
916 | offset.Y = contact.pos.Y - _position.Y; | ||
917 | |||
918 | d.GeomClassID gtype = d.GeomGetClass(other); | ||
919 | if (gtype == d.GeomClassID.CapsuleClass) | ||
920 | { | ||
921 | Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); | ||
922 | float r = roff.X *roff.X / AvaAvaSizeXsq; | ||
923 | r += (roff.Y * roff.Y) / AvaAvaSizeYsq; | ||
924 | if (r > 1.0f) | ||
925 | return false; | ||
926 | |||
927 | float dp = 1.0f -(float)Math.Sqrt((double)r); | ||
928 | if (dp > 0.05f) | ||
929 | dp = 0.05f; | ||
930 | |||
931 | contact.depth = dp; | ||
932 | |||
933 | if (offset.Z < 0) | ||
934 | { | ||
935 | feetcollision = true; | ||
936 | if (h < boneOff) | ||
937 | { | ||
938 | m_collideNormal.X = contact.normal.X; | ||
939 | m_collideNormal.Y = contact.normal.Y; | ||
940 | m_collideNormal.Z = contact.normal.Z; | ||
941 | IsColliding = true; | ||
942 | } | ||
943 | } | ||
944 | return true; | ||
945 | } | ||
946 | |||
947 | if (gtype == d.GeomClassID.SphereClass && d.GeomGetBody(other) != IntPtr.Zero) | ||
948 | { | ||
949 | if(d.GeomSphereGetRadius(other) < 0.5) | ||
950 | return true; | ||
951 | } | ||
952 | |||
953 | if (offset.Z > 0 || contact.normal.Z > 0.35f) | ||
954 | { | ||
955 | if (offset.Z <= 0) | ||
956 | { | ||
957 | feetcollision = true; | ||
958 | if (h < boneOff) | ||
959 | { | ||
960 | m_collideNormal.X = contact.normal.X; | ||
961 | m_collideNormal.Y = contact.normal.Y; | ||
962 | m_collideNormal.Z = contact.normal.Z; | ||
963 | IsColliding = true; | ||
964 | } | ||
965 | } | ||
966 | return true; | ||
967 | } | ||
968 | |||
969 | if(m_flying) | ||
970 | return true; | ||
971 | |||
972 | feetcollision = true; | ||
973 | if (h < boneOff) | ||
974 | { | ||
975 | m_collideNormal.X = contact.normal.X; | ||
976 | m_collideNormal.Y = contact.normal.Y; | ||
977 | m_collideNormal.Z = contact.normal.Z; | ||
978 | IsColliding = true; | ||
979 | } | ||
980 | |||
981 | altContact = contact; | ||
982 | useAltcontact = true; | ||
983 | |||
984 | offset.Z -= 0.2f; | ||
985 | |||
986 | offset.Normalize(); | ||
987 | |||
988 | float tdp = contact.depth; | ||
989 | float t = offset.X; | ||
990 | t = Math.Abs(t); | ||
991 | if(t > 1e-6) | ||
992 | { | ||
993 | tdp /= t; | ||
994 | tdp *= contact.normal.X; | ||
995 | } | ||
996 | else | ||
997 | tdp *= 10; | ||
998 | |||
999 | if (tdp > 0.25f) | ||
1000 | tdp = 0.25f; | ||
1001 | |||
1002 | altContact.depth = tdp; | ||
1003 | |||
1004 | if (reverse) | ||
1005 | { | ||
1006 | altContact.normal.X = offset.X; | ||
1007 | altContact.normal.Y = offset.Y; | ||
1008 | altContact.normal.Z = offset.Z; | ||
1009 | } | ||
1010 | else | ||
1011 | { | ||
1012 | altContact.normal.X = -offset.X; | ||
1013 | altContact.normal.Y = -offset.Y; | ||
1014 | altContact.normal.Z = -offset.Z; | ||
1015 | } | ||
1016 | return true; | ||
1017 | } | ||
1018 | return false; | ||
1019 | } | ||
1020 | |||
1021 | /// <summary> | ||
1022 | /// Called from Simulate | ||
1023 | /// This is the avatar's movement control + PID Controller | ||
1024 | /// </summary> | ||
1025 | /// <param name="timeStep"></param> | ||
1026 | public void Move(List<OdeCharacter> defects) | ||
1027 | { | ||
1028 | if (Body == IntPtr.Zero) | ||
1029 | return; | ||
1030 | |||
1031 | d.Vector3 dtmp = d.BodyGetPosition(Body); | ||
1032 | Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
1033 | |||
1034 | // the Amotor still lets avatar rotation to drift during colisions | ||
1035 | // so force it back to identity | ||
1036 | |||
1037 | d.Quaternion qtmp; | ||
1038 | qtmp.W = m_orientation2D.W; | ||
1039 | qtmp.X = m_orientation2D.X; | ||
1040 | qtmp.Y = m_orientation2D.Y; | ||
1041 | qtmp.Z = m_orientation2D.Z; | ||
1042 | d.BodySetQuaternion(Body, ref qtmp); | ||
1043 | |||
1044 | if (m_pidControllerActive == false) | ||
1045 | { | ||
1046 | _zeroPosition = localpos; | ||
1047 | } | ||
1048 | |||
1049 | if (!localpos.IsFinite()) | ||
1050 | { | ||
1051 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
1052 | defects.Add(this); | ||
1053 | // _parent_scene.RemoveCharacter(this); | ||
1054 | |||
1055 | // destroy avatar capsule and related ODE data | ||
1056 | AvatarGeomAndBodyDestroy(); | ||
1057 | return; | ||
1058 | } | ||
1059 | |||
1060 | // check outbounds forcing to be in world | ||
1061 | bool fixbody = false; | ||
1062 | if (localpos.X < 0.0f) | ||
1063 | { | ||
1064 | fixbody = true; | ||
1065 | localpos.X = 0.1f; | ||
1066 | } | ||
1067 | else if (localpos.X > m_parent_scene.WorldExtents.X - 0.1f) | ||
1068 | { | ||
1069 | fixbody = true; | ||
1070 | localpos.X = m_parent_scene.WorldExtents.X - 0.1f; | ||
1071 | } | ||
1072 | if (localpos.Y < 0.0f) | ||
1073 | { | ||
1074 | fixbody = true; | ||
1075 | localpos.Y = 0.1f; | ||
1076 | } | ||
1077 | else if (localpos.Y > m_parent_scene.WorldExtents.Y - 0.1) | ||
1078 | { | ||
1079 | fixbody = true; | ||
1080 | localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f; | ||
1081 | } | ||
1082 | if (fixbody) | ||
1083 | { | ||
1084 | m_freemove = false; | ||
1085 | d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); | ||
1086 | } | ||
1087 | |||
1088 | float breakfactor; | ||
1089 | |||
1090 | Vector3 vec = Vector3.Zero; | ||
1091 | dtmp = d.BodyGetLinearVel(Body); | ||
1092 | Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
1093 | float velLengthSquared = vel.LengthSquared(); | ||
1094 | |||
1095 | Vector3 ctz = _target_velocity; | ||
1096 | |||
1097 | float movementdivisor = 1f; | ||
1098 | //Ubit change divisions into multiplications below | ||
1099 | if (!m_alwaysRun) | ||
1100 | movementdivisor = 1 / walkDivisor; | ||
1101 | else | ||
1102 | movementdivisor = 1 / runDivisor; | ||
1103 | |||
1104 | ctz.X *= movementdivisor; | ||
1105 | ctz.Y *= movementdivisor; | ||
1106 | |||
1107 | //****************************************** | ||
1108 | // colide with land | ||
1109 | |||
1110 | d.AABB aabb; | ||
1111 | // d.GeomGetAABB(feetbox, out aabb); | ||
1112 | d.GeomGetAABB(capsule, out aabb); | ||
1113 | float chrminZ = aabb.MinZ; // move up a bit | ||
1114 | Vector3 posch = localpos; | ||
1115 | |||
1116 | float ftmp; | ||
1117 | |||
1118 | if (m_flying) | ||
1119 | { | ||
1120 | ftmp = timeStep; | ||
1121 | posch.X += vel.X * ftmp; | ||
1122 | posch.Y += vel.Y * ftmp; | ||
1123 | } | ||
1124 | |||
1125 | float terrainheight = m_parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); | ||
1126 | if (chrminZ < terrainheight) | ||
1127 | { | ||
1128 | if (ctz.Z < 0) | ||
1129 | ctz.Z = 0; | ||
1130 | |||
1131 | if(!m_haveLastFallVel) | ||
1132 | { | ||
1133 | m_lastFallVel = vel; | ||
1134 | m_haveLastFallVel = true; | ||
1135 | } | ||
1136 | |||
1137 | Vector3 n = m_parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); | ||
1138 | float depth = terrainheight - chrminZ; | ||
1139 | |||
1140 | vec.Z = depth * PID_P * 50; | ||
1141 | |||
1142 | if (!m_flying) | ||
1143 | { | ||
1144 | vec.Z += -vel.Z * PID_D; | ||
1145 | if(n.Z < 0.4f) | ||
1146 | { | ||
1147 | vec.X = depth * PID_P * 50 - vel.X * PID_D; | ||
1148 | vec.X *= n.X; | ||
1149 | vec.Y = depth * PID_P * 50 - vel.Y * PID_D; | ||
1150 | vec.Y *= n.Y; | ||
1151 | vec.Z *= n.Z; | ||
1152 | if(n.Z < 0.1f) | ||
1153 | { | ||
1154 | // cancel the slope pose | ||
1155 | n.X = 0f; | ||
1156 | n.Y = 0f; | ||
1157 | n.Z = 1.0f; | ||
1158 | } | ||
1159 | } | ||
1160 | } | ||
1161 | |||
1162 | if (depth < 0.2f) | ||
1163 | { | ||
1164 | m_colliderGroundfilter++; | ||
1165 | if (m_colliderGroundfilter > 2) | ||
1166 | { | ||
1167 | m_iscolliding = true; | ||
1168 | m_colliderfilter = 2; | ||
1169 | |||
1170 | if (m_colliderGroundfilter > 10) | ||
1171 | { | ||
1172 | m_colliderGroundfilter = 10; | ||
1173 | m_freemove = false; | ||
1174 | } | ||
1175 | |||
1176 | m_collideNormal.X = n.X; | ||
1177 | m_collideNormal.Y = n.Y; | ||
1178 | m_collideNormal.Z = n.Z; | ||
1179 | |||
1180 | m_iscollidingGround = true; | ||
1181 | |||
1182 | ContactPoint contact = new ContactPoint(); | ||
1183 | contact.PenetrationDepth = depth; | ||
1184 | contact.Position.X = localpos.X; | ||
1185 | contact.Position.Y = localpos.Y; | ||
1186 | contact.Position.Z = terrainheight; | ||
1187 | contact.SurfaceNormal.X = -n.X; | ||
1188 | contact.SurfaceNormal.Y = -n.Y; | ||
1189 | contact.SurfaceNormal.Z = -n.Z; | ||
1190 | contact.RelativeSpeed = Vector3.Dot(m_lastFallVel, n); | ||
1191 | contact.CharacterFeet = true; | ||
1192 | AddCollisionEvent(0, contact); | ||
1193 | m_lastFallVel = vel; | ||
1194 | |||
1195 | // vec.Z *= 0.5f; | ||
1196 | } | ||
1197 | } | ||
1198 | |||
1199 | else | ||
1200 | { | ||
1201 | m_colliderGroundfilter -= 5; | ||
1202 | if (m_colliderGroundfilter <= 0) | ||
1203 | { | ||
1204 | m_colliderGroundfilter = 0; | ||
1205 | m_iscollidingGround = false; | ||
1206 | } | ||
1207 | } | ||
1208 | } | ||
1209 | else | ||
1210 | { | ||
1211 | m_haveLastFallVel = false; | ||
1212 | m_colliderGroundfilter -= 5; | ||
1213 | if (m_colliderGroundfilter <= 0) | ||
1214 | { | ||
1215 | m_colliderGroundfilter = 0; | ||
1216 | m_iscollidingGround = false; | ||
1217 | } | ||
1218 | } | ||
1219 | |||
1220 | bool hoverPIDActive = false; | ||
1221 | |||
1222 | if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) | ||
1223 | { | ||
1224 | hoverPIDActive = true; | ||
1225 | |||
1226 | switch (m_PIDHoverType) | ||
1227 | { | ||
1228 | case PIDHoverType.Ground: | ||
1229 | m_targetHoverHeight = terrainheight + m_PIDHoverHeight; | ||
1230 | break; | ||
1231 | |||
1232 | case PIDHoverType.GroundAndWater: | ||
1233 | float waterHeight = m_parent_scene.GetWaterLevel(); | ||
1234 | if (terrainheight > waterHeight) | ||
1235 | m_targetHoverHeight = terrainheight + m_PIDHoverHeight; | ||
1236 | else | ||
1237 | m_targetHoverHeight = waterHeight + m_PIDHoverHeight; | ||
1238 | break; | ||
1239 | } // end switch (m_PIDHoverType) | ||
1240 | |||
1241 | // don't go underground | ||
1242 | if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ)) | ||
1243 | { | ||
1244 | float fz = (m_targetHoverHeight - localpos.Z); | ||
1245 | |||
1246 | // if error is zero, use position control; otherwise, velocity control | ||
1247 | if (Math.Abs(fz) < 0.01f) | ||
1248 | { | ||
1249 | ctz.Z = 0; | ||
1250 | } | ||
1251 | else | ||
1252 | { | ||
1253 | _zeroFlag = false; | ||
1254 | fz /= m_PIDHoverTau; | ||
1255 | |||
1256 | float tmp = Math.Abs(fz); | ||
1257 | if (tmp > 50) | ||
1258 | fz = 50 * Math.Sign(fz); | ||
1259 | else if (tmp < 0.1) | ||
1260 | fz = 0.1f * Math.Sign(fz); | ||
1261 | |||
1262 | ctz.Z = fz; | ||
1263 | } | ||
1264 | } | ||
1265 | } | ||
1266 | |||
1267 | //****************************************** | ||
1268 | if (!m_iscolliding) | ||
1269 | m_collideNormal.Z = 0; | ||
1270 | |||
1271 | bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); | ||
1272 | |||
1273 | if (!tviszero) | ||
1274 | { | ||
1275 | m_freemove = false; | ||
1276 | |||
1277 | // movement relative to surface if moving on it | ||
1278 | // dont disturbe vertical movement, ie jumps | ||
1279 | if (m_iscolliding && !m_flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) | ||
1280 | { | ||
1281 | float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; | ||
1282 | ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); | ||
1283 | ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); | ||
1284 | ctz.Z -= p; | ||
1285 | if (ctz.Z < 0) | ||
1286 | ctz.Z *= 2; | ||
1287 | |||
1288 | } | ||
1289 | |||
1290 | } | ||
1291 | |||
1292 | if (!m_freemove) | ||
1293 | { | ||
1294 | |||
1295 | // if velocity is zero, use position control; otherwise, velocity control | ||
1296 | if (tviszero && m_iscolliding && !m_flying) | ||
1297 | { | ||
1298 | // keep track of where we stopped. No more slippin' & slidin' | ||
1299 | if (!_zeroFlag) | ||
1300 | { | ||
1301 | _zeroFlag = true; | ||
1302 | _zeroPosition = localpos; | ||
1303 | } | ||
1304 | if (m_pidControllerActive) | ||
1305 | { | ||
1306 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
1307 | // react to the physics scene by moving it's position. | ||
1308 | // Avatar to Avatar collisions | ||
1309 | // Prim to avatar collisions | ||
1310 | |||
1311 | vec.X = -vel.X * PID_D * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); | ||
1312 | vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); | ||
1313 | if(vel.Z > 0) | ||
1314 | vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; | ||
1315 | else | ||
1316 | vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; | ||
1317 | /* | ||
1318 | if (flying) | ||
1319 | { | ||
1320 | vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; | ||
1321 | } | ||
1322 | */ | ||
1323 | } | ||
1324 | //PidStatus = true; | ||
1325 | } | ||
1326 | else | ||
1327 | { | ||
1328 | m_pidControllerActive = true; | ||
1329 | _zeroFlag = false; | ||
1330 | |||
1331 | if (m_iscolliding) | ||
1332 | { | ||
1333 | if (!m_flying) | ||
1334 | { | ||
1335 | // we are on a surface | ||
1336 | if (ctz.Z > 0f) | ||
1337 | { | ||
1338 | // moving up or JUMPING | ||
1339 | vec.Z += (ctz.Z - vel.Z) * PID_D * 2f; | ||
1340 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1341 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1342 | } | ||
1343 | else | ||
1344 | { | ||
1345 | // we are moving down on a surface | ||
1346 | if (ctz.Z == 0) | ||
1347 | { | ||
1348 | if (vel.Z > 0) | ||
1349 | vec.Z -= vel.Z * PID_D * 2f; | ||
1350 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1351 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1352 | } | ||
1353 | // intencionally going down | ||
1354 | else | ||
1355 | { | ||
1356 | if (ctz.Z < vel.Z) | ||
1357 | vec.Z += (ctz.Z - vel.Z) * PID_D; | ||
1358 | else | ||
1359 | { | ||
1360 | } | ||
1361 | |||
1362 | if (Math.Abs(ctz.X) > Math.Abs(vel.X)) | ||
1363 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1364 | if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) | ||
1365 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1366 | } | ||
1367 | } | ||
1368 | |||
1369 | // We're standing on something | ||
1370 | } | ||
1371 | else | ||
1372 | { | ||
1373 | // We're flying and colliding with something | ||
1374 | vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); | ||
1375 | vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); | ||
1376 | vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); | ||
1377 | } | ||
1378 | } | ||
1379 | else // ie not colliding | ||
1380 | { | ||
1381 | if (m_flying || hoverPIDActive) //(!m_iscolliding && flying) | ||
1382 | { | ||
1383 | // we're in mid air suspended | ||
1384 | vec.X += (ctz.X - vel.X) * (PID_D); | ||
1385 | vec.Y += (ctz.Y - vel.Y) * (PID_D); | ||
1386 | vec.Z += (ctz.Z - vel.Z) * (PID_D); | ||
1387 | } | ||
1388 | |||
1389 | else | ||
1390 | { | ||
1391 | // we're not colliding and we're not flying so that means we're falling! | ||
1392 | // m_iscolliding includes collisions with the ground. | ||
1393 | |||
1394 | // d.Vector3 pos = d.BodyGetPosition(Body); | ||
1395 | vec.X += (ctz.X - vel.X) * PID_D * 0.833f; | ||
1396 | vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; | ||
1397 | // hack for breaking on fall | ||
1398 | if (ctz.Z == -9999f) | ||
1399 | vec.Z += -vel.Z * PID_D - m_parent_scene.gravityz * m_mass; | ||
1400 | } | ||
1401 | } | ||
1402 | } | ||
1403 | |||
1404 | if (velLengthSquared > 2500.0f) // 50m/s apply breaks | ||
1405 | { | ||
1406 | breakfactor = 0.16f * m_mass; | ||
1407 | vec.X -= breakfactor * vel.X; | ||
1408 | vec.Y -= breakfactor * vel.Y; | ||
1409 | vec.Z -= breakfactor * vel.Z; | ||
1410 | } | ||
1411 | } | ||
1412 | else | ||
1413 | { | ||
1414 | breakfactor = m_mass; | ||
1415 | vec.X -= breakfactor * vel.X; | ||
1416 | vec.Y -= breakfactor * vel.Y; | ||
1417 | if (m_flying) | ||
1418 | vec.Z -= 0.5f * breakfactor * vel.Z; | ||
1419 | else | ||
1420 | vec.Z -= .16f* m_mass * vel.Z; | ||
1421 | } | ||
1422 | |||
1423 | if (m_flying || hoverPIDActive) | ||
1424 | { | ||
1425 | vec.Z -= m_parent_scene.gravityz * m_mass; | ||
1426 | |||
1427 | if(!hoverPIDActive) | ||
1428 | { | ||
1429 | //Added for auto fly height. Kitto Flora | ||
1430 | float target_altitude = terrainheight + MinimumGroundFlightOffset; | ||
1431 | |||
1432 | if (localpos.Z < target_altitude) | ||
1433 | { | ||
1434 | vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; | ||
1435 | } | ||
1436 | // end add Kitto Flora | ||
1437 | } | ||
1438 | } | ||
1439 | |||
1440 | if (vec.IsFinite()) | ||
1441 | { | ||
1442 | if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) | ||
1443 | d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); | ||
1444 | } | ||
1445 | else | ||
1446 | { | ||
1447 | m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); | ||
1448 | m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); | ||
1449 | defects.Add(this); | ||
1450 | // _parent_scene.RemoveCharacter(this); | ||
1451 | // destroy avatar capsule and related ODE data | ||
1452 | AvatarGeomAndBodyDestroy(); | ||
1453 | return; | ||
1454 | } | ||
1455 | |||
1456 | // update our local ideia of position velocity and aceleration | ||
1457 | // _position = localpos; | ||
1458 | _position = localpos; | ||
1459 | |||
1460 | if (_zeroFlag) | ||
1461 | { | ||
1462 | _velocity = Vector3.Zero; | ||
1463 | _acceleration = Vector3.Zero; | ||
1464 | m_rotationalVelocity = Vector3.Zero; | ||
1465 | } | ||
1466 | else | ||
1467 | { | ||
1468 | Vector3 a =_velocity; // previus velocity | ||
1469 | SetSmooth(ref _velocity, ref vel, 2); | ||
1470 | a = (_velocity - a) * invtimeStep; | ||
1471 | SetSmooth(ref _acceleration, ref a, 2); | ||
1472 | |||
1473 | dtmp = d.BodyGetAngularVel(Body); | ||
1474 | m_rotationalVelocity.X = 0f; | ||
1475 | m_rotationalVelocity.Y = 0f; | ||
1476 | m_rotationalVelocity.Z = dtmp.Z; | ||
1477 | Math.Round(m_rotationalVelocity.Z,3); | ||
1478 | } | ||
1479 | } | ||
1480 | |||
1481 | public void round(ref Vector3 v, int digits) | ||
1482 | { | ||
1483 | v.X = (float)Math.Round(v.X, digits); | ||
1484 | v.Y = (float)Math.Round(v.Y, digits); | ||
1485 | v.Z = (float)Math.Round(v.Z, digits); | ||
1486 | } | ||
1487 | |||
1488 | public void SetSmooth(ref Vector3 dst, ref Vector3 value) | ||
1489 | { | ||
1490 | dst.X = 0.1f * dst.X + 0.9f * value.X; | ||
1491 | dst.Y = 0.1f * dst.Y + 0.9f * value.Y; | ||
1492 | dst.Z = 0.1f * dst.Z + 0.9f * value.Z; | ||
1493 | } | ||
1494 | |||
1495 | public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) | ||
1496 | { | ||
1497 | dst.X = 0.4f * dst.X + 0.6f * value.X; | ||
1498 | dst.X = (float)Math.Round(dst.X, rounddigits); | ||
1499 | |||
1500 | dst.Y = 0.4f * dst.Y + 0.6f * value.Y; | ||
1501 | dst.Y = (float)Math.Round(dst.Y, rounddigits); | ||
1502 | |||
1503 | dst.Z = 0.4f * dst.Z + 0.6f * value.Z; | ||
1504 | dst.Z = (float)Math.Round(dst.Z, rounddigits); | ||
1505 | } | ||
1506 | |||
1507 | |||
1508 | /// <summary> | ||
1509 | /// Updates the reported position and velocity. | ||
1510 | /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording | ||
1511 | /// also outbounds checking | ||
1512 | /// copy and outbounds now done in move(..) at ode rate | ||
1513 | /// | ||
1514 | /// </summary> | ||
1515 | public void UpdatePositionAndVelocity() | ||
1516 | { | ||
1517 | return; | ||
1518 | |||
1519 | // if (Body == IntPtr.Zero) | ||
1520 | // return; | ||
1521 | |||
1522 | } | ||
1523 | |||
1524 | /// <summary> | ||
1525 | /// Cleanup the things we use in the scene. | ||
1526 | /// </summary> | ||
1527 | public void Destroy() | ||
1528 | { | ||
1529 | AddChange(changes.Remove, null); | ||
1530 | } | ||
1531 | |||
1532 | public override void CrossingFailure() | ||
1533 | { | ||
1534 | } | ||
1535 | |||
1536 | public override Vector3 PIDTarget { set { return; } } | ||
1537 | public override bool PIDActive {get {return m_pidControllerActive;} set { return; } } | ||
1538 | public override float PIDTau { set { return; } } | ||
1539 | |||
1540 | public override float PIDHoverHeight | ||
1541 | { | ||
1542 | set | ||
1543 | { | ||
1544 | AddChange(changes.PIDHoverHeight,value); | ||
1545 | } | ||
1546 | } | ||
1547 | public override bool PIDHoverActive | ||
1548 | { | ||
1549 | get | ||
1550 | { | ||
1551 | return m_useHoverPID; | ||
1552 | } | ||
1553 | set | ||
1554 | { | ||
1555 | AddChange(changes.PIDHoverActive, value); | ||
1556 | } | ||
1557 | } | ||
1558 | |||
1559 | public override PIDHoverType PIDHoverType | ||
1560 | { | ||
1561 | set | ||
1562 | { | ||
1563 | AddChange(changes.PIDHoverType,value); | ||
1564 | } | ||
1565 | } | ||
1566 | |||
1567 | public override float PIDHoverTau | ||
1568 | { | ||
1569 | set | ||
1570 | { | ||
1571 | float tmp =0; | ||
1572 | if (value > 0) | ||
1573 | { | ||
1574 | float mint = (0.05f > timeStep ? 0.05f : timeStep); | ||
1575 | if (value < mint) | ||
1576 | tmp = mint; | ||
1577 | else | ||
1578 | tmp = value; | ||
1579 | } | ||
1580 | AddChange(changes.PIDHoverTau, tmp); | ||
1581 | } | ||
1582 | } | ||
1583 | |||
1584 | public override Quaternion APIDTarget { set { return; } } | ||
1585 | |||
1586 | public override bool APIDActive { set { return; } } | ||
1587 | |||
1588 | public override float APIDStrength { set { return; } } | ||
1589 | |||
1590 | public override float APIDDamping { set { return; } } | ||
1591 | |||
1592 | public override void SubscribeEvents(int ms) | ||
1593 | { | ||
1594 | m_eventsubscription = ms; | ||
1595 | m_cureventsubscription = 0; | ||
1596 | CollisionEventsThisFrame.Clear(); | ||
1597 | SentEmptyCollisionsEvent = false; | ||
1598 | } | ||
1599 | |||
1600 | public override void UnSubscribeEvents() | ||
1601 | { | ||
1602 | m_eventsubscription = 0; | ||
1603 | m_parent_scene.RemoveCollisionEventReporting(this); | ||
1604 | lock(CollisionEventsThisFrame) | ||
1605 | CollisionEventsThisFrame.Clear(); | ||
1606 | } | ||
1607 | |||
1608 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1609 | { | ||
1610 | lock(CollisionEventsThisFrame) | ||
1611 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1612 | m_parent_scene.AddCollisionEventReporting(this); | ||
1613 | } | ||
1614 | |||
1615 | public void SendCollisions(int timestep) | ||
1616 | { | ||
1617 | if (m_cureventsubscription < 50000) | ||
1618 | m_cureventsubscription += timestep; | ||
1619 | |||
1620 | if (m_cureventsubscription < m_eventsubscription) | ||
1621 | return; | ||
1622 | |||
1623 | lock(CollisionEventsThisFrame) | ||
1624 | { | ||
1625 | int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; | ||
1626 | |||
1627 | if (!SentEmptyCollisionsEvent || ncolisions > 0) | ||
1628 | { | ||
1629 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1630 | m_cureventsubscription = 0; | ||
1631 | |||
1632 | if (ncolisions == 0) | ||
1633 | { | ||
1634 | SentEmptyCollisionsEvent = true; | ||
1635 | // _parent_scene.RemoveCollisionEventReporting(this); | ||
1636 | } | ||
1637 | else | ||
1638 | { | ||
1639 | SentEmptyCollisionsEvent = false; | ||
1640 | CollisionEventsThisFrame.Clear(); | ||
1641 | } | ||
1642 | } | ||
1643 | } | ||
1644 | } | ||
1645 | |||
1646 | public override bool SubscribedEvents() | ||
1647 | { | ||
1648 | if (m_eventsubscription > 0) | ||
1649 | return true; | ||
1650 | return false; | ||
1651 | } | ||
1652 | |||
1653 | private void changePhysicsStatus(bool NewStatus) | ||
1654 | { | ||
1655 | if (NewStatus != m_isPhysical) | ||
1656 | { | ||
1657 | if (NewStatus) | ||
1658 | { | ||
1659 | AvatarGeomAndBodyDestroy(); | ||
1660 | |||
1661 | AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); | ||
1662 | |||
1663 | m_parent_scene.actor_name_map[collider] = (PhysicsActor)this; | ||
1664 | m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this; | ||
1665 | m_parent_scene.AddCharacter(this); | ||
1666 | } | ||
1667 | else | ||
1668 | { | ||
1669 | m_parent_scene.RemoveCollisionEventReporting(this); | ||
1670 | m_parent_scene.RemoveCharacter(this); | ||
1671 | // destroy avatar capsule and related ODE data | ||
1672 | AvatarGeomAndBodyDestroy(); | ||
1673 | } | ||
1674 | m_freemove = false; | ||
1675 | m_isPhysical = NewStatus; | ||
1676 | } | ||
1677 | } | ||
1678 | |||
1679 | private void changeAdd() | ||
1680 | { | ||
1681 | changePhysicsStatus(true); | ||
1682 | } | ||
1683 | |||
1684 | private void changeRemove() | ||
1685 | { | ||
1686 | changePhysicsStatus(false); | ||
1687 | } | ||
1688 | |||
1689 | private void changeShape(PrimitiveBaseShape arg) | ||
1690 | { | ||
1691 | } | ||
1692 | |||
1693 | private void changeAvatarSize(strAvatarSize st) | ||
1694 | { | ||
1695 | m_feetOffset = st.offset; | ||
1696 | changeSize(st.size); | ||
1697 | } | ||
1698 | |||
1699 | private void changeSize(Vector3 pSize) | ||
1700 | { | ||
1701 | if (pSize.IsFinite()) | ||
1702 | { | ||
1703 | // for now only look to Z changes since viewers also don't change X and Y | ||
1704 | if (pSize.Z != m_size.Z) | ||
1705 | { | ||
1706 | AvatarGeomAndBodyDestroy(); | ||
1707 | |||
1708 | float oldsz = m_size.Z; | ||
1709 | m_size = pSize; | ||
1710 | |||
1711 | AvatarGeomAndBodyCreation(_position.X, _position.Y, | ||
1712 | _position.Z + (m_size.Z - oldsz) * 0.5f); | ||
1713 | |||
1714 | // Velocity = Vector3.Zero; | ||
1715 | m_targetVelocity = Vector3.Zero; | ||
1716 | |||
1717 | m_parent_scene.actor_name_map[collider] = (PhysicsActor)this; | ||
1718 | m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this; | ||
1719 | } | ||
1720 | m_freemove = false; | ||
1721 | m_pidControllerActive = true; | ||
1722 | } | ||
1723 | else | ||
1724 | { | ||
1725 | m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); | ||
1726 | } | ||
1727 | } | ||
1728 | |||
1729 | private void changePosition( Vector3 newPos) | ||
1730 | { | ||
1731 | if (Body != IntPtr.Zero) | ||
1732 | d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); | ||
1733 | _position = newPos; | ||
1734 | m_freemove = false; | ||
1735 | m_pidControllerActive = true; | ||
1736 | } | ||
1737 | |||
1738 | private void changeOrientation(Quaternion newOri) | ||
1739 | { | ||
1740 | if (m_orientation != newOri) | ||
1741 | { | ||
1742 | m_orientation = newOri; // keep a copy for core use | ||
1743 | // but only use rotations around Z | ||
1744 | |||
1745 | m_orientation2D.W = newOri.W; | ||
1746 | m_orientation2D.Z = newOri.Z; | ||
1747 | |||
1748 | float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; | ||
1749 | if (t > 0) | ||
1750 | { | ||
1751 | t = 1.0f / (float)Math.Sqrt(t); | ||
1752 | m_orientation2D.W *= t; | ||
1753 | m_orientation2D.Z *= t; | ||
1754 | } | ||
1755 | else | ||
1756 | { | ||
1757 | m_orientation2D.W = 1.0f; | ||
1758 | m_orientation2D.Z = 0f; | ||
1759 | } | ||
1760 | m_orientation2D.Y = 0f; | ||
1761 | m_orientation2D.X = 0f; | ||
1762 | |||
1763 | d.Quaternion myrot = new d.Quaternion(); | ||
1764 | myrot.X = m_orientation2D.X; | ||
1765 | myrot.Y = m_orientation2D.Y; | ||
1766 | myrot.Z = m_orientation2D.Z; | ||
1767 | myrot.W = m_orientation2D.W; | ||
1768 | d.BodySetQuaternion(Body, ref myrot); | ||
1769 | } | ||
1770 | } | ||
1771 | |||
1772 | private void changeVelocity(Vector3 newVel) | ||
1773 | { | ||
1774 | _velocity = newVel; | ||
1775 | setFreeMove(); | ||
1776 | |||
1777 | if (Body != IntPtr.Zero) | ||
1778 | d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); | ||
1779 | } | ||
1780 | |||
1781 | private void changeTargetVelocity(Vector3 newVel) | ||
1782 | { | ||
1783 | m_pidControllerActive = true; | ||
1784 | m_freemove = false; | ||
1785 | _target_velocity = newVel; | ||
1786 | } | ||
1787 | |||
1788 | private void changeSetTorque(Vector3 newTorque) | ||
1789 | { | ||
1790 | } | ||
1791 | |||
1792 | private void changeAddForce(Vector3 newForce) | ||
1793 | { | ||
1794 | } | ||
1795 | |||
1796 | private void changeAddAngularForce(Vector3 arg) | ||
1797 | { | ||
1798 | } | ||
1799 | |||
1800 | private void changeAngularLock(byte arg) | ||
1801 | { | ||
1802 | } | ||
1803 | |||
1804 | private void changeFloatOnWater(bool arg) | ||
1805 | { | ||
1806 | } | ||
1807 | |||
1808 | private void changeVolumedetetion(bool arg) | ||
1809 | { | ||
1810 | } | ||
1811 | |||
1812 | private void changeSelectedStatus(bool arg) | ||
1813 | { | ||
1814 | } | ||
1815 | |||
1816 | private void changeDisable(bool arg) | ||
1817 | { | ||
1818 | } | ||
1819 | |||
1820 | private void changeBuilding(bool arg) | ||
1821 | { | ||
1822 | } | ||
1823 | |||
1824 | private void setFreeMove() | ||
1825 | { | ||
1826 | m_pidControllerActive = true; | ||
1827 | _zeroFlag = false; | ||
1828 | _target_velocity = Vector3.Zero; | ||
1829 | m_freemove = true; | ||
1830 | m_colliderfilter = -1; | ||
1831 | m_colliderObjectfilter = -1; | ||
1832 | m_colliderGroundfilter = -1; | ||
1833 | |||
1834 | m_iscolliding = false; | ||
1835 | m_iscollidingGround = false; | ||
1836 | m_iscollidingObj = false; | ||
1837 | |||
1838 | CollisionEventsThisFrame.Clear(); | ||
1839 | } | ||
1840 | |||
1841 | private void changeForce(Vector3 newForce) | ||
1842 | { | ||
1843 | setFreeMove(); | ||
1844 | |||
1845 | if (Body != IntPtr.Zero) | ||
1846 | { | ||
1847 | if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) | ||
1848 | d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); | ||
1849 | } | ||
1850 | } | ||
1851 | |||
1852 | // for now momentum is actually velocity | ||
1853 | private void changeMomentum(Vector3 newmomentum) | ||
1854 | { | ||
1855 | _velocity = newmomentum; | ||
1856 | setFreeMove(); | ||
1857 | |||
1858 | if (Body != IntPtr.Zero) | ||
1859 | d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); | ||
1860 | } | ||
1861 | |||
1862 | private void changePIDHoverHeight(float val) | ||
1863 | { | ||
1864 | m_PIDHoverHeight = val; | ||
1865 | if (val == 0) | ||
1866 | m_useHoverPID = false; | ||
1867 | } | ||
1868 | |||
1869 | private void changePIDHoverType(PIDHoverType type) | ||
1870 | { | ||
1871 | m_PIDHoverType = type; | ||
1872 | } | ||
1873 | |||
1874 | private void changePIDHoverTau(float tau) | ||
1875 | { | ||
1876 | m_PIDHoverTau = tau; | ||
1877 | } | ||
1878 | |||
1879 | private void changePIDHoverActive(bool active) | ||
1880 | { | ||
1881 | m_useHoverPID = active; | ||
1882 | } | ||
1883 | |||
1884 | private void donullchange() | ||
1885 | { | ||
1886 | } | ||
1887 | |||
1888 | public bool DoAChange(changes what, object arg) | ||
1889 | { | ||
1890 | if (collider == IntPtr.Zero && what != changes.Add && what != changes.Remove) | ||
1891 | { | ||
1892 | return false; | ||
1893 | } | ||
1894 | |||
1895 | // nasty switch | ||
1896 | switch (what) | ||
1897 | { | ||
1898 | case changes.Add: | ||
1899 | changeAdd(); | ||
1900 | break; | ||
1901 | case changes.Remove: | ||
1902 | changeRemove(); | ||
1903 | break; | ||
1904 | |||
1905 | case changes.Position: | ||
1906 | changePosition((Vector3)arg); | ||
1907 | break; | ||
1908 | |||
1909 | case changes.Orientation: | ||
1910 | changeOrientation((Quaternion)arg); | ||
1911 | break; | ||
1912 | |||
1913 | case changes.PosOffset: | ||
1914 | donullchange(); | ||
1915 | break; | ||
1916 | |||
1917 | case changes.OriOffset: | ||
1918 | donullchange(); | ||
1919 | break; | ||
1920 | |||
1921 | case changes.Velocity: | ||
1922 | changeVelocity((Vector3)arg); | ||
1923 | break; | ||
1924 | |||
1925 | case changes.TargetVelocity: | ||
1926 | changeTargetVelocity((Vector3)arg); | ||
1927 | break; | ||
1928 | |||
1929 | // case changes.Acceleration: | ||
1930 | // changeacceleration((Vector3)arg); | ||
1931 | // break; | ||
1932 | // case changes.AngVelocity: | ||
1933 | // changeangvelocity((Vector3)arg); | ||
1934 | // break; | ||
1935 | |||
1936 | case changes.Force: | ||
1937 | changeForce((Vector3)arg); | ||
1938 | break; | ||
1939 | |||
1940 | case changes.Torque: | ||
1941 | changeSetTorque((Vector3)arg); | ||
1942 | break; | ||
1943 | |||
1944 | case changes.AddForce: | ||
1945 | changeAddForce((Vector3)arg); | ||
1946 | break; | ||
1947 | |||
1948 | case changes.AddAngForce: | ||
1949 | changeAddAngularForce((Vector3)arg); | ||
1950 | break; | ||
1951 | |||
1952 | case changes.AngLock: | ||
1953 | changeAngularLock((byte)arg); | ||
1954 | break; | ||
1955 | |||
1956 | case changes.Size: | ||
1957 | changeSize((Vector3)arg); | ||
1958 | break; | ||
1959 | |||
1960 | case changes.AvatarSize: | ||
1961 | changeAvatarSize((strAvatarSize)arg); | ||
1962 | break; | ||
1963 | |||
1964 | case changes.Momentum: | ||
1965 | changeMomentum((Vector3)arg); | ||
1966 | break; | ||
1967 | |||
1968 | case changes.PIDHoverHeight: | ||
1969 | changePIDHoverHeight((float)arg); | ||
1970 | break; | ||
1971 | |||
1972 | case changes.PIDHoverType: | ||
1973 | changePIDHoverType((PIDHoverType)arg); | ||
1974 | break; | ||
1975 | |||
1976 | case changes.PIDHoverTau: | ||
1977 | changePIDHoverTau((float)arg); | ||
1978 | break; | ||
1979 | |||
1980 | case changes.PIDHoverActive: | ||
1981 | changePIDHoverActive((bool)arg); | ||
1982 | break; | ||
1983 | |||
1984 | /* not in use for now | ||
1985 | case changes.Shape: | ||
1986 | changeShape((PrimitiveBaseShape)arg); | ||
1987 | break; | ||
1988 | |||
1989 | case changes.CollidesWater: | ||
1990 | changeFloatOnWater((bool)arg); | ||
1991 | break; | ||
1992 | |||
1993 | case changes.VolumeDtc: | ||
1994 | changeVolumedetetion((bool)arg); | ||
1995 | break; | ||
1996 | |||
1997 | case changes.Physical: | ||
1998 | changePhysicsStatus((bool)arg); | ||
1999 | break; | ||
2000 | |||
2001 | case changes.Selected: | ||
2002 | changeSelectedStatus((bool)arg); | ||
2003 | break; | ||
2004 | |||
2005 | case changes.disabled: | ||
2006 | changeDisable((bool)arg); | ||
2007 | break; | ||
2008 | |||
2009 | case changes.building: | ||
2010 | changeBuilding((bool)arg); | ||
2011 | break; | ||
2012 | */ | ||
2013 | case changes.Null: | ||
2014 | donullchange(); | ||
2015 | break; | ||
2016 | |||
2017 | default: | ||
2018 | donullchange(); | ||
2019 | break; | ||
2020 | } | ||
2021 | return false; | ||
2022 | } | ||
2023 | |||
2024 | public void AddChange(changes what, object arg) | ||
2025 | { | ||
2026 | m_parent_scene.AddChange((PhysicsActor)this, what, arg); | ||
2027 | } | ||
2028 | |||
2029 | private struct strAvatarSize | ||
2030 | { | ||
2031 | public Vector3 size; | ||
2032 | public float offset; | ||
2033 | } | ||
2034 | |||
2035 | } | ||
2036 | } | ||