diff options
author | Robert Adams | 2015-09-08 06:15:46 -0700 |
---|---|---|
committer | Robert Adams | 2015-09-08 06:15:46 -0700 |
commit | 4dd17c4117ea413fb0c4418511956cb3abfe258c (patch) | |
tree | 6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs | |
parent | Merge of ubitworkvarnew with opensim/master as of 20150905. (diff) | |
download | opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2 opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz |
More 'everything is a module' merging.
Have most of UbitOde converted.
There are compile errors in OpenSimBase as the new modules stuff is not all there.
Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to 'OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs | 3901 |
1 files changed, 3901 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs new file mode 100644 index 0000000..b52a242 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODEPrim.cs | |||
@@ -0,0 +1,3901 @@ | |||
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 | /* Revision 2011/12/13 by Ubit Umarov | ||
29 | * | ||
30 | * | ||
31 | */ | ||
32 | |||
33 | /* | ||
34 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
35 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
36 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
37 | * characteristics and Kinetic motion. | ||
38 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
39 | * (dynamics) and the associated settings. Old Linear and angular | ||
40 | * motors for dynamic motion have been replace with MoveLinear() | ||
41 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
42 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
43 | * switch between 'VEHICLE' parameter use and general dynamics | ||
44 | * settings use. | ||
45 | */ | ||
46 | |||
47 | //#define SPAM | ||
48 | |||
49 | using System; | ||
50 | using System.Collections.Generic; | ||
51 | using System.Reflection; | ||
52 | using System.Runtime.InteropServices; | ||
53 | using System.Threading; | ||
54 | using log4net; | ||
55 | using OpenMetaverse; | ||
56 | using OdeAPI; | ||
57 | using OpenSim.Framework; | ||
58 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
59 | |||
60 | namespace OpenSim.Region.PhysicsModules.OdePlugin | ||
61 | { | ||
62 | public class OdePrim : PhysicsActor | ||
63 | { | ||
64 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
65 | |||
66 | private bool m_isphysical; | ||
67 | private bool m_fakeisphysical; | ||
68 | private bool m_isphantom; | ||
69 | private bool m_fakeisphantom; | ||
70 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
71 | private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
72 | |||
73 | protected bool m_building; | ||
74 | protected bool m_forcePosOrRotation; | ||
75 | private bool m_iscolliding; | ||
76 | |||
77 | internal bool m_isSelected; | ||
78 | private bool m_delaySelect; | ||
79 | private bool m_lastdoneSelected; | ||
80 | internal bool m_outbounds; | ||
81 | |||
82 | private Quaternion m_lastorientation; | ||
83 | private Quaternion _orientation; | ||
84 | |||
85 | private Vector3 _position; | ||
86 | private Vector3 _velocity; | ||
87 | private Vector3 m_torque; | ||
88 | private Vector3 m_lastVelocity; | ||
89 | private Vector3 m_lastposition; | ||
90 | private Vector3 m_rotationalVelocity; | ||
91 | private Vector3 _size; | ||
92 | private Vector3 _acceleration; | ||
93 | private Vector3 m_angularlock = Vector3.One; | ||
94 | private IntPtr Amotor; | ||
95 | |||
96 | private Vector3 m_force; | ||
97 | private Vector3 m_forceacc; | ||
98 | private Vector3 m_angularForceacc; | ||
99 | |||
100 | private float m_invTimeStep; | ||
101 | private float m_timeStep; | ||
102 | |||
103 | private Vector3 m_PIDTarget; | ||
104 | private float m_PIDTau; | ||
105 | private bool m_usePID; | ||
106 | |||
107 | private float m_PIDHoverHeight; | ||
108 | private float m_PIDHoverTau; | ||
109 | private bool m_useHoverPID; | ||
110 | private PIDHoverType m_PIDHoverType; | ||
111 | private float m_targetHoverHeight; | ||
112 | private float m_groundHeight; | ||
113 | private float m_waterHeight; | ||
114 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | ||
115 | |||
116 | private int body_autodisable_frames; | ||
117 | public int bodydisablecontrol; | ||
118 | private float m_gravmod = 1.0f; | ||
119 | |||
120 | // Default we're a Geometry | ||
121 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | ||
122 | // Default colide nonphysical don't try to colide with anything | ||
123 | private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; | ||
124 | |||
125 | private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | | ||
126 | CollisionCategories.Character | | ||
127 | CollisionCategories.Land | | ||
128 | CollisionCategories.VolumeDtc); | ||
129 | |||
130 | // private bool m_collidesLand = true; | ||
131 | private bool m_collidesWater; | ||
132 | // public bool m_returnCollisions; | ||
133 | |||
134 | private bool m_NoColide; // for now only for internal use for bad meshs | ||
135 | |||
136 | |||
137 | // Default, Collide with Other Geometries, spaces and Bodies | ||
138 | private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; | ||
139 | |||
140 | public bool m_disabled; | ||
141 | |||
142 | private uint m_localID; | ||
143 | |||
144 | private IMesh m_mesh; | ||
145 | private object m_meshlock = new object(); | ||
146 | private PrimitiveBaseShape _pbs; | ||
147 | |||
148 | private UUID? m_assetID; | ||
149 | private MeshState m_meshState; | ||
150 | |||
151 | public OdeScene _parent_scene; | ||
152 | |||
153 | /// <summary> | ||
154 | /// The physics space which contains prim geometry | ||
155 | /// </summary> | ||
156 | public IntPtr m_targetSpace; | ||
157 | |||
158 | public IntPtr prim_geom; | ||
159 | public IntPtr _triMeshData; | ||
160 | |||
161 | private PhysicsActor _parent; | ||
162 | |||
163 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | ||
164 | |||
165 | public float m_collisionscore; | ||
166 | private int m_colliderfilter = 0; | ||
167 | |||
168 | public IntPtr collide_geom; // for objects: geom if single prim space it linkset | ||
169 | |||
170 | private float m_density; | ||
171 | private byte m_shapetype; | ||
172 | public bool _zeroFlag; | ||
173 | private bool m_lastUpdateSent; | ||
174 | |||
175 | public IntPtr Body; | ||
176 | |||
177 | private Vector3 _target_velocity; | ||
178 | |||
179 | public Vector3 m_OBBOffset; | ||
180 | public Vector3 m_OBB; | ||
181 | public float primOOBradiusSQ; | ||
182 | |||
183 | private bool m_hasOBB = true; | ||
184 | |||
185 | private float m_physCost; | ||
186 | private float m_streamCost; | ||
187 | |||
188 | public d.Mass primdMass; // prim inertia information on it's own referencial | ||
189 | float primMass; // prim own mass | ||
190 | float primVolume; // prim own volume; | ||
191 | float _mass; // object mass acording to case | ||
192 | |||
193 | public int givefakepos; | ||
194 | private Vector3 fakepos; | ||
195 | public int givefakeori; | ||
196 | private Quaternion fakeori; | ||
197 | |||
198 | private int m_eventsubscription; | ||
199 | private int m_cureventsubscription; | ||
200 | private CollisionEventUpdate CollisionEventsThisFrame = null; | ||
201 | private bool SentEmptyCollisionsEvent; | ||
202 | |||
203 | public volatile bool childPrim; | ||
204 | |||
205 | public ODEDynamics m_vehicle; | ||
206 | |||
207 | internal int m_material = (int)Material.Wood; | ||
208 | private float mu; | ||
209 | private float bounce; | ||
210 | |||
211 | /// <summary> | ||
212 | /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. | ||
213 | /// </summary> | ||
214 | public override bool IsPhysical // this is not reliable for internal use | ||
215 | { | ||
216 | get { return m_fakeisphysical; } | ||
217 | set | ||
218 | { | ||
219 | m_fakeisphysical = value; // we show imediatly to outside that we changed physical | ||
220 | // and also to stop imediatly some updates | ||
221 | // but real change will only happen in taintprocessing | ||
222 | |||
223 | if (!value) // Zero the remembered last velocity | ||
224 | m_lastVelocity = Vector3.Zero; | ||
225 | AddChange(changes.Physical, value); | ||
226 | } | ||
227 | } | ||
228 | |||
229 | public override bool IsVolumeDtc | ||
230 | { | ||
231 | get { return m_fakeisVolumeDetect; } | ||
232 | set | ||
233 | { | ||
234 | m_fakeisVolumeDetect = value; | ||
235 | AddChange(changes.VolumeDtc, value); | ||
236 | } | ||
237 | } | ||
238 | |||
239 | public override bool Phantom // this is not reliable for internal use | ||
240 | { | ||
241 | get { return m_fakeisphantom; } | ||
242 | set | ||
243 | { | ||
244 | m_fakeisphantom = value; | ||
245 | AddChange(changes.Phantom, value); | ||
246 | } | ||
247 | } | ||
248 | |||
249 | public override bool Building // this is not reliable for internal use | ||
250 | { | ||
251 | get { return m_building; } | ||
252 | set | ||
253 | { | ||
254 | // if (value) | ||
255 | // m_building = true; | ||
256 | AddChange(changes.building, value); | ||
257 | } | ||
258 | } | ||
259 | |||
260 | public override void getContactData(ref ContactData cdata) | ||
261 | { | ||
262 | cdata.mu = mu; | ||
263 | cdata.bounce = bounce; | ||
264 | |||
265 | // cdata.softcolide = m_softcolide; | ||
266 | cdata.softcolide = false; | ||
267 | |||
268 | if (m_isphysical) | ||
269 | { | ||
270 | ODEDynamics veh; | ||
271 | if (_parent != null) | ||
272 | veh = ((OdePrim)_parent).m_vehicle; | ||
273 | else | ||
274 | veh = m_vehicle; | ||
275 | |||
276 | if (veh != null && veh.Type != Vehicle.TYPE_NONE) | ||
277 | cdata.mu *= veh.FrictionFactor; | ||
278 | // cdata.mu *= 0; | ||
279 | } | ||
280 | } | ||
281 | |||
282 | public override float PhysicsCost | ||
283 | { | ||
284 | get | ||
285 | { | ||
286 | return m_physCost; | ||
287 | } | ||
288 | } | ||
289 | |||
290 | public override float StreamCost | ||
291 | { | ||
292 | get | ||
293 | { | ||
294 | return m_streamCost; | ||
295 | } | ||
296 | } | ||
297 | |||
298 | public override int PhysicsActorType | ||
299 | { | ||
300 | get { return (int)ActorTypes.Prim; } | ||
301 | set { return; } | ||
302 | } | ||
303 | |||
304 | public override bool SetAlwaysRun | ||
305 | { | ||
306 | get { return false; } | ||
307 | set { return; } | ||
308 | } | ||
309 | |||
310 | public override uint LocalID | ||
311 | { | ||
312 | get { return m_localID; } | ||
313 | set { m_localID = value; } | ||
314 | } | ||
315 | |||
316 | public override PhysicsActor ParentActor | ||
317 | { | ||
318 | get | ||
319 | { | ||
320 | if (childPrim) | ||
321 | return _parent; | ||
322 | else | ||
323 | return (PhysicsActor)this; | ||
324 | } | ||
325 | } | ||
326 | |||
327 | public override bool Grabbed | ||
328 | { | ||
329 | set { return; } | ||
330 | } | ||
331 | |||
332 | public override bool Selected | ||
333 | { | ||
334 | set | ||
335 | { | ||
336 | if (value) | ||
337 | m_isSelected = value; // if true set imediatly to stop moves etc | ||
338 | AddChange(changes.Selected, value); | ||
339 | } | ||
340 | } | ||
341 | |||
342 | public override bool Flying | ||
343 | { | ||
344 | // no flying prims for you | ||
345 | get { return false; } | ||
346 | set { } | ||
347 | } | ||
348 | |||
349 | public override bool IsColliding | ||
350 | { | ||
351 | get { return m_iscolliding; } | ||
352 | set | ||
353 | { | ||
354 | if (value) | ||
355 | { | ||
356 | m_colliderfilter += 2; | ||
357 | if (m_colliderfilter > 2) | ||
358 | m_colliderfilter = 2; | ||
359 | } | ||
360 | else | ||
361 | { | ||
362 | m_colliderfilter--; | ||
363 | if (m_colliderfilter < 0) | ||
364 | m_colliderfilter = 0; | ||
365 | } | ||
366 | |||
367 | if (m_colliderfilter == 0) | ||
368 | m_iscolliding = false; | ||
369 | else | ||
370 | m_iscolliding = true; | ||
371 | } | ||
372 | } | ||
373 | |||
374 | public override bool CollidingGround | ||
375 | { | ||
376 | get { return false; } | ||
377 | set { return; } | ||
378 | } | ||
379 | |||
380 | public override bool CollidingObj | ||
381 | { | ||
382 | get { return false; } | ||
383 | set { return; } | ||
384 | } | ||
385 | |||
386 | |||
387 | public override bool ThrottleUpdates {get;set;} | ||
388 | |||
389 | public override bool Stopped | ||
390 | { | ||
391 | get { return _zeroFlag; } | ||
392 | } | ||
393 | |||
394 | public override Vector3 Position | ||
395 | { | ||
396 | get | ||
397 | { | ||
398 | if (givefakepos > 0) | ||
399 | return fakepos; | ||
400 | else | ||
401 | return _position; | ||
402 | } | ||
403 | |||
404 | set | ||
405 | { | ||
406 | fakepos = value; | ||
407 | givefakepos++; | ||
408 | AddChange(changes.Position, value); | ||
409 | } | ||
410 | } | ||
411 | |||
412 | public override Vector3 Size | ||
413 | { | ||
414 | get { return _size; } | ||
415 | set | ||
416 | { | ||
417 | if (value.IsFinite()) | ||
418 | { | ||
419 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype); | ||
420 | } | ||
421 | else | ||
422 | { | ||
423 | m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); | ||
424 | } | ||
425 | } | ||
426 | } | ||
427 | |||
428 | public override float Mass | ||
429 | { | ||
430 | get { return primMass; } | ||
431 | } | ||
432 | |||
433 | public override Vector3 Force | ||
434 | { | ||
435 | get { return m_force; } | ||
436 | set | ||
437 | { | ||
438 | if (value.IsFinite()) | ||
439 | { | ||
440 | AddChange(changes.Force, value); | ||
441 | } | ||
442 | else | ||
443 | { | ||
444 | m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); | ||
445 | } | ||
446 | } | ||
447 | } | ||
448 | |||
449 | public override void SetVolumeDetect(int param) | ||
450 | { | ||
451 | m_fakeisVolumeDetect = (param != 0); | ||
452 | AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); | ||
453 | } | ||
454 | |||
455 | public override Vector3 GeometricCenter | ||
456 | { | ||
457 | // this is not real geometric center but a average of positions relative to root prim acording to | ||
458 | // http://wiki.secondlife.com/wiki/llGetGeometricCenter | ||
459 | // ignoring tortured prims details since sl also seems to ignore | ||
460 | // so no real use in doing it on physics | ||
461 | get | ||
462 | { | ||
463 | return Vector3.Zero; | ||
464 | } | ||
465 | } | ||
466 | |||
467 | public override Vector3 CenterOfMass | ||
468 | { | ||
469 | get | ||
470 | { | ||
471 | lock (_parent_scene.OdeLock) | ||
472 | { | ||
473 | d.Vector3 dtmp; | ||
474 | if (!childPrim && Body != IntPtr.Zero) | ||
475 | { | ||
476 | dtmp = d.BodyGetPosition(Body); | ||
477 | return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); | ||
478 | } | ||
479 | else if (prim_geom != IntPtr.Zero) | ||
480 | { | ||
481 | d.Quaternion dq; | ||
482 | d.GeomCopyQuaternion(prim_geom, out dq); | ||
483 | Quaternion q; | ||
484 | q.X = dq.X; | ||
485 | q.Y = dq.Y; | ||
486 | q.Z = dq.Z; | ||
487 | q.W = dq.W; | ||
488 | |||
489 | Vector3 Ptot = m_OBBOffset * q; | ||
490 | dtmp = d.GeomGetPosition(prim_geom); | ||
491 | Ptot.X += dtmp.X; | ||
492 | Ptot.Y += dtmp.Y; | ||
493 | Ptot.Z += dtmp.Z; | ||
494 | |||
495 | // if(childPrim) we only know about physical linksets | ||
496 | return Ptot; | ||
497 | /* | ||
498 | float tmass = _mass; | ||
499 | Ptot *= tmass; | ||
500 | |||
501 | float m; | ||
502 | |||
503 | foreach (OdePrim prm in childrenPrim) | ||
504 | { | ||
505 | m = prm._mass; | ||
506 | Ptot += prm.CenterOfMass * m; | ||
507 | tmass += m; | ||
508 | } | ||
509 | |||
510 | if (tmass == 0) | ||
511 | tmass = 0; | ||
512 | else | ||
513 | tmass = 1.0f / tmass; | ||
514 | |||
515 | Ptot *= tmass; | ||
516 | return Ptot; | ||
517 | */ | ||
518 | } | ||
519 | else | ||
520 | return _position; | ||
521 | } | ||
522 | } | ||
523 | } | ||
524 | |||
525 | public override Vector3 OOBsize | ||
526 | { | ||
527 | get | ||
528 | { | ||
529 | return m_OBB; | ||
530 | } | ||
531 | } | ||
532 | |||
533 | public override Vector3 OOBoffset | ||
534 | { | ||
535 | get | ||
536 | { | ||
537 | return m_OBBOffset; | ||
538 | } | ||
539 | } | ||
540 | |||
541 | public override float OOBRadiusSQ | ||
542 | { | ||
543 | get | ||
544 | { | ||
545 | return primOOBradiusSQ; | ||
546 | } | ||
547 | } | ||
548 | |||
549 | public override PrimitiveBaseShape Shape | ||
550 | { | ||
551 | set | ||
552 | { | ||
553 | // AddChange(changes.Shape, value); | ||
554 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype); | ||
555 | } | ||
556 | } | ||
557 | |||
558 | public override byte PhysicsShapeType | ||
559 | { | ||
560 | get | ||
561 | { | ||
562 | return m_shapetype; | ||
563 | } | ||
564 | set | ||
565 | { | ||
566 | m_shapetype = value; | ||
567 | _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); | ||
568 | } | ||
569 | } | ||
570 | |||
571 | public override Vector3 Velocity | ||
572 | { | ||
573 | get | ||
574 | { | ||
575 | if (_zeroFlag) | ||
576 | return Vector3.Zero; | ||
577 | return _velocity; | ||
578 | } | ||
579 | set | ||
580 | { | ||
581 | if (value.IsFinite()) | ||
582 | { | ||
583 | AddChange(changes.Velocity, value); | ||
584 | } | ||
585 | else | ||
586 | { | ||
587 | m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); | ||
588 | } | ||
589 | |||
590 | } | ||
591 | } | ||
592 | |||
593 | public override Vector3 Torque | ||
594 | { | ||
595 | get | ||
596 | { | ||
597 | if (!IsPhysical || Body == IntPtr.Zero) | ||
598 | return Vector3.Zero; | ||
599 | |||
600 | return m_torque; | ||
601 | } | ||
602 | |||
603 | set | ||
604 | { | ||
605 | if (value.IsFinite()) | ||
606 | { | ||
607 | AddChange(changes.Torque, value); | ||
608 | } | ||
609 | else | ||
610 | { | ||
611 | m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); | ||
612 | } | ||
613 | } | ||
614 | } | ||
615 | |||
616 | public override float CollisionScore | ||
617 | { | ||
618 | get { return m_collisionscore; } | ||
619 | set { m_collisionscore = value; } | ||
620 | } | ||
621 | |||
622 | public override bool Kinematic | ||
623 | { | ||
624 | get { return false; } | ||
625 | set { } | ||
626 | } | ||
627 | |||
628 | public override Quaternion Orientation | ||
629 | { | ||
630 | get | ||
631 | { | ||
632 | if (givefakeori > 0) | ||
633 | return fakeori; | ||
634 | else | ||
635 | |||
636 | return _orientation; | ||
637 | } | ||
638 | set | ||
639 | { | ||
640 | if (QuaternionIsFinite(value)) | ||
641 | { | ||
642 | fakeori = value; | ||
643 | givefakeori++; | ||
644 | |||
645 | value.Normalize(); | ||
646 | |||
647 | AddChange(changes.Orientation, value); | ||
648 | } | ||
649 | else | ||
650 | m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); | ||
651 | |||
652 | } | ||
653 | } | ||
654 | |||
655 | public override Vector3 Acceleration | ||
656 | { | ||
657 | get { return _acceleration; } | ||
658 | set { } | ||
659 | } | ||
660 | |||
661 | public override Vector3 RotationalVelocity | ||
662 | { | ||
663 | get | ||
664 | { | ||
665 | Vector3 pv = Vector3.Zero; | ||
666 | if (_zeroFlag) | ||
667 | return pv; | ||
668 | |||
669 | if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) | ||
670 | return pv; | ||
671 | |||
672 | return m_rotationalVelocity; | ||
673 | } | ||
674 | set | ||
675 | { | ||
676 | if (value.IsFinite()) | ||
677 | { | ||
678 | AddChange(changes.AngVelocity, value); | ||
679 | } | ||
680 | else | ||
681 | { | ||
682 | m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); | ||
683 | } | ||
684 | } | ||
685 | } | ||
686 | |||
687 | public override float Buoyancy | ||
688 | { | ||
689 | get { return m_buoyancy; } | ||
690 | set | ||
691 | { | ||
692 | AddChange(changes.Buoyancy,value); | ||
693 | } | ||
694 | } | ||
695 | |||
696 | public override bool FloatOnWater | ||
697 | { | ||
698 | set | ||
699 | { | ||
700 | AddChange(changes.CollidesWater, value); | ||
701 | } | ||
702 | } | ||
703 | |||
704 | public override Vector3 PIDTarget | ||
705 | { | ||
706 | set | ||
707 | { | ||
708 | if (value.IsFinite()) | ||
709 | { | ||
710 | AddChange(changes.PIDTarget,value); | ||
711 | } | ||
712 | else | ||
713 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | ||
714 | } | ||
715 | } | ||
716 | |||
717 | public override bool PIDActive | ||
718 | { | ||
719 | get | ||
720 | { | ||
721 | return m_usePID; | ||
722 | } | ||
723 | set | ||
724 | { | ||
725 | AddChange(changes.PIDActive,value); | ||
726 | } | ||
727 | } | ||
728 | |||
729 | public override float PIDTau | ||
730 | { | ||
731 | set | ||
732 | { | ||
733 | float tmp = 0; | ||
734 | if (value > 0) | ||
735 | { | ||
736 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); | ||
737 | if (value < mint) | ||
738 | tmp = mint; | ||
739 | else | ||
740 | tmp = value; | ||
741 | } | ||
742 | AddChange(changes.PIDTau,tmp); | ||
743 | } | ||
744 | } | ||
745 | |||
746 | public override float PIDHoverHeight | ||
747 | { | ||
748 | set | ||
749 | { | ||
750 | AddChange(changes.PIDHoverHeight,value); | ||
751 | } | ||
752 | } | ||
753 | public override bool PIDHoverActive | ||
754 | { | ||
755 | set | ||
756 | { | ||
757 | AddChange(changes.PIDHoverActive, value); | ||
758 | } | ||
759 | } | ||
760 | |||
761 | public override PIDHoverType PIDHoverType | ||
762 | { | ||
763 | set | ||
764 | { | ||
765 | AddChange(changes.PIDHoverType,value); | ||
766 | } | ||
767 | } | ||
768 | |||
769 | public override float PIDHoverTau | ||
770 | { | ||
771 | set | ||
772 | { | ||
773 | float tmp =0; | ||
774 | if (value > 0) | ||
775 | { | ||
776 | float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); | ||
777 | if (value < mint) | ||
778 | tmp = mint; | ||
779 | else | ||
780 | tmp = value; | ||
781 | } | ||
782 | AddChange(changes.PIDHoverTau, tmp); | ||
783 | } | ||
784 | } | ||
785 | |||
786 | public override Quaternion APIDTarget { set { return; } } | ||
787 | |||
788 | public override bool APIDActive { set { return; } } | ||
789 | |||
790 | public override float APIDStrength { set { return; } } | ||
791 | |||
792 | public override float APIDDamping { set { return; } } | ||
793 | |||
794 | public override int VehicleType | ||
795 | { | ||
796 | // we may need to put a fake on this | ||
797 | get | ||
798 | { | ||
799 | if (m_vehicle == null) | ||
800 | return (int)Vehicle.TYPE_NONE; | ||
801 | else | ||
802 | return (int)m_vehicle.Type; | ||
803 | } | ||
804 | set | ||
805 | { | ||
806 | AddChange(changes.VehicleType, value); | ||
807 | } | ||
808 | } | ||
809 | |||
810 | public override void VehicleFloatParam(int param, float value) | ||
811 | { | ||
812 | strVehicleFloatParam fp = new strVehicleFloatParam(); | ||
813 | fp.param = param; | ||
814 | fp.value = value; | ||
815 | AddChange(changes.VehicleFloatParam, fp); | ||
816 | } | ||
817 | |||
818 | public override void VehicleVectorParam(int param, Vector3 value) | ||
819 | { | ||
820 | strVehicleVectorParam fp = new strVehicleVectorParam(); | ||
821 | fp.param = param; | ||
822 | fp.value = value; | ||
823 | AddChange(changes.VehicleVectorParam, fp); | ||
824 | } | ||
825 | |||
826 | public override void VehicleRotationParam(int param, Quaternion value) | ||
827 | { | ||
828 | strVehicleQuatParam fp = new strVehicleQuatParam(); | ||
829 | fp.param = param; | ||
830 | fp.value = value; | ||
831 | AddChange(changes.VehicleRotationParam, fp); | ||
832 | } | ||
833 | |||
834 | public override void VehicleFlags(int param, bool value) | ||
835 | { | ||
836 | strVehicleBoolParam bp = new strVehicleBoolParam(); | ||
837 | bp.param = param; | ||
838 | bp.value = value; | ||
839 | AddChange(changes.VehicleFlags, bp); | ||
840 | } | ||
841 | |||
842 | public override void SetVehicle(object vdata) | ||
843 | { | ||
844 | AddChange(changes.SetVehicle, vdata); | ||
845 | } | ||
846 | public void SetAcceleration(Vector3 accel) | ||
847 | { | ||
848 | _acceleration = accel; | ||
849 | } | ||
850 | |||
851 | public override void AddForce(Vector3 force, bool pushforce) | ||
852 | { | ||
853 | if (force.IsFinite()) | ||
854 | { | ||
855 | if(pushforce) | ||
856 | AddChange(changes.AddForce, force); | ||
857 | else // a impulse | ||
858 | AddChange(changes.AddForce, force * m_invTimeStep); | ||
859 | } | ||
860 | else | ||
861 | { | ||
862 | m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); | ||
863 | } | ||
864 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
865 | } | ||
866 | |||
867 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
868 | { | ||
869 | if (force.IsFinite()) | ||
870 | { | ||
871 | // if(pushforce) for now applyrotationimpulse seems more happy applied as a force | ||
872 | AddChange(changes.AddAngForce, force); | ||
873 | // else // a impulse | ||
874 | // AddChange(changes.AddAngForce, force * m_invTimeStep); | ||
875 | } | ||
876 | else | ||
877 | { | ||
878 | m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); | ||
879 | } | ||
880 | } | ||
881 | |||
882 | public override void CrossingFailure() | ||
883 | { | ||
884 | if (m_outbounds) | ||
885 | { | ||
886 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
887 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
888 | _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); | ||
889 | |||
890 | m_lastposition = _position; | ||
891 | _velocity.X = 0; | ||
892 | _velocity.Y = 0; | ||
893 | _velocity.Z = 0; | ||
894 | |||
895 | m_lastVelocity = _velocity; | ||
896 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
897 | m_vehicle.Stop(); | ||
898 | |||
899 | if(Body != IntPtr.Zero) | ||
900 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
901 | if (prim_geom != IntPtr.Zero) | ||
902 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
903 | |||
904 | m_outbounds = false; | ||
905 | changeDisable(false); | ||
906 | base.RequestPhysicsterseUpdate(); | ||
907 | } | ||
908 | } | ||
909 | |||
910 | public override void SetMomentum(Vector3 momentum) | ||
911 | { | ||
912 | } | ||
913 | |||
914 | public override void SetMaterial(int pMaterial) | ||
915 | { | ||
916 | m_material = pMaterial; | ||
917 | mu = _parent_scene.m_materialContactsData[pMaterial].mu; | ||
918 | bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; | ||
919 | } | ||
920 | |||
921 | public override float Density | ||
922 | { | ||
923 | get | ||
924 | { | ||
925 | return m_density * 100f; | ||
926 | } | ||
927 | set | ||
928 | { | ||
929 | m_density = value / 100f; | ||
930 | // for not prim mass is not updated since this implies full rebuild of body inertia TODO | ||
931 | } | ||
932 | } | ||
933 | public override float GravModifier | ||
934 | { | ||
935 | get | ||
936 | { | ||
937 | return m_gravmod; | ||
938 | } | ||
939 | set | ||
940 | { | ||
941 | m_gravmod = value; | ||
942 | if (m_vehicle != null) | ||
943 | m_vehicle.GravMod = m_gravmod; | ||
944 | } | ||
945 | } | ||
946 | public override float Friction | ||
947 | { | ||
948 | get | ||
949 | { | ||
950 | return mu; | ||
951 | } | ||
952 | set | ||
953 | { | ||
954 | mu = value; | ||
955 | } | ||
956 | } | ||
957 | |||
958 | public override float Restitution | ||
959 | { | ||
960 | get | ||
961 | { | ||
962 | return bounce; | ||
963 | } | ||
964 | set | ||
965 | { | ||
966 | bounce = value; | ||
967 | } | ||
968 | } | ||
969 | |||
970 | public void setPrimForRemoval() | ||
971 | { | ||
972 | AddChange(changes.Remove, null); | ||
973 | } | ||
974 | |||
975 | public override void link(PhysicsActor obj) | ||
976 | { | ||
977 | AddChange(changes.Link, obj); | ||
978 | } | ||
979 | |||
980 | public override void delink() | ||
981 | { | ||
982 | AddChange(changes.DeLink, null); | ||
983 | } | ||
984 | |||
985 | public override void LockAngularMotion(Vector3 axis) | ||
986 | { | ||
987 | // reverse the zero/non zero values for ODE. | ||
988 | if (axis.IsFinite()) | ||
989 | { | ||
990 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
991 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
992 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
993 | // m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
994 | AddChange(changes.AngLock, axis); | ||
995 | } | ||
996 | else | ||
997 | { | ||
998 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | ||
999 | } | ||
1000 | } | ||
1001 | |||
1002 | public override void SubscribeEvents(int ms) | ||
1003 | { | ||
1004 | m_eventsubscription = ms; | ||
1005 | m_cureventsubscription = 0; | ||
1006 | if (CollisionEventsThisFrame == null) | ||
1007 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1008 | SentEmptyCollisionsEvent = false; | ||
1009 | } | ||
1010 | |||
1011 | public override void UnSubscribeEvents() | ||
1012 | { | ||
1013 | if (CollisionEventsThisFrame != null) | ||
1014 | { | ||
1015 | CollisionEventsThisFrame.Clear(); | ||
1016 | CollisionEventsThisFrame = null; | ||
1017 | } | ||
1018 | m_eventsubscription = 0; | ||
1019 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1020 | } | ||
1021 | |||
1022 | public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
1023 | { | ||
1024 | if (CollisionEventsThisFrame == null) | ||
1025 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
1026 | // if(CollisionEventsThisFrame.Count < 32) | ||
1027 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
1028 | } | ||
1029 | |||
1030 | public void SendCollisions() | ||
1031 | { | ||
1032 | if (CollisionEventsThisFrame == null) | ||
1033 | return; | ||
1034 | |||
1035 | if (m_cureventsubscription < m_eventsubscription) | ||
1036 | return; | ||
1037 | |||
1038 | m_cureventsubscription = 0; | ||
1039 | |||
1040 | int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; | ||
1041 | |||
1042 | if (!SentEmptyCollisionsEvent || ncolisions > 0) | ||
1043 | { | ||
1044 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
1045 | |||
1046 | if (ncolisions == 0) | ||
1047 | { | ||
1048 | SentEmptyCollisionsEvent = true; | ||
1049 | _parent_scene.RemoveCollisionEventReporting(this); | ||
1050 | } | ||
1051 | else | ||
1052 | { | ||
1053 | SentEmptyCollisionsEvent = false; | ||
1054 | CollisionEventsThisFrame.Clear(); | ||
1055 | } | ||
1056 | } | ||
1057 | } | ||
1058 | |||
1059 | internal void AddCollisionFrameTime(int t) | ||
1060 | { | ||
1061 | if (m_cureventsubscription < 50000) | ||
1062 | m_cureventsubscription += t; | ||
1063 | } | ||
1064 | |||
1065 | public override bool SubscribedEvents() | ||
1066 | { | ||
1067 | if (m_eventsubscription > 0) | ||
1068 | return true; | ||
1069 | return false; | ||
1070 | } | ||
1071 | |||
1072 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
1073 | Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) | ||
1074 | { | ||
1075 | Name = primName; | ||
1076 | LocalID = plocalID; | ||
1077 | |||
1078 | m_vehicle = null; | ||
1079 | |||
1080 | if (!pos.IsFinite()) | ||
1081 | { | ||
1082 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | ||
1083 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | ||
1084 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); | ||
1085 | } | ||
1086 | _position = pos; | ||
1087 | givefakepos = 0; | ||
1088 | |||
1089 | m_timeStep = parent_scene.ODE_STEPSIZE; | ||
1090 | m_invTimeStep = 1f / m_timeStep; | ||
1091 | |||
1092 | m_density = parent_scene.geomDefaultDensity; | ||
1093 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
1094 | |||
1095 | prim_geom = IntPtr.Zero; | ||
1096 | collide_geom = IntPtr.Zero; | ||
1097 | Body = IntPtr.Zero; | ||
1098 | |||
1099 | if (!size.IsFinite()) | ||
1100 | { | ||
1101 | size = new Vector3(0.5f, 0.5f, 0.5f); | ||
1102 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); | ||
1103 | } | ||
1104 | |||
1105 | if (size.X <= 0) size.X = 0.01f; | ||
1106 | if (size.Y <= 0) size.Y = 0.01f; | ||
1107 | if (size.Z <= 0) size.Z = 0.01f; | ||
1108 | |||
1109 | _size = size; | ||
1110 | |||
1111 | if (!QuaternionIsFinite(rotation)) | ||
1112 | { | ||
1113 | rotation = Quaternion.Identity; | ||
1114 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); | ||
1115 | } | ||
1116 | |||
1117 | _orientation = rotation; | ||
1118 | givefakeori = 0; | ||
1119 | |||
1120 | _pbs = pbs; | ||
1121 | |||
1122 | _parent_scene = parent_scene; | ||
1123 | m_targetSpace = IntPtr.Zero; | ||
1124 | |||
1125 | if (pos.Z < 0) | ||
1126 | { | ||
1127 | m_isphysical = false; | ||
1128 | } | ||
1129 | else | ||
1130 | { | ||
1131 | m_isphysical = pisPhysical; | ||
1132 | } | ||
1133 | m_fakeisphysical = m_isphysical; | ||
1134 | |||
1135 | m_isVolumeDetect = false; | ||
1136 | m_fakeisVolumeDetect = false; | ||
1137 | |||
1138 | m_force = Vector3.Zero; | ||
1139 | |||
1140 | m_iscolliding = false; | ||
1141 | m_colliderfilter = 0; | ||
1142 | m_NoColide = false; | ||
1143 | |||
1144 | _triMeshData = IntPtr.Zero; | ||
1145 | |||
1146 | m_shapetype = _shapeType; | ||
1147 | |||
1148 | m_lastdoneSelected = false; | ||
1149 | m_isSelected = false; | ||
1150 | m_delaySelect = false; | ||
1151 | |||
1152 | m_isphantom = pisPhantom; | ||
1153 | m_fakeisphantom = pisPhantom; | ||
1154 | |||
1155 | mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; | ||
1156 | bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; | ||
1157 | |||
1158 | m_building = true; // control must set this to false when done | ||
1159 | |||
1160 | // get basic mass parameters | ||
1161 | ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype); | ||
1162 | |||
1163 | primVolume = repData.volume; | ||
1164 | m_OBB = repData.OBB; | ||
1165 | m_OBBOffset = repData.OBBOffset; | ||
1166 | |||
1167 | UpdatePrimBodyData(); | ||
1168 | } | ||
1169 | |||
1170 | private void resetCollisionAccounting() | ||
1171 | { | ||
1172 | m_collisionscore = 0; | ||
1173 | } | ||
1174 | |||
1175 | private void UpdateCollisionCatFlags() | ||
1176 | { | ||
1177 | if(m_isphysical && m_disabled) | ||
1178 | { | ||
1179 | m_collisionCategories = 0; | ||
1180 | m_collisionFlags = 0; | ||
1181 | } | ||
1182 | |||
1183 | else if (m_isSelected) | ||
1184 | { | ||
1185 | m_collisionCategories = CollisionCategories.Selected; | ||
1186 | m_collisionFlags = 0; | ||
1187 | } | ||
1188 | |||
1189 | else if (m_isVolumeDetect) | ||
1190 | { | ||
1191 | m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1192 | if (m_isphysical) | ||
1193 | m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; | ||
1194 | else | ||
1195 | m_collisionFlags = 0; | ||
1196 | } | ||
1197 | else if (m_isphantom) | ||
1198 | { | ||
1199 | m_collisionCategories = CollisionCategories.Phantom; | ||
1200 | if (m_isphysical) | ||
1201 | m_collisionFlags = CollisionCategories.Land; | ||
1202 | else | ||
1203 | m_collisionFlags = 0; | ||
1204 | } | ||
1205 | else | ||
1206 | { | ||
1207 | m_collisionCategories = CollisionCategories.Geom; | ||
1208 | if (m_isphysical) | ||
1209 | m_collisionFlags = m_default_collisionFlagsPhysical; | ||
1210 | else | ||
1211 | m_collisionFlags = m_default_collisionFlagsNotPhysical; | ||
1212 | } | ||
1213 | } | ||
1214 | |||
1215 | private void ApplyCollisionCatFlags() | ||
1216 | { | ||
1217 | if (prim_geom != IntPtr.Zero) | ||
1218 | { | ||
1219 | if (!childPrim && childrenPrim.Count > 0) | ||
1220 | { | ||
1221 | foreach (OdePrim prm in childrenPrim) | ||
1222 | { | ||
1223 | if (m_isphysical && m_disabled) | ||
1224 | { | ||
1225 | prm.m_collisionCategories = 0; | ||
1226 | prm.m_collisionFlags = 0; | ||
1227 | } | ||
1228 | else | ||
1229 | { | ||
1230 | // preserve some | ||
1231 | if (prm.m_isSelected) | ||
1232 | { | ||
1233 | prm.m_collisionCategories = CollisionCategories.Selected; | ||
1234 | prm.m_collisionFlags = 0; | ||
1235 | } | ||
1236 | else if (prm.m_isVolumeDetect) | ||
1237 | { | ||
1238 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1239 | if (m_isphysical) | ||
1240 | prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; | ||
1241 | else | ||
1242 | prm.m_collisionFlags = 0; | ||
1243 | } | ||
1244 | else if (prm.m_isphantom) | ||
1245 | { | ||
1246 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1247 | if (m_isphysical) | ||
1248 | prm.m_collisionFlags = CollisionCategories.Land; | ||
1249 | else | ||
1250 | prm.m_collisionFlags = 0; | ||
1251 | } | ||
1252 | else | ||
1253 | { | ||
1254 | prm.m_collisionCategories = m_collisionCategories; | ||
1255 | prm.m_collisionFlags = m_collisionFlags; | ||
1256 | } | ||
1257 | } | ||
1258 | |||
1259 | if (prm.prim_geom != IntPtr.Zero) | ||
1260 | { | ||
1261 | if (prm.m_NoColide) | ||
1262 | { | ||
1263 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1264 | if (m_isphysical) | ||
1265 | d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); | ||
1266 | else | ||
1267 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1268 | } | ||
1269 | else | ||
1270 | { | ||
1271 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); | ||
1272 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1273 | } | ||
1274 | } | ||
1275 | } | ||
1276 | } | ||
1277 | |||
1278 | if (m_NoColide) | ||
1279 | { | ||
1280 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1281 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); | ||
1282 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1283 | { | ||
1284 | d.GeomSetCategoryBits(collide_geom, 0); | ||
1285 | d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); | ||
1286 | } | ||
1287 | } | ||
1288 | else | ||
1289 | { | ||
1290 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1291 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1292 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
1293 | { | ||
1294 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); | ||
1295 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
1296 | } | ||
1297 | } | ||
1298 | } | ||
1299 | } | ||
1300 | |||
1301 | private void createAMotor(Vector3 axis) | ||
1302 | { | ||
1303 | if (Body == IntPtr.Zero) | ||
1304 | return; | ||
1305 | |||
1306 | if (Amotor != IntPtr.Zero) | ||
1307 | { | ||
1308 | d.JointDestroy(Amotor); | ||
1309 | Amotor = IntPtr.Zero; | ||
1310 | } | ||
1311 | |||
1312 | int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); | ||
1313 | |||
1314 | if (axisnum <= 0) | ||
1315 | return; | ||
1316 | |||
1317 | // stop it | ||
1318 | d.BodySetTorque(Body, 0, 0, 0); | ||
1319 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
1320 | |||
1321 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
1322 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
1323 | |||
1324 | d.JointSetAMotorMode(Amotor, 0); | ||
1325 | |||
1326 | d.JointSetAMotorNumAxes(Amotor, axisnum); | ||
1327 | |||
1328 | // get current orientation to lock | ||
1329 | |||
1330 | d.Quaternion dcur = d.BodyGetQuaternion(Body); | ||
1331 | Quaternion curr; // crap convertion between identical things | ||
1332 | curr.X = dcur.X; | ||
1333 | curr.Y = dcur.Y; | ||
1334 | curr.Z = dcur.Z; | ||
1335 | curr.W = dcur.W; | ||
1336 | Vector3 ax; | ||
1337 | |||
1338 | int i = 0; | ||
1339 | int j = 0; | ||
1340 | if (axis.X == 0) | ||
1341 | { | ||
1342 | ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X | ||
1343 | // ODE should do this with axis relative to body 1 but seems to fail | ||
1344 | d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); | ||
1345 | d.JointSetAMotorAngle(Amotor, 0, 0); | ||
1346 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); | ||
1347 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); | ||
1348 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); | ||
1349 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1350 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); | ||
1351 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); | ||
1352 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); | ||
1353 | d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); | ||
1354 | i++; | ||
1355 | j = 256; // move to next axis set | ||
1356 | } | ||
1357 | |||
1358 | if (axis.Y == 0) | ||
1359 | { | ||
1360 | ax = (new Vector3(0, 1, 0)) * curr; | ||
1361 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
1362 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
1363 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
1364 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
1365 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1366 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1367 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1368 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1369 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1370 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1371 | i++; | ||
1372 | j += 256; | ||
1373 | } | ||
1374 | |||
1375 | if (axis.Z == 0) | ||
1376 | { | ||
1377 | ax = (new Vector3(0, 0, 1)) * curr; | ||
1378 | d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); | ||
1379 | d.JointSetAMotorAngle(Amotor, i, 0); | ||
1380 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); | ||
1381 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); | ||
1382 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); | ||
1383 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); | ||
1384 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); | ||
1385 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); | ||
1386 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); | ||
1387 | d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); | ||
1388 | } | ||
1389 | } | ||
1390 | |||
1391 | |||
1392 | private void SetGeom(IntPtr geom) | ||
1393 | { | ||
1394 | prim_geom = geom; | ||
1395 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); | ||
1396 | if (prim_geom != IntPtr.Zero) | ||
1397 | { | ||
1398 | |||
1399 | if (m_NoColide) | ||
1400 | { | ||
1401 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1402 | if (m_isphysical) | ||
1403 | { | ||
1404 | d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); | ||
1405 | } | ||
1406 | else | ||
1407 | { | ||
1408 | d.GeomSetCollideBits(prim_geom, 0); | ||
1409 | d.GeomDisable(prim_geom); | ||
1410 | } | ||
1411 | } | ||
1412 | else | ||
1413 | { | ||
1414 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1415 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1416 | } | ||
1417 | |||
1418 | UpdatePrimBodyData(); | ||
1419 | _parent_scene.actor_name_map[prim_geom] = this; | ||
1420 | |||
1421 | /* | ||
1422 | // debug | ||
1423 | d.AABB aabb; | ||
1424 | d.GeomGetAABB(prim_geom, out aabb); | ||
1425 | float x = aabb.MaxX - aabb.MinX; | ||
1426 | float y = aabb.MaxY - aabb.MinY; | ||
1427 | float z = aabb.MaxZ - aabb.MinZ; | ||
1428 | if( x > 60.0f || y > 60.0f || z > 60.0f) | ||
1429 | m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1430 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1431 | else if (x < 0.001f || y < 0.001f || z < 0.001f) | ||
1432 | m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", | ||
1433 | Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); | ||
1434 | |||
1435 | // | ||
1436 | */ | ||
1437 | |||
1438 | } | ||
1439 | else | ||
1440 | m_log.Warn("Setting bad Geom"); | ||
1441 | } | ||
1442 | |||
1443 | private bool GetMeshGeom() | ||
1444 | { | ||
1445 | IntPtr vertices, indices; | ||
1446 | int vertexCount, indexCount; | ||
1447 | int vertexStride, triStride; | ||
1448 | |||
1449 | IMesh mesh = m_mesh; | ||
1450 | |||
1451 | if (mesh == null) | ||
1452 | return false; | ||
1453 | |||
1454 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); | ||
1455 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); | ||
1456 | |||
1457 | if (vertexCount == 0 || indexCount == 0) | ||
1458 | { | ||
1459 | m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", | ||
1460 | Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); | ||
1461 | |||
1462 | m_hasOBB = false; | ||
1463 | m_OBBOffset = Vector3.Zero; | ||
1464 | m_OBB = _size * 0.5f; | ||
1465 | |||
1466 | m_physCost = 0.1f; | ||
1467 | m_streamCost = 1.0f; | ||
1468 | |||
1469 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1470 | m_meshState = MeshState.MeshFailed; | ||
1471 | m_mesh = null; | ||
1472 | return false; | ||
1473 | } | ||
1474 | |||
1475 | IntPtr geo = IntPtr.Zero; | ||
1476 | |||
1477 | try | ||
1478 | { | ||
1479 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
1480 | |||
1481 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | ||
1482 | d.GeomTriMeshDataPreprocess(_triMeshData); | ||
1483 | |||
1484 | geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); | ||
1485 | } | ||
1486 | |||
1487 | catch (Exception e) | ||
1488 | { | ||
1489 | m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); | ||
1490 | if (_triMeshData != IntPtr.Zero) | ||
1491 | { | ||
1492 | try | ||
1493 | { | ||
1494 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1495 | } | ||
1496 | catch | ||
1497 | { | ||
1498 | } | ||
1499 | } | ||
1500 | _triMeshData = IntPtr.Zero; | ||
1501 | |||
1502 | m_hasOBB = false; | ||
1503 | m_OBBOffset = Vector3.Zero; | ||
1504 | m_OBB = _size * 0.5f; | ||
1505 | m_physCost = 0.1f; | ||
1506 | m_streamCost = 1.0f; | ||
1507 | |||
1508 | _parent_scene.mesher.ReleaseMesh(mesh); | ||
1509 | m_meshState = MeshState.MeshFailed; | ||
1510 | m_mesh = null; | ||
1511 | return false; | ||
1512 | } | ||
1513 | |||
1514 | m_physCost = 0.0013f * (float)indexCount; | ||
1515 | // todo | ||
1516 | m_streamCost = 1.0f; | ||
1517 | |||
1518 | SetGeom(geo); | ||
1519 | |||
1520 | return true; | ||
1521 | } | ||
1522 | |||
1523 | private void CreateGeom() | ||
1524 | { | ||
1525 | bool hasMesh = false; | ||
1526 | |||
1527 | m_NoColide = false; | ||
1528 | |||
1529 | if ((m_meshState & MeshState.MeshNoColide) != 0) | ||
1530 | m_NoColide = true; | ||
1531 | |||
1532 | else if(m_mesh != null) | ||
1533 | { | ||
1534 | if (GetMeshGeom()) | ||
1535 | hasMesh = true; | ||
1536 | else | ||
1537 | m_NoColide = true; | ||
1538 | } | ||
1539 | |||
1540 | |||
1541 | if (!hasMesh) | ||
1542 | { | ||
1543 | IntPtr geo = IntPtr.Zero; | ||
1544 | |||
1545 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 | ||
1546 | && _size.X == _size.Y && _size.Y == _size.Z) | ||
1547 | { // it's a sphere | ||
1548 | try | ||
1549 | { | ||
1550 | geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); | ||
1551 | } | ||
1552 | catch (Exception e) | ||
1553 | { | ||
1554 | m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); | ||
1555 | return; | ||
1556 | } | ||
1557 | } | ||
1558 | else | ||
1559 | {// do it as a box | ||
1560 | try | ||
1561 | { | ||
1562 | geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); | ||
1563 | } | ||
1564 | catch (Exception e) | ||
1565 | { | ||
1566 | m_log.Warn("[PHYSICS]: Create box failed: {0}", e); | ||
1567 | return; | ||
1568 | } | ||
1569 | } | ||
1570 | m_physCost = 0.1f; | ||
1571 | m_streamCost = 1.0f; | ||
1572 | SetGeom(geo); | ||
1573 | } | ||
1574 | } | ||
1575 | |||
1576 | private void RemoveGeom() | ||
1577 | { | ||
1578 | if (prim_geom != IntPtr.Zero) | ||
1579 | { | ||
1580 | _parent_scene.actor_name_map.Remove(prim_geom); | ||
1581 | |||
1582 | try | ||
1583 | { | ||
1584 | d.GeomDestroy(prim_geom); | ||
1585 | if (_triMeshData != IntPtr.Zero) | ||
1586 | { | ||
1587 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1588 | _triMeshData = IntPtr.Zero; | ||
1589 | } | ||
1590 | } | ||
1591 | catch (Exception e) | ||
1592 | { | ||
1593 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); | ||
1594 | } | ||
1595 | |||
1596 | prim_geom = IntPtr.Zero; | ||
1597 | collide_geom = IntPtr.Zero; | ||
1598 | m_targetSpace = IntPtr.Zero; | ||
1599 | } | ||
1600 | else | ||
1601 | { | ||
1602 | m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); | ||
1603 | } | ||
1604 | |||
1605 | lock (m_meshlock) | ||
1606 | { | ||
1607 | if (m_mesh != null) | ||
1608 | { | ||
1609 | _parent_scene.mesher.ReleaseMesh(m_mesh); | ||
1610 | m_mesh = null; | ||
1611 | } | ||
1612 | } | ||
1613 | |||
1614 | Body = IntPtr.Zero; | ||
1615 | m_hasOBB = false; | ||
1616 | } | ||
1617 | |||
1618 | //sets non physical prim m_targetSpace to right space in spaces grid for static prims | ||
1619 | // should only be called for non physical prims unless they are becoming non physical | ||
1620 | private void SetInStaticSpace(OdePrim prim) | ||
1621 | { | ||
1622 | IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); | ||
1623 | prim.m_targetSpace = targetSpace; | ||
1624 | collide_geom = IntPtr.Zero; | ||
1625 | } | ||
1626 | |||
1627 | public void enableBodySoft() | ||
1628 | { | ||
1629 | m_disabled = false; | ||
1630 | if (!childPrim && !m_isSelected) | ||
1631 | { | ||
1632 | if (m_isphysical && Body != IntPtr.Zero) | ||
1633 | { | ||
1634 | UpdateCollisionCatFlags(); | ||
1635 | ApplyCollisionCatFlags(); | ||
1636 | |||
1637 | d.BodyEnable(Body); | ||
1638 | } | ||
1639 | } | ||
1640 | resetCollisionAccounting(); | ||
1641 | } | ||
1642 | |||
1643 | private void disableBodySoft() | ||
1644 | { | ||
1645 | m_disabled = true; | ||
1646 | if (!childPrim) | ||
1647 | { | ||
1648 | if (m_isphysical && Body != IntPtr.Zero) | ||
1649 | { | ||
1650 | if (m_isSelected) | ||
1651 | m_collisionFlags = CollisionCategories.Selected; | ||
1652 | else | ||
1653 | m_collisionCategories = 0; | ||
1654 | m_collisionFlags = 0; | ||
1655 | ApplyCollisionCatFlags(); | ||
1656 | d.BodyDisable(Body); | ||
1657 | } | ||
1658 | } | ||
1659 | } | ||
1660 | |||
1661 | private void MakeBody() | ||
1662 | { | ||
1663 | if (!m_isphysical) // only physical get bodies | ||
1664 | return; | ||
1665 | |||
1666 | if (childPrim) // child prims don't get bodies; | ||
1667 | return; | ||
1668 | |||
1669 | if (m_building) | ||
1670 | return; | ||
1671 | |||
1672 | if (prim_geom == IntPtr.Zero) | ||
1673 | { | ||
1674 | m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); | ||
1675 | return; | ||
1676 | } | ||
1677 | |||
1678 | if (Body != IntPtr.Zero) | ||
1679 | { | ||
1680 | DestroyBody(); | ||
1681 | m_log.Warn("[PHYSICS]: MakeBody called having a body"); | ||
1682 | } | ||
1683 | |||
1684 | if (d.GeomGetBody(prim_geom) != IntPtr.Zero) | ||
1685 | { | ||
1686 | d.GeomSetBody(prim_geom, IntPtr.Zero); | ||
1687 | m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); | ||
1688 | } | ||
1689 | |||
1690 | d.Matrix3 mymat = new d.Matrix3(); | ||
1691 | d.Quaternion myrot = new d.Quaternion(); | ||
1692 | d.Mass objdmass = new d.Mass { }; | ||
1693 | |||
1694 | Body = d.BodyCreate(_parent_scene.world); | ||
1695 | |||
1696 | objdmass = primdMass; | ||
1697 | |||
1698 | // rotate inertia | ||
1699 | myrot.X = _orientation.X; | ||
1700 | myrot.Y = _orientation.Y; | ||
1701 | myrot.Z = _orientation.Z; | ||
1702 | myrot.W = _orientation.W; | ||
1703 | |||
1704 | d.RfromQ(out mymat, ref myrot); | ||
1705 | d.MassRotate(ref objdmass, ref mymat); | ||
1706 | |||
1707 | // set the body rotation | ||
1708 | d.BodySetRotation(Body, ref mymat); | ||
1709 | |||
1710 | // recompute full object inertia if needed | ||
1711 | if (childrenPrim.Count > 0) | ||
1712 | { | ||
1713 | d.Matrix3 mat = new d.Matrix3(); | ||
1714 | d.Quaternion quat = new d.Quaternion(); | ||
1715 | d.Mass tmpdmass = new d.Mass { }; | ||
1716 | Vector3 rcm; | ||
1717 | |||
1718 | rcm.X = _position.X; | ||
1719 | rcm.Y = _position.Y; | ||
1720 | rcm.Z = _position.Z; | ||
1721 | |||
1722 | lock (childrenPrim) | ||
1723 | { | ||
1724 | foreach (OdePrim prm in childrenPrim) | ||
1725 | { | ||
1726 | if (prm.prim_geom == IntPtr.Zero) | ||
1727 | { | ||
1728 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); | ||
1729 | continue; | ||
1730 | } | ||
1731 | |||
1732 | tmpdmass = prm.primdMass; | ||
1733 | |||
1734 | // apply prim current rotation to inertia | ||
1735 | quat.X = prm._orientation.X; | ||
1736 | quat.Y = prm._orientation.Y; | ||
1737 | quat.Z = prm._orientation.Z; | ||
1738 | quat.W = prm._orientation.W; | ||
1739 | d.RfromQ(out mat, ref quat); | ||
1740 | d.MassRotate(ref tmpdmass, ref mat); | ||
1741 | |||
1742 | Vector3 ppos = prm._position; | ||
1743 | ppos.X -= rcm.X; | ||
1744 | ppos.Y -= rcm.Y; | ||
1745 | ppos.Z -= rcm.Z; | ||
1746 | // refer inertia to root prim center of mass position | ||
1747 | d.MassTranslate(ref tmpdmass, | ||
1748 | ppos.X, | ||
1749 | ppos.Y, | ||
1750 | ppos.Z); | ||
1751 | |||
1752 | d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia | ||
1753 | // fix prim colision cats | ||
1754 | |||
1755 | if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) | ||
1756 | { | ||
1757 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); | ||
1758 | m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); | ||
1759 | } | ||
1760 | |||
1761 | d.GeomClearOffset(prm.prim_geom); | ||
1762 | d.GeomSetBody(prm.prim_geom, Body); | ||
1763 | prm.Body = Body; | ||
1764 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation | ||
1765 | } | ||
1766 | } | ||
1767 | } | ||
1768 | |||
1769 | d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset | ||
1770 | // associate root geom with body | ||
1771 | d.GeomSetBody(prim_geom, Body); | ||
1772 | |||
1773 | d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); | ||
1774 | d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1775 | |||
1776 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
1777 | myrot.X = -myrot.X; | ||
1778 | myrot.Y = -myrot.Y; | ||
1779 | myrot.Z = -myrot.Z; | ||
1780 | |||
1781 | d.RfromQ(out mymat, ref myrot); | ||
1782 | d.MassRotate(ref objdmass, ref mymat); | ||
1783 | |||
1784 | d.BodySetMass(Body, ref objdmass); | ||
1785 | _mass = objdmass.mass; | ||
1786 | |||
1787 | // disconnect from world gravity so we can apply buoyancy | ||
1788 | d.BodySetGravityMode(Body, false); | ||
1789 | |||
1790 | d.BodySetAutoDisableFlag(Body, true); | ||
1791 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1792 | d.BodySetAutoDisableAngularThreshold(Body, 0.01f); | ||
1793 | d.BodySetAutoDisableLinearThreshold(Body, 0.01f); | ||
1794 | d.BodySetDamping(Body, .005f, .001f); | ||
1795 | |||
1796 | if (m_targetSpace != IntPtr.Zero) | ||
1797 | { | ||
1798 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1799 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
1800 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
1801 | } | ||
1802 | |||
1803 | if (childrenPrim.Count == 0) | ||
1804 | { | ||
1805 | collide_geom = prim_geom; | ||
1806 | m_targetSpace = _parent_scene.ActiveSpace; | ||
1807 | } | ||
1808 | else | ||
1809 | { | ||
1810 | m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); | ||
1811 | d.HashSpaceSetLevels(m_targetSpace, -2, 8); | ||
1812 | d.SpaceSetSublevel(m_targetSpace, 3); | ||
1813 | d.SpaceSetCleanup(m_targetSpace, false); | ||
1814 | |||
1815 | d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | | ||
1816 | CollisionCategories.Geom | | ||
1817 | CollisionCategories.Phantom | | ||
1818 | CollisionCategories.VolumeDtc | ||
1819 | )); | ||
1820 | d.GeomSetCollideBits(m_targetSpace, 0); | ||
1821 | collide_geom = m_targetSpace; | ||
1822 | } | ||
1823 | |||
1824 | d.SpaceAdd(m_targetSpace, prim_geom); | ||
1825 | |||
1826 | if (m_delaySelect) | ||
1827 | { | ||
1828 | m_isSelected = true; | ||
1829 | m_delaySelect = false; | ||
1830 | } | ||
1831 | |||
1832 | m_collisionscore = 0; | ||
1833 | |||
1834 | UpdateCollisionCatFlags(); | ||
1835 | ApplyCollisionCatFlags(); | ||
1836 | |||
1837 | _parent_scene.addActivePrim(this); | ||
1838 | |||
1839 | lock (childrenPrim) | ||
1840 | { | ||
1841 | foreach (OdePrim prm in childrenPrim) | ||
1842 | { | ||
1843 | if (prm.prim_geom == IntPtr.Zero) | ||
1844 | continue; | ||
1845 | |||
1846 | Vector3 ppos = prm._position; | ||
1847 | d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position | ||
1848 | |||
1849 | if (prm.m_targetSpace != m_targetSpace) | ||
1850 | { | ||
1851 | if (prm.m_targetSpace != IntPtr.Zero) | ||
1852 | { | ||
1853 | _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); | ||
1854 | if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) | ||
1855 | d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); | ||
1856 | } | ||
1857 | prm.m_targetSpace = m_targetSpace; | ||
1858 | d.SpaceAdd(m_targetSpace, prm.prim_geom); | ||
1859 | } | ||
1860 | |||
1861 | prm.m_collisionscore = 0; | ||
1862 | |||
1863 | if(!m_disabled) | ||
1864 | prm.m_disabled = false; | ||
1865 | |||
1866 | _parent_scene.addActivePrim(prm); | ||
1867 | } | ||
1868 | } | ||
1869 | |||
1870 | // The body doesn't already have a finite rotation mode set here | ||
1871 | if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) | ||
1872 | { | ||
1873 | createAMotor(m_angularlock); | ||
1874 | } | ||
1875 | |||
1876 | |||
1877 | if (m_isSelected || m_disabled) | ||
1878 | { | ||
1879 | d.BodyDisable(Body); | ||
1880 | } | ||
1881 | else | ||
1882 | { | ||
1883 | d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); | ||
1884 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | ||
1885 | } | ||
1886 | _parent_scene.addActiveGroups(this); | ||
1887 | } | ||
1888 | |||
1889 | private void DestroyBody() | ||
1890 | { | ||
1891 | if (Body != IntPtr.Zero) | ||
1892 | { | ||
1893 | _parent_scene.remActivePrim(this); | ||
1894 | |||
1895 | collide_geom = IntPtr.Zero; | ||
1896 | |||
1897 | if (m_disabled) | ||
1898 | m_collisionCategories = 0; | ||
1899 | else if (m_isSelected) | ||
1900 | m_collisionCategories = CollisionCategories.Selected; | ||
1901 | else if (m_isVolumeDetect) | ||
1902 | m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1903 | else if (m_isphantom) | ||
1904 | m_collisionCategories = CollisionCategories.Phantom; | ||
1905 | else | ||
1906 | m_collisionCategories = CollisionCategories.Geom; | ||
1907 | |||
1908 | m_collisionFlags = 0; | ||
1909 | |||
1910 | if (prim_geom != IntPtr.Zero) | ||
1911 | { | ||
1912 | if (m_NoColide) | ||
1913 | { | ||
1914 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1915 | d.GeomSetCollideBits(prim_geom, 0); | ||
1916 | } | ||
1917 | else | ||
1918 | { | ||
1919 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
1920 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
1921 | } | ||
1922 | UpdateDataFromGeom(); | ||
1923 | d.GeomSetBody(prim_geom, IntPtr.Zero); | ||
1924 | SetInStaticSpace(this); | ||
1925 | } | ||
1926 | |||
1927 | if (!childPrim) | ||
1928 | { | ||
1929 | lock (childrenPrim) | ||
1930 | { | ||
1931 | foreach (OdePrim prm in childrenPrim) | ||
1932 | { | ||
1933 | _parent_scene.remActivePrim(prm); | ||
1934 | |||
1935 | if (prm.m_isSelected) | ||
1936 | prm.m_collisionCategories = CollisionCategories.Selected; | ||
1937 | else if (prm.m_isVolumeDetect) | ||
1938 | prm.m_collisionCategories = CollisionCategories.VolumeDtc; | ||
1939 | else if (prm.m_isphantom) | ||
1940 | prm.m_collisionCategories = CollisionCategories.Phantom; | ||
1941 | else | ||
1942 | prm.m_collisionCategories = CollisionCategories.Geom; | ||
1943 | |||
1944 | prm.m_collisionFlags = 0; | ||
1945 | |||
1946 | if (prm.prim_geom != IntPtr.Zero) | ||
1947 | { | ||
1948 | if (prm.m_NoColide) | ||
1949 | { | ||
1950 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1951 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1952 | } | ||
1953 | else | ||
1954 | { | ||
1955 | d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); | ||
1956 | d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); | ||
1957 | } | ||
1958 | prm.UpdateDataFromGeom(); | ||
1959 | SetInStaticSpace(prm); | ||
1960 | } | ||
1961 | prm.Body = IntPtr.Zero; | ||
1962 | prm._mass = prm.primMass; | ||
1963 | prm.m_collisionscore = 0; | ||
1964 | } | ||
1965 | } | ||
1966 | if (Amotor != IntPtr.Zero) | ||
1967 | { | ||
1968 | d.JointDestroy(Amotor); | ||
1969 | Amotor = IntPtr.Zero; | ||
1970 | } | ||
1971 | _parent_scene.remActiveGroup(this); | ||
1972 | d.BodyDestroy(Body); | ||
1973 | } | ||
1974 | Body = IntPtr.Zero; | ||
1975 | } | ||
1976 | _mass = primMass; | ||
1977 | m_collisionscore = 0; | ||
1978 | } | ||
1979 | |||
1980 | private void FixInertia(Vector3 NewPos,Quaternion newrot) | ||
1981 | { | ||
1982 | d.Matrix3 mat = new d.Matrix3(); | ||
1983 | d.Quaternion quat = new d.Quaternion(); | ||
1984 | |||
1985 | d.Mass tmpdmass = new d.Mass { }; | ||
1986 | d.Mass objdmass = new d.Mass { }; | ||
1987 | |||
1988 | d.BodyGetMass(Body, out tmpdmass); | ||
1989 | objdmass = tmpdmass; | ||
1990 | |||
1991 | d.Vector3 dobjpos; | ||
1992 | d.Vector3 thispos; | ||
1993 | |||
1994 | // get current object position and rotation | ||
1995 | dobjpos = d.BodyGetPosition(Body); | ||
1996 | |||
1997 | // get prim own inertia in its local frame | ||
1998 | tmpdmass = primdMass; | ||
1999 | |||
2000 | // transform to object frame | ||
2001 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2002 | d.MassRotate(ref tmpdmass, ref mat); | ||
2003 | |||
2004 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2005 | d.MassTranslate(ref tmpdmass, | ||
2006 | thispos.X, | ||
2007 | thispos.Y, | ||
2008 | thispos.Z); | ||
2009 | |||
2010 | // subtract current prim inertia from object | ||
2011 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2012 | |||
2013 | // back prim own inertia | ||
2014 | tmpdmass = primdMass; | ||
2015 | |||
2016 | // update to new position and orientation | ||
2017 | _position = NewPos; | ||
2018 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); | ||
2019 | _orientation = newrot; | ||
2020 | quat.X = newrot.X; | ||
2021 | quat.Y = newrot.Y; | ||
2022 | quat.Z = newrot.Z; | ||
2023 | quat.W = newrot.W; | ||
2024 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); | ||
2025 | |||
2026 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2027 | d.MassRotate(ref tmpdmass, ref mat); | ||
2028 | |||
2029 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2030 | d.MassTranslate(ref tmpdmass, | ||
2031 | thispos.X, | ||
2032 | thispos.Y, | ||
2033 | thispos.Z); | ||
2034 | |||
2035 | d.MassAdd(ref objdmass, ref tmpdmass); | ||
2036 | |||
2037 | // fix all positions | ||
2038 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2039 | while (g != IntPtr.Zero) | ||
2040 | { | ||
2041 | thispos = d.GeomGetOffsetPosition(g); | ||
2042 | thispos.X -= objdmass.c.X; | ||
2043 | thispos.Y -= objdmass.c.Y; | ||
2044 | thispos.Z -= objdmass.c.Z; | ||
2045 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2046 | g = d.dBodyGetNextGeom(g); | ||
2047 | } | ||
2048 | d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); | ||
2049 | |||
2050 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2051 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2052 | d.BodySetMass(Body, ref objdmass); | ||
2053 | _mass = objdmass.mass; | ||
2054 | } | ||
2055 | |||
2056 | |||
2057 | |||
2058 | private void FixInertia(Vector3 NewPos) | ||
2059 | { | ||
2060 | d.Matrix3 primmat = new d.Matrix3(); | ||
2061 | d.Mass tmpdmass = new d.Mass { }; | ||
2062 | d.Mass objdmass = new d.Mass { }; | ||
2063 | d.Mass primmass = new d.Mass { }; | ||
2064 | |||
2065 | d.Vector3 dobjpos; | ||
2066 | d.Vector3 thispos; | ||
2067 | |||
2068 | d.BodyGetMass(Body, out objdmass); | ||
2069 | |||
2070 | // get prim own inertia in its local frame | ||
2071 | primmass = primdMass; | ||
2072 | // transform to object frame | ||
2073 | primmat = d.GeomGetOffsetRotation(prim_geom); | ||
2074 | d.MassRotate(ref primmass, ref primmat); | ||
2075 | |||
2076 | tmpdmass = primmass; | ||
2077 | |||
2078 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2079 | d.MassTranslate(ref tmpdmass, | ||
2080 | thispos.X, | ||
2081 | thispos.Y, | ||
2082 | thispos.Z); | ||
2083 | |||
2084 | // subtract current prim inertia from object | ||
2085 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2086 | |||
2087 | // update to new position | ||
2088 | _position = NewPos; | ||
2089 | d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); | ||
2090 | |||
2091 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2092 | d.MassTranslate(ref primmass, | ||
2093 | thispos.X, | ||
2094 | thispos.Y, | ||
2095 | thispos.Z); | ||
2096 | |||
2097 | d.MassAdd(ref objdmass, ref primmass); | ||
2098 | |||
2099 | // fix all positions | ||
2100 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2101 | while (g != IntPtr.Zero) | ||
2102 | { | ||
2103 | thispos = d.GeomGetOffsetPosition(g); | ||
2104 | thispos.X -= objdmass.c.X; | ||
2105 | thispos.Y -= objdmass.c.Y; | ||
2106 | thispos.Z -= objdmass.c.Z; | ||
2107 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2108 | g = d.dBodyGetNextGeom(g); | ||
2109 | } | ||
2110 | |||
2111 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); | ||
2112 | |||
2113 | // get current object position and rotation | ||
2114 | dobjpos = d.BodyGetPosition(Body); | ||
2115 | |||
2116 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2117 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2118 | d.BodySetMass(Body, ref objdmass); | ||
2119 | _mass = objdmass.mass; | ||
2120 | } | ||
2121 | |||
2122 | private void FixInertia(Quaternion newrot) | ||
2123 | { | ||
2124 | d.Matrix3 mat = new d.Matrix3(); | ||
2125 | d.Quaternion quat = new d.Quaternion(); | ||
2126 | |||
2127 | d.Mass tmpdmass = new d.Mass { }; | ||
2128 | d.Mass objdmass = new d.Mass { }; | ||
2129 | d.Vector3 dobjpos; | ||
2130 | d.Vector3 thispos; | ||
2131 | |||
2132 | d.BodyGetMass(Body, out objdmass); | ||
2133 | |||
2134 | // get prim own inertia in its local frame | ||
2135 | tmpdmass = primdMass; | ||
2136 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2137 | d.MassRotate(ref tmpdmass, ref mat); | ||
2138 | // transform to object frame | ||
2139 | thispos = d.GeomGetOffsetPosition(prim_geom); | ||
2140 | d.MassTranslate(ref tmpdmass, | ||
2141 | thispos.X, | ||
2142 | thispos.Y, | ||
2143 | thispos.Z); | ||
2144 | |||
2145 | // subtract current prim inertia from object | ||
2146 | DMassSubPartFromObj(ref tmpdmass, ref objdmass); | ||
2147 | |||
2148 | // update to new orientation | ||
2149 | _orientation = newrot; | ||
2150 | quat.X = newrot.X; | ||
2151 | quat.Y = newrot.Y; | ||
2152 | quat.Z = newrot.Z; | ||
2153 | quat.W = newrot.W; | ||
2154 | d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); | ||
2155 | |||
2156 | tmpdmass = primdMass; | ||
2157 | mat = d.GeomGetOffsetRotation(prim_geom); | ||
2158 | d.MassRotate(ref tmpdmass, ref mat); | ||
2159 | d.MassTranslate(ref tmpdmass, | ||
2160 | thispos.X, | ||
2161 | thispos.Y, | ||
2162 | thispos.Z); | ||
2163 | |||
2164 | d.MassAdd(ref objdmass, ref tmpdmass); | ||
2165 | |||
2166 | // fix all positions | ||
2167 | IntPtr g = d.BodyGetFirstGeom(Body); | ||
2168 | while (g != IntPtr.Zero) | ||
2169 | { | ||
2170 | thispos = d.GeomGetOffsetPosition(g); | ||
2171 | thispos.X -= objdmass.c.X; | ||
2172 | thispos.Y -= objdmass.c.Y; | ||
2173 | thispos.Z -= objdmass.c.Z; | ||
2174 | d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); | ||
2175 | g = d.dBodyGetNextGeom(g); | ||
2176 | } | ||
2177 | |||
2178 | d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); | ||
2179 | // get current object position and rotation | ||
2180 | dobjpos = d.BodyGetPosition(Body); | ||
2181 | |||
2182 | d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); | ||
2183 | d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body | ||
2184 | d.BodySetMass(Body, ref objdmass); | ||
2185 | _mass = objdmass.mass; | ||
2186 | } | ||
2187 | |||
2188 | |||
2189 | #region Mass Calculation | ||
2190 | |||
2191 | private void UpdatePrimBodyData() | ||
2192 | { | ||
2193 | primMass = m_density * primVolume; | ||
2194 | |||
2195 | if (primMass <= 0) | ||
2196 | primMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
2197 | if (primMass > _parent_scene.maximumMassObject) | ||
2198 | primMass = _parent_scene.maximumMassObject; | ||
2199 | |||
2200 | _mass = primMass; // just in case | ||
2201 | |||
2202 | d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); | ||
2203 | |||
2204 | d.MassTranslate(ref primdMass, | ||
2205 | m_OBBOffset.X, | ||
2206 | m_OBBOffset.Y, | ||
2207 | m_OBBOffset.Z); | ||
2208 | |||
2209 | primOOBradiusSQ = m_OBB.LengthSquared(); | ||
2210 | |||
2211 | if (_triMeshData != IntPtr.Zero) | ||
2212 | { | ||
2213 | float pc = m_physCost; | ||
2214 | float psf = primOOBradiusSQ; | ||
2215 | psf *= 1.33f * .2f; | ||
2216 | pc *= psf; | ||
2217 | if (pc < 0.1f) | ||
2218 | pc = 0.1f; | ||
2219 | |||
2220 | m_physCost = pc; | ||
2221 | } | ||
2222 | else | ||
2223 | m_physCost = 0.1f; | ||
2224 | |||
2225 | m_streamCost = 1.0f; | ||
2226 | } | ||
2227 | |||
2228 | #endregion | ||
2229 | |||
2230 | |||
2231 | /// <summary> | ||
2232 | /// Add a child prim to this parent prim. | ||
2233 | /// </summary> | ||
2234 | /// <param name="prim">Child prim</param> | ||
2235 | // I'm the parent | ||
2236 | // prim is the child | ||
2237 | public void ParentPrim(OdePrim prim) | ||
2238 | { | ||
2239 | //Console.WriteLine("ParentPrim " + m_primName); | ||
2240 | if (this.m_localID != prim.m_localID) | ||
2241 | { | ||
2242 | DestroyBody(); // for now we need to rebuil entire object on link change | ||
2243 | |||
2244 | lock (childrenPrim) | ||
2245 | { | ||
2246 | // adopt the prim | ||
2247 | if (!childrenPrim.Contains(prim)) | ||
2248 | childrenPrim.Add(prim); | ||
2249 | |||
2250 | // see if this prim has kids and adopt them also | ||
2251 | // should not happen for now | ||
2252 | foreach (OdePrim prm in prim.childrenPrim) | ||
2253 | { | ||
2254 | if (!childrenPrim.Contains(prm)) | ||
2255 | { | ||
2256 | if (prm.Body != IntPtr.Zero) | ||
2257 | { | ||
2258 | if (prm.prim_geom != IntPtr.Zero) | ||
2259 | d.GeomSetBody(prm.prim_geom, IntPtr.Zero); | ||
2260 | if (prm.Body != prim.Body) | ||
2261 | prm.DestroyBody(); // don't loose bodies around | ||
2262 | prm.Body = IntPtr.Zero; | ||
2263 | } | ||
2264 | |||
2265 | childrenPrim.Add(prm); | ||
2266 | prm._parent = this; | ||
2267 | } | ||
2268 | } | ||
2269 | } | ||
2270 | //Remove old children from the prim | ||
2271 | prim.childrenPrim.Clear(); | ||
2272 | |||
2273 | if (prim.Body != IntPtr.Zero) | ||
2274 | { | ||
2275 | if (prim.prim_geom != IntPtr.Zero) | ||
2276 | d.GeomSetBody(prim.prim_geom, IntPtr.Zero); | ||
2277 | prim.DestroyBody(); // don't loose bodies around | ||
2278 | prim.Body = IntPtr.Zero; | ||
2279 | } | ||
2280 | |||
2281 | prim.childPrim = true; | ||
2282 | prim._parent = this; | ||
2283 | |||
2284 | MakeBody(); // full nasty reconstruction | ||
2285 | } | ||
2286 | } | ||
2287 | |||
2288 | private void UpdateChildsfromgeom() | ||
2289 | { | ||
2290 | if (childrenPrim.Count > 0) | ||
2291 | { | ||
2292 | foreach (OdePrim prm in childrenPrim) | ||
2293 | prm.UpdateDataFromGeom(); | ||
2294 | } | ||
2295 | } | ||
2296 | |||
2297 | private void UpdateDataFromGeom() | ||
2298 | { | ||
2299 | if (prim_geom != IntPtr.Zero) | ||
2300 | { | ||
2301 | d.Quaternion qtmp; | ||
2302 | d.GeomCopyQuaternion(prim_geom, out qtmp); | ||
2303 | _orientation.X = qtmp.X; | ||
2304 | _orientation.Y = qtmp.Y; | ||
2305 | _orientation.Z = qtmp.Z; | ||
2306 | _orientation.W = qtmp.W; | ||
2307 | /* | ||
2308 | // Debug | ||
2309 | float qlen = _orientation.Length(); | ||
2310 | if (qlen > 1.01f || qlen < 0.99) | ||
2311 | m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); | ||
2312 | // | ||
2313 | */ | ||
2314 | _orientation.Normalize(); | ||
2315 | |||
2316 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
2317 | _position.X = lpos.X; | ||
2318 | _position.Y = lpos.Y; | ||
2319 | _position.Z = lpos.Z; | ||
2320 | } | ||
2321 | } | ||
2322 | |||
2323 | private void ChildDelink(OdePrim odePrim, bool remakebodies) | ||
2324 | { | ||
2325 | // Okay, we have a delinked child.. destroy all body and remake | ||
2326 | if (odePrim != this && !childrenPrim.Contains(odePrim)) | ||
2327 | return; | ||
2328 | |||
2329 | DestroyBody(); | ||
2330 | |||
2331 | if (odePrim == this) // delinking the root prim | ||
2332 | { | ||
2333 | OdePrim newroot = null; | ||
2334 | lock (childrenPrim) | ||
2335 | { | ||
2336 | if (childrenPrim.Count > 0) | ||
2337 | { | ||
2338 | newroot = childrenPrim[0]; | ||
2339 | childrenPrim.RemoveAt(0); | ||
2340 | foreach (OdePrim prm in childrenPrim) | ||
2341 | { | ||
2342 | newroot.childrenPrim.Add(prm); | ||
2343 | } | ||
2344 | childrenPrim.Clear(); | ||
2345 | } | ||
2346 | if (newroot != null) | ||
2347 | { | ||
2348 | newroot.childPrim = false; | ||
2349 | newroot._parent = null; | ||
2350 | if (remakebodies) | ||
2351 | newroot.MakeBody(); | ||
2352 | } | ||
2353 | } | ||
2354 | } | ||
2355 | |||
2356 | else | ||
2357 | { | ||
2358 | lock (childrenPrim) | ||
2359 | { | ||
2360 | childrenPrim.Remove(odePrim); | ||
2361 | odePrim.childPrim = false; | ||
2362 | odePrim._parent = null; | ||
2363 | // odePrim.UpdateDataFromGeom(); | ||
2364 | if (remakebodies) | ||
2365 | odePrim.MakeBody(); | ||
2366 | } | ||
2367 | } | ||
2368 | if (remakebodies) | ||
2369 | MakeBody(); | ||
2370 | } | ||
2371 | |||
2372 | protected void ChildRemove(OdePrim odePrim, bool reMakeBody) | ||
2373 | { | ||
2374 | // Okay, we have a delinked child.. destroy all body and remake | ||
2375 | if (odePrim != this && !childrenPrim.Contains(odePrim)) | ||
2376 | return; | ||
2377 | |||
2378 | DestroyBody(); | ||
2379 | |||
2380 | if (odePrim == this) | ||
2381 | { | ||
2382 | OdePrim newroot = null; | ||
2383 | lock (childrenPrim) | ||
2384 | { | ||
2385 | if (childrenPrim.Count > 0) | ||
2386 | { | ||
2387 | newroot = childrenPrim[0]; | ||
2388 | childrenPrim.RemoveAt(0); | ||
2389 | foreach (OdePrim prm in childrenPrim) | ||
2390 | { | ||
2391 | newroot.childrenPrim.Add(prm); | ||
2392 | } | ||
2393 | childrenPrim.Clear(); | ||
2394 | } | ||
2395 | if (newroot != null) | ||
2396 | { | ||
2397 | newroot.childPrim = false; | ||
2398 | newroot._parent = null; | ||
2399 | newroot.MakeBody(); | ||
2400 | } | ||
2401 | } | ||
2402 | if (reMakeBody) | ||
2403 | MakeBody(); | ||
2404 | return; | ||
2405 | } | ||
2406 | else | ||
2407 | { | ||
2408 | lock (childrenPrim) | ||
2409 | { | ||
2410 | childrenPrim.Remove(odePrim); | ||
2411 | odePrim.childPrim = false; | ||
2412 | odePrim._parent = null; | ||
2413 | if (reMakeBody) | ||
2414 | odePrim.MakeBody(); | ||
2415 | } | ||
2416 | } | ||
2417 | MakeBody(); | ||
2418 | } | ||
2419 | |||
2420 | |||
2421 | #region changes | ||
2422 | |||
2423 | private void changeadd() | ||
2424 | { | ||
2425 | } | ||
2426 | |||
2427 | private void changeAngularLock(Vector3 newLock) | ||
2428 | { | ||
2429 | // do we have a Physical object? | ||
2430 | if (Body != IntPtr.Zero) | ||
2431 | { | ||
2432 | //Check that we have a Parent | ||
2433 | //If we have a parent then we're not authorative here | ||
2434 | if (_parent == null) | ||
2435 | { | ||
2436 | if (!newLock.ApproxEquals(Vector3.One, 0f)) | ||
2437 | { | ||
2438 | createAMotor(newLock); | ||
2439 | } | ||
2440 | else | ||
2441 | { | ||
2442 | if (Amotor != IntPtr.Zero) | ||
2443 | { | ||
2444 | d.JointDestroy(Amotor); | ||
2445 | Amotor = IntPtr.Zero; | ||
2446 | } | ||
2447 | } | ||
2448 | } | ||
2449 | } | ||
2450 | // Store this for later in case we get turned into a separate body | ||
2451 | m_angularlock = newLock; | ||
2452 | } | ||
2453 | |||
2454 | private void changeLink(OdePrim NewParent) | ||
2455 | { | ||
2456 | if (_parent == null && NewParent != null) | ||
2457 | { | ||
2458 | NewParent.ParentPrim(this); | ||
2459 | } | ||
2460 | else if (_parent != null) | ||
2461 | { | ||
2462 | if (_parent is OdePrim) | ||
2463 | { | ||
2464 | if (NewParent != _parent) | ||
2465 | { | ||
2466 | (_parent as OdePrim).ChildDelink(this, false); // for now... | ||
2467 | childPrim = false; | ||
2468 | |||
2469 | if (NewParent != null) | ||
2470 | { | ||
2471 | NewParent.ParentPrim(this); | ||
2472 | } | ||
2473 | } | ||
2474 | } | ||
2475 | } | ||
2476 | _parent = NewParent; | ||
2477 | } | ||
2478 | |||
2479 | |||
2480 | private void Stop() | ||
2481 | { | ||
2482 | if (!childPrim) | ||
2483 | { | ||
2484 | // m_force = Vector3.Zero; | ||
2485 | m_forceacc = Vector3.Zero; | ||
2486 | m_angularForceacc = Vector3.Zero; | ||
2487 | // m_torque = Vector3.Zero; | ||
2488 | _velocity = Vector3.Zero; | ||
2489 | _acceleration = Vector3.Zero; | ||
2490 | m_rotationalVelocity = Vector3.Zero; | ||
2491 | _target_velocity = Vector3.Zero; | ||
2492 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
2493 | m_vehicle.Stop(); | ||
2494 | |||
2495 | _zeroFlag = false; | ||
2496 | base.RequestPhysicsterseUpdate(); | ||
2497 | } | ||
2498 | |||
2499 | if (Body != IntPtr.Zero) | ||
2500 | { | ||
2501 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
2502 | d.BodySetTorque(Body, 0f, 0f, 0f); | ||
2503 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
2504 | d.BodySetAngularVel(Body, 0f, 0f, 0f); | ||
2505 | } | ||
2506 | } | ||
2507 | |||
2508 | private void changePhantomStatus(bool newval) | ||
2509 | { | ||
2510 | m_isphantom = newval; | ||
2511 | |||
2512 | UpdateCollisionCatFlags(); | ||
2513 | ApplyCollisionCatFlags(); | ||
2514 | } | ||
2515 | |||
2516 | /* not in use | ||
2517 | internal void ChildSelectedChange(bool childSelect) | ||
2518 | { | ||
2519 | if(childPrim) | ||
2520 | return; | ||
2521 | |||
2522 | if (childSelect == m_isSelected) | ||
2523 | return; | ||
2524 | |||
2525 | if (childSelect) | ||
2526 | { | ||
2527 | DoSelectedStatus(true); | ||
2528 | } | ||
2529 | |||
2530 | else | ||
2531 | { | ||
2532 | foreach (OdePrim prm in childrenPrim) | ||
2533 | { | ||
2534 | if (prm.m_isSelected) | ||
2535 | return; | ||
2536 | } | ||
2537 | DoSelectedStatus(false); | ||
2538 | } | ||
2539 | } | ||
2540 | */ | ||
2541 | private void changeSelectedStatus(bool newval) | ||
2542 | { | ||
2543 | if (m_lastdoneSelected == newval) | ||
2544 | return; | ||
2545 | |||
2546 | m_lastdoneSelected = newval; | ||
2547 | DoSelectedStatus(newval); | ||
2548 | } | ||
2549 | |||
2550 | private void CheckDelaySelect() | ||
2551 | { | ||
2552 | if (m_delaySelect) | ||
2553 | { | ||
2554 | DoSelectedStatus(m_isSelected); | ||
2555 | } | ||
2556 | } | ||
2557 | |||
2558 | private void DoSelectedStatus(bool newval) | ||
2559 | { | ||
2560 | m_isSelected = newval; | ||
2561 | Stop(); | ||
2562 | |||
2563 | if (newval) | ||
2564 | { | ||
2565 | if (!childPrim && Body != IntPtr.Zero) | ||
2566 | d.BodyDisable(Body); | ||
2567 | |||
2568 | if (m_delaySelect || m_isphysical) | ||
2569 | { | ||
2570 | m_collisionCategories = CollisionCategories.Selected; | ||
2571 | m_collisionFlags = 0; | ||
2572 | |||
2573 | if (!childPrim) | ||
2574 | { | ||
2575 | foreach (OdePrim prm in childrenPrim) | ||
2576 | { | ||
2577 | prm.m_collisionCategories = m_collisionCategories; | ||
2578 | prm.m_collisionFlags = m_collisionFlags; | ||
2579 | |||
2580 | if (prm.prim_geom != IntPtr.Zero) | ||
2581 | { | ||
2582 | |||
2583 | if (prm.m_NoColide) | ||
2584 | { | ||
2585 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
2586 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
2587 | } | ||
2588 | else | ||
2589 | { | ||
2590 | d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); | ||
2591 | d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); | ||
2592 | } | ||
2593 | } | ||
2594 | prm.m_delaySelect = false; | ||
2595 | } | ||
2596 | } | ||
2597 | // else if (_parent != null) | ||
2598 | // ((OdePrim)_parent).ChildSelectedChange(true); | ||
2599 | |||
2600 | |||
2601 | if (prim_geom != IntPtr.Zero) | ||
2602 | { | ||
2603 | if (m_NoColide) | ||
2604 | { | ||
2605 | d.GeomSetCategoryBits(prim_geom, 0); | ||
2606 | d.GeomSetCollideBits(prim_geom, 0); | ||
2607 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
2608 | { | ||
2609 | d.GeomSetCategoryBits(collide_geom, 0); | ||
2610 | d.GeomSetCollideBits(collide_geom, 0); | ||
2611 | } | ||
2612 | |||
2613 | } | ||
2614 | else | ||
2615 | { | ||
2616 | d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); | ||
2617 | d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); | ||
2618 | if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) | ||
2619 | { | ||
2620 | d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); | ||
2621 | d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); | ||
2622 | } | ||
2623 | } | ||
2624 | } | ||
2625 | |||
2626 | m_delaySelect = false; | ||
2627 | } | ||
2628 | else if(!m_isphysical) | ||
2629 | { | ||
2630 | m_delaySelect = true; | ||
2631 | } | ||
2632 | } | ||
2633 | else | ||
2634 | { | ||
2635 | if (!childPrim) | ||
2636 | { | ||
2637 | if (Body != IntPtr.Zero && !m_disabled) | ||
2638 | d.BodyEnable(Body); | ||
2639 | } | ||
2640 | // else if (_parent != null) | ||
2641 | // ((OdePrim)_parent).ChildSelectedChange(false); | ||
2642 | |||
2643 | UpdateCollisionCatFlags(); | ||
2644 | ApplyCollisionCatFlags(); | ||
2645 | |||
2646 | m_delaySelect = false; | ||
2647 | } | ||
2648 | |||
2649 | resetCollisionAccounting(); | ||
2650 | } | ||
2651 | |||
2652 | private void changePosition(Vector3 newPos) | ||
2653 | { | ||
2654 | CheckDelaySelect(); | ||
2655 | if (m_isphysical) | ||
2656 | { | ||
2657 | if (childPrim) // inertia is messed, must rebuild | ||
2658 | { | ||
2659 | if (m_building) | ||
2660 | { | ||
2661 | _position = newPos; | ||
2662 | } | ||
2663 | |||
2664 | else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) | ||
2665 | { | ||
2666 | FixInertia(newPos); | ||
2667 | if (!d.BodyIsEnabled(Body)) | ||
2668 | d.BodyEnable(Body); | ||
2669 | } | ||
2670 | } | ||
2671 | else | ||
2672 | { | ||
2673 | if (_position != newPos) | ||
2674 | { | ||
2675 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2676 | _position = newPos; | ||
2677 | } | ||
2678 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2679 | d.BodyEnable(Body); | ||
2680 | } | ||
2681 | } | ||
2682 | else | ||
2683 | { | ||
2684 | if (prim_geom != IntPtr.Zero) | ||
2685 | { | ||
2686 | if (newPos != _position) | ||
2687 | { | ||
2688 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2689 | _position = newPos; | ||
2690 | |||
2691 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); | ||
2692 | } | ||
2693 | } | ||
2694 | } | ||
2695 | givefakepos--; | ||
2696 | if (givefakepos < 0) | ||
2697 | givefakepos = 0; | ||
2698 | // changeSelectedStatus(); | ||
2699 | resetCollisionAccounting(); | ||
2700 | } | ||
2701 | |||
2702 | private void changeOrientation(Quaternion newOri) | ||
2703 | { | ||
2704 | CheckDelaySelect(); | ||
2705 | if (m_isphysical) | ||
2706 | { | ||
2707 | if (childPrim) // inertia is messed, must rebuild | ||
2708 | { | ||
2709 | if (m_building) | ||
2710 | { | ||
2711 | _orientation = newOri; | ||
2712 | } | ||
2713 | /* | ||
2714 | else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) | ||
2715 | { | ||
2716 | FixInertia(_position, newOri); | ||
2717 | if (!d.BodyIsEnabled(Body)) | ||
2718 | d.BodyEnable(Body); | ||
2719 | } | ||
2720 | */ | ||
2721 | } | ||
2722 | else | ||
2723 | { | ||
2724 | if (newOri != _orientation) | ||
2725 | { | ||
2726 | d.Quaternion myrot = new d.Quaternion(); | ||
2727 | myrot.X = newOri.X; | ||
2728 | myrot.Y = newOri.Y; | ||
2729 | myrot.Z = newOri.Z; | ||
2730 | myrot.W = newOri.W; | ||
2731 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2732 | _orientation = newOri; | ||
2733 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2734 | createAMotor(m_angularlock); | ||
2735 | } | ||
2736 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2737 | d.BodyEnable(Body); | ||
2738 | } | ||
2739 | } | ||
2740 | else | ||
2741 | { | ||
2742 | if (prim_geom != IntPtr.Zero) | ||
2743 | { | ||
2744 | if (newOri != _orientation) | ||
2745 | { | ||
2746 | d.Quaternion myrot = new d.Quaternion(); | ||
2747 | myrot.X = newOri.X; | ||
2748 | myrot.Y = newOri.Y; | ||
2749 | myrot.Z = newOri.Z; | ||
2750 | myrot.W = newOri.W; | ||
2751 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2752 | _orientation = newOri; | ||
2753 | } | ||
2754 | } | ||
2755 | } | ||
2756 | givefakeori--; | ||
2757 | if (givefakeori < 0) | ||
2758 | givefakeori = 0; | ||
2759 | resetCollisionAccounting(); | ||
2760 | } | ||
2761 | |||
2762 | private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) | ||
2763 | { | ||
2764 | CheckDelaySelect(); | ||
2765 | if (m_isphysical) | ||
2766 | { | ||
2767 | if (childPrim && m_building) // inertia is messed, must rebuild | ||
2768 | { | ||
2769 | _position = newPos; | ||
2770 | _orientation = newOri; | ||
2771 | } | ||
2772 | else | ||
2773 | { | ||
2774 | if (newOri != _orientation) | ||
2775 | { | ||
2776 | d.Quaternion myrot = new d.Quaternion(); | ||
2777 | myrot.X = newOri.X; | ||
2778 | myrot.Y = newOri.Y; | ||
2779 | myrot.Z = newOri.Z; | ||
2780 | myrot.W = newOri.W; | ||
2781 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2782 | _orientation = newOri; | ||
2783 | if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
2784 | createAMotor(m_angularlock); | ||
2785 | } | ||
2786 | if (_position != newPos) | ||
2787 | { | ||
2788 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2789 | _position = newPos; | ||
2790 | } | ||
2791 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
2792 | d.BodyEnable(Body); | ||
2793 | } | ||
2794 | } | ||
2795 | else | ||
2796 | { | ||
2797 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
2798 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
2799 | |||
2800 | if (prim_geom != IntPtr.Zero) | ||
2801 | { | ||
2802 | if (newOri != _orientation) | ||
2803 | { | ||
2804 | d.Quaternion myrot = new d.Quaternion(); | ||
2805 | myrot.X = newOri.X; | ||
2806 | myrot.Y = newOri.Y; | ||
2807 | myrot.Z = newOri.Z; | ||
2808 | myrot.W = newOri.W; | ||
2809 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2810 | _orientation = newOri; | ||
2811 | } | ||
2812 | |||
2813 | if (newPos != _position) | ||
2814 | { | ||
2815 | d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); | ||
2816 | _position = newPos; | ||
2817 | |||
2818 | m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); | ||
2819 | } | ||
2820 | } | ||
2821 | } | ||
2822 | givefakepos--; | ||
2823 | if (givefakepos < 0) | ||
2824 | givefakepos = 0; | ||
2825 | givefakeori--; | ||
2826 | if (givefakeori < 0) | ||
2827 | givefakeori = 0; | ||
2828 | resetCollisionAccounting(); | ||
2829 | } | ||
2830 | |||
2831 | private void changeDisable(bool disable) | ||
2832 | { | ||
2833 | if (disable) | ||
2834 | { | ||
2835 | if (!m_disabled) | ||
2836 | disableBodySoft(); | ||
2837 | } | ||
2838 | else | ||
2839 | { | ||
2840 | if (m_disabled) | ||
2841 | enableBodySoft(); | ||
2842 | } | ||
2843 | } | ||
2844 | |||
2845 | private void changePhysicsStatus(bool NewStatus) | ||
2846 | { | ||
2847 | CheckDelaySelect(); | ||
2848 | |||
2849 | m_isphysical = NewStatus; | ||
2850 | |||
2851 | if (!childPrim) | ||
2852 | { | ||
2853 | if (NewStatus) | ||
2854 | { | ||
2855 | if (Body == IntPtr.Zero) | ||
2856 | MakeBody(); | ||
2857 | } | ||
2858 | else | ||
2859 | { | ||
2860 | if (Body != IntPtr.Zero) | ||
2861 | { | ||
2862 | DestroyBody(); | ||
2863 | } | ||
2864 | Stop(); | ||
2865 | } | ||
2866 | } | ||
2867 | |||
2868 | resetCollisionAccounting(); | ||
2869 | } | ||
2870 | |||
2871 | private void changeSize(Vector3 newSize) | ||
2872 | { | ||
2873 | } | ||
2874 | |||
2875 | private void changeShape(PrimitiveBaseShape newShape) | ||
2876 | { | ||
2877 | } | ||
2878 | |||
2879 | private void changeAddPhysRep(ODEPhysRepData repData) | ||
2880 | { | ||
2881 | _size = repData.size; //?? | ||
2882 | _pbs = repData.pbs; | ||
2883 | m_shapetype = repData.shapetype; | ||
2884 | |||
2885 | m_mesh = repData.mesh; | ||
2886 | |||
2887 | m_assetID = repData.assetID; | ||
2888 | m_meshState = repData.meshState; | ||
2889 | |||
2890 | m_hasOBB = repData.hasOBB; | ||
2891 | m_OBBOffset = repData.OBBOffset; | ||
2892 | m_OBB = repData.OBB; | ||
2893 | |||
2894 | primVolume = repData.volume; | ||
2895 | |||
2896 | CreateGeom(); | ||
2897 | |||
2898 | if (prim_geom != IntPtr.Zero) | ||
2899 | { | ||
2900 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2901 | d.Quaternion myrot = new d.Quaternion(); | ||
2902 | myrot.X = _orientation.X; | ||
2903 | myrot.Y = _orientation.Y; | ||
2904 | myrot.Z = _orientation.Z; | ||
2905 | myrot.W = _orientation.W; | ||
2906 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2907 | } | ||
2908 | |||
2909 | if (!m_isphysical) | ||
2910 | { | ||
2911 | SetInStaticSpace(this); | ||
2912 | UpdateCollisionCatFlags(); | ||
2913 | ApplyCollisionCatFlags(); | ||
2914 | } | ||
2915 | else | ||
2916 | MakeBody(); | ||
2917 | |||
2918 | if ((m_meshState & MeshState.NeedMask) != 0) | ||
2919 | { | ||
2920 | repData.size = _size; | ||
2921 | repData.pbs = _pbs; | ||
2922 | repData.shapetype = m_shapetype; | ||
2923 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
2924 | } | ||
2925 | } | ||
2926 | |||
2927 | private void changePhysRepData(ODEPhysRepData repData) | ||
2928 | { | ||
2929 | CheckDelaySelect(); | ||
2930 | |||
2931 | OdePrim parent = (OdePrim)_parent; | ||
2932 | |||
2933 | bool chp = childPrim; | ||
2934 | |||
2935 | if (chp) | ||
2936 | { | ||
2937 | if (parent != null) | ||
2938 | { | ||
2939 | parent.DestroyBody(); | ||
2940 | } | ||
2941 | } | ||
2942 | else | ||
2943 | { | ||
2944 | DestroyBody(); | ||
2945 | } | ||
2946 | |||
2947 | RemoveGeom(); | ||
2948 | |||
2949 | _size = repData.size; | ||
2950 | _pbs = repData.pbs; | ||
2951 | m_shapetype = repData.shapetype; | ||
2952 | |||
2953 | m_mesh = repData.mesh; | ||
2954 | |||
2955 | m_assetID = repData.assetID; | ||
2956 | m_meshState = repData.meshState; | ||
2957 | |||
2958 | m_hasOBB = repData.hasOBB; | ||
2959 | m_OBBOffset = repData.OBBOffset; | ||
2960 | m_OBB = repData.OBB; | ||
2961 | |||
2962 | primVolume = repData.volume; | ||
2963 | |||
2964 | CreateGeom(); | ||
2965 | |||
2966 | if (prim_geom != IntPtr.Zero) | ||
2967 | { | ||
2968 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2969 | d.Quaternion myrot = new d.Quaternion(); | ||
2970 | myrot.X = _orientation.X; | ||
2971 | myrot.Y = _orientation.Y; | ||
2972 | myrot.Z = _orientation.Z; | ||
2973 | myrot.W = _orientation.W; | ||
2974 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2975 | } | ||
2976 | |||
2977 | if (m_isphysical) | ||
2978 | { | ||
2979 | if (chp) | ||
2980 | { | ||
2981 | if (parent != null) | ||
2982 | { | ||
2983 | parent.MakeBody(); | ||
2984 | } | ||
2985 | } | ||
2986 | else | ||
2987 | MakeBody(); | ||
2988 | } | ||
2989 | else | ||
2990 | { | ||
2991 | SetInStaticSpace(this); | ||
2992 | UpdateCollisionCatFlags(); | ||
2993 | ApplyCollisionCatFlags(); | ||
2994 | } | ||
2995 | |||
2996 | resetCollisionAccounting(); | ||
2997 | |||
2998 | if ((m_meshState & MeshState.NeedMask) != 0) | ||
2999 | { | ||
3000 | repData.size = _size; | ||
3001 | repData.pbs = _pbs; | ||
3002 | repData.shapetype = m_shapetype; | ||
3003 | _parent_scene.m_meshWorker.RequestMesh(repData); | ||
3004 | } | ||
3005 | } | ||
3006 | |||
3007 | private void changeFloatOnWater(bool newval) | ||
3008 | { | ||
3009 | m_collidesWater = newval; | ||
3010 | |||
3011 | UpdateCollisionCatFlags(); | ||
3012 | ApplyCollisionCatFlags(); | ||
3013 | } | ||
3014 | |||
3015 | private void changeSetTorque(Vector3 newtorque) | ||
3016 | { | ||
3017 | if (!m_isSelected) | ||
3018 | { | ||
3019 | if (m_isphysical && Body != IntPtr.Zero) | ||
3020 | { | ||
3021 | if (m_disabled) | ||
3022 | enableBodySoft(); | ||
3023 | else if (!d.BodyIsEnabled(Body)) | ||
3024 | d.BodyEnable(Body); | ||
3025 | |||
3026 | } | ||
3027 | m_torque = newtorque; | ||
3028 | } | ||
3029 | } | ||
3030 | |||
3031 | private void changeForce(Vector3 force) | ||
3032 | { | ||
3033 | m_force = force; | ||
3034 | if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) | ||
3035 | d.BodyEnable(Body); | ||
3036 | } | ||
3037 | |||
3038 | private void changeAddForce(Vector3 theforce) | ||
3039 | { | ||
3040 | m_forceacc += theforce; | ||
3041 | if (!m_isSelected) | ||
3042 | { | ||
3043 | lock (this) | ||
3044 | { | ||
3045 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
3046 | if (m_isphysical && Body != IntPtr.Zero) | ||
3047 | { | ||
3048 | if (m_disabled) | ||
3049 | enableBodySoft(); | ||
3050 | else if (!d.BodyIsEnabled(Body)) | ||
3051 | d.BodyEnable(Body); | ||
3052 | } | ||
3053 | } | ||
3054 | m_collisionscore = 0; | ||
3055 | } | ||
3056 | } | ||
3057 | |||
3058 | // actually angular impulse | ||
3059 | private void changeAddAngularImpulse(Vector3 aimpulse) | ||
3060 | { | ||
3061 | m_angularForceacc += aimpulse * m_invTimeStep; | ||
3062 | if (!m_isSelected) | ||
3063 | { | ||
3064 | lock (this) | ||
3065 | { | ||
3066 | if (m_isphysical && Body != IntPtr.Zero) | ||
3067 | { | ||
3068 | if (m_disabled) | ||
3069 | enableBodySoft(); | ||
3070 | else if (!d.BodyIsEnabled(Body)) | ||
3071 | d.BodyEnable(Body); | ||
3072 | } | ||
3073 | } | ||
3074 | m_collisionscore = 0; | ||
3075 | } | ||
3076 | } | ||
3077 | |||
3078 | private void changevelocity(Vector3 newVel) | ||
3079 | { | ||
3080 | float len = newVel.LengthSquared(); | ||
3081 | if (len > 100000.0f) // limit to 100m/s | ||
3082 | { | ||
3083 | len = 100.0f / (float)Math.Sqrt(len); | ||
3084 | newVel *= len; | ||
3085 | } | ||
3086 | |||
3087 | if (!m_isSelected) | ||
3088 | { | ||
3089 | if (Body != IntPtr.Zero) | ||
3090 | { | ||
3091 | if (m_disabled) | ||
3092 | enableBodySoft(); | ||
3093 | else if (!d.BodyIsEnabled(Body)) | ||
3094 | d.BodyEnable(Body); | ||
3095 | |||
3096 | d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); | ||
3097 | } | ||
3098 | //resetCollisionAccounting(); | ||
3099 | } | ||
3100 | _velocity = newVel; | ||
3101 | } | ||
3102 | |||
3103 | private void changeangvelocity(Vector3 newAngVel) | ||
3104 | { | ||
3105 | float len = newAngVel.LengthSquared(); | ||
3106 | if (len > 144.0f) // limit to 12rad/s | ||
3107 | { | ||
3108 | len = 12.0f / (float)Math.Sqrt(len); | ||
3109 | newAngVel *= len; | ||
3110 | } | ||
3111 | |||
3112 | if (!m_isSelected) | ||
3113 | { | ||
3114 | if (Body != IntPtr.Zero) | ||
3115 | { | ||
3116 | if (m_disabled) | ||
3117 | enableBodySoft(); | ||
3118 | else if (!d.BodyIsEnabled(Body)) | ||
3119 | d.BodyEnable(Body); | ||
3120 | |||
3121 | |||
3122 | d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); | ||
3123 | } | ||
3124 | //resetCollisionAccounting(); | ||
3125 | } | ||
3126 | m_rotationalVelocity = newAngVel; | ||
3127 | } | ||
3128 | |||
3129 | private void changeVolumedetetion(bool newVolDtc) | ||
3130 | { | ||
3131 | m_isVolumeDetect = newVolDtc; | ||
3132 | m_fakeisVolumeDetect = newVolDtc; | ||
3133 | UpdateCollisionCatFlags(); | ||
3134 | ApplyCollisionCatFlags(); | ||
3135 | } | ||
3136 | |||
3137 | protected void changeBuilding(bool newbuilding) | ||
3138 | { | ||
3139 | // Check if we need to do anything | ||
3140 | if (newbuilding == m_building) | ||
3141 | return; | ||
3142 | |||
3143 | if ((bool)newbuilding) | ||
3144 | { | ||
3145 | m_building = true; | ||
3146 | if (!childPrim) | ||
3147 | DestroyBody(); | ||
3148 | } | ||
3149 | else | ||
3150 | { | ||
3151 | m_building = false; | ||
3152 | CheckDelaySelect(); | ||
3153 | if (!childPrim) | ||
3154 | MakeBody(); | ||
3155 | } | ||
3156 | if (!childPrim && childrenPrim.Count > 0) | ||
3157 | { | ||
3158 | foreach (OdePrim prm in childrenPrim) | ||
3159 | prm.changeBuilding(m_building); // call directly | ||
3160 | } | ||
3161 | } | ||
3162 | |||
3163 | public void changeSetVehicle(VehicleData vdata) | ||
3164 | { | ||
3165 | if (m_vehicle == null) | ||
3166 | m_vehicle = new ODEDynamics(this); | ||
3167 | m_vehicle.DoSetVehicle(vdata); | ||
3168 | } | ||
3169 | |||
3170 | private void changeVehicleType(int value) | ||
3171 | { | ||
3172 | if (value == (int)Vehicle.TYPE_NONE) | ||
3173 | { | ||
3174 | if (m_vehicle != null) | ||
3175 | m_vehicle = null; | ||
3176 | } | ||
3177 | else | ||
3178 | { | ||
3179 | if (m_vehicle == null) | ||
3180 | m_vehicle = new ODEDynamics(this); | ||
3181 | |||
3182 | m_vehicle.ProcessTypeChange((Vehicle)value); | ||
3183 | } | ||
3184 | } | ||
3185 | |||
3186 | private void changeVehicleFloatParam(strVehicleFloatParam fp) | ||
3187 | { | ||
3188 | if (m_vehicle == null) | ||
3189 | return; | ||
3190 | |||
3191 | m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); | ||
3192 | } | ||
3193 | |||
3194 | private void changeVehicleVectorParam(strVehicleVectorParam vp) | ||
3195 | { | ||
3196 | if (m_vehicle == null) | ||
3197 | return; | ||
3198 | m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); | ||
3199 | } | ||
3200 | |||
3201 | private void changeVehicleRotationParam(strVehicleQuatParam qp) | ||
3202 | { | ||
3203 | if (m_vehicle == null) | ||
3204 | return; | ||
3205 | m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); | ||
3206 | } | ||
3207 | |||
3208 | private void changeVehicleFlags(strVehicleBoolParam bp) | ||
3209 | { | ||
3210 | if (m_vehicle == null) | ||
3211 | return; | ||
3212 | m_vehicle.ProcessVehicleFlags(bp.param, bp.value); | ||
3213 | } | ||
3214 | |||
3215 | private void changeBuoyancy(float b) | ||
3216 | { | ||
3217 | m_buoyancy = b; | ||
3218 | } | ||
3219 | |||
3220 | private void changePIDTarget(Vector3 trg) | ||
3221 | { | ||
3222 | m_PIDTarget = trg; | ||
3223 | } | ||
3224 | |||
3225 | private void changePIDTau(float tau) | ||
3226 | { | ||
3227 | m_PIDTau = tau; | ||
3228 | } | ||
3229 | |||
3230 | private void changePIDActive(bool val) | ||
3231 | { | ||
3232 | m_usePID = val; | ||
3233 | } | ||
3234 | |||
3235 | private void changePIDHoverHeight(float val) | ||
3236 | { | ||
3237 | m_PIDHoverHeight = val; | ||
3238 | if (val == 0) | ||
3239 | m_useHoverPID = false; | ||
3240 | } | ||
3241 | |||
3242 | private void changePIDHoverType(PIDHoverType type) | ||
3243 | { | ||
3244 | m_PIDHoverType = type; | ||
3245 | } | ||
3246 | |||
3247 | private void changePIDHoverTau(float tau) | ||
3248 | { | ||
3249 | m_PIDHoverTau = tau; | ||
3250 | } | ||
3251 | |||
3252 | private void changePIDHoverActive(bool active) | ||
3253 | { | ||
3254 | m_useHoverPID = active; | ||
3255 | } | ||
3256 | |||
3257 | #endregion | ||
3258 | |||
3259 | public void Move() | ||
3260 | { | ||
3261 | if (!childPrim && m_isphysical && Body != IntPtr.Zero && | ||
3262 | !m_disabled && !m_isSelected && !m_building && !m_outbounds) | ||
3263 | { | ||
3264 | if (!d.BodyIsEnabled(Body)) | ||
3265 | { | ||
3266 | // let vehicles sleep | ||
3267 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
3268 | return; | ||
3269 | |||
3270 | if (++bodydisablecontrol < 20) | ||
3271 | return; | ||
3272 | |||
3273 | d.BodyEnable(Body); | ||
3274 | } | ||
3275 | |||
3276 | bodydisablecontrol = 0; | ||
3277 | |||
3278 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator | ||
3279 | |||
3280 | if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) | ||
3281 | { | ||
3282 | // 'VEHICLES' are dealt with in ODEDynamics.cs | ||
3283 | m_vehicle.Step(); | ||
3284 | return; | ||
3285 | } | ||
3286 | |||
3287 | float fx = 0; | ||
3288 | float fy = 0; | ||
3289 | float fz = 0; | ||
3290 | |||
3291 | float m_mass = _mass; | ||
3292 | |||
3293 | if (m_usePID && m_PIDTau > 0) | ||
3294 | { | ||
3295 | // for now position error | ||
3296 | _target_velocity = | ||
3297 | new Vector3( | ||
3298 | (m_PIDTarget.X - lpos.X), | ||
3299 | (m_PIDTarget.Y - lpos.Y), | ||
3300 | (m_PIDTarget.Z - lpos.Z) | ||
3301 | ); | ||
3302 | |||
3303 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) | ||
3304 | { | ||
3305 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
3306 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
3307 | return; | ||
3308 | } | ||
3309 | else | ||
3310 | { | ||
3311 | _zeroFlag = false; | ||
3312 | |||
3313 | float tmp = 1 / m_PIDTau; | ||
3314 | _target_velocity *= tmp; | ||
3315 | |||
3316 | // apply limits | ||
3317 | tmp = _target_velocity.Length(); | ||
3318 | if (tmp > 50.0f) | ||
3319 | { | ||
3320 | tmp = 50 / tmp; | ||
3321 | _target_velocity *= tmp; | ||
3322 | } | ||
3323 | else if (tmp < 0.05f) | ||
3324 | { | ||
3325 | tmp = 0.05f / tmp; | ||
3326 | _target_velocity *= tmp; | ||
3327 | } | ||
3328 | |||
3329 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3330 | fx = (_target_velocity.X - vel.X) * m_invTimeStep; | ||
3331 | fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; | ||
3332 | fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; | ||
3333 | // d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3334 | } | ||
3335 | } // end if (m_usePID) | ||
3336 | |||
3337 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | ||
3338 | else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) | ||
3339 | { | ||
3340 | |||
3341 | // Non-Vehicles have a limited set of Hover options. | ||
3342 | // determine what our target height really is based on HoverType | ||
3343 | |||
3344 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); | ||
3345 | |||
3346 | switch (m_PIDHoverType) | ||
3347 | { | ||
3348 | case PIDHoverType.Ground: | ||
3349 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3350 | break; | ||
3351 | |||
3352 | case PIDHoverType.GroundAndWater: | ||
3353 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
3354 | if (m_groundHeight > m_waterHeight) | ||
3355 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3356 | else | ||
3357 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
3358 | break; | ||
3359 | } // end switch (m_PIDHoverType) | ||
3360 | |||
3361 | // don't go underground unless volumedetector | ||
3362 | |||
3363 | if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) | ||
3364 | { | ||
3365 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3366 | |||
3367 | fz = (m_targetHoverHeight - lpos.Z); | ||
3368 | |||
3369 | // if error is zero, use position control; otherwise, velocity control | ||
3370 | if (Math.Abs(fz) < 0.01f) | ||
3371 | { | ||
3372 | d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); | ||
3373 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | ||
3374 | } | ||
3375 | else | ||
3376 | { | ||
3377 | _zeroFlag = false; | ||
3378 | fz /= m_PIDHoverTau; | ||
3379 | |||
3380 | float tmp = Math.Abs(fz); | ||
3381 | if (tmp > 50) | ||
3382 | fz = 50 * Math.Sign(fz); | ||
3383 | else if (tmp < 0.1) | ||
3384 | fz = 0.1f * Math.Sign(fz); | ||
3385 | |||
3386 | fz = ((fz - vel.Z) * m_invTimeStep); | ||
3387 | } | ||
3388 | } | ||
3389 | } | ||
3390 | else | ||
3391 | { | ||
3392 | float b = (1.0f - m_buoyancy) * m_gravmod; | ||
3393 | fx = _parent_scene.gravityx * b; | ||
3394 | fy = _parent_scene.gravityy * b; | ||
3395 | fz = _parent_scene.gravityz * b; | ||
3396 | } | ||
3397 | |||
3398 | fx *= m_mass; | ||
3399 | fy *= m_mass; | ||
3400 | fz *= m_mass; | ||
3401 | |||
3402 | // constant force | ||
3403 | fx += m_force.X; | ||
3404 | fy += m_force.Y; | ||
3405 | fz += m_force.Z; | ||
3406 | |||
3407 | fx += m_forceacc.X; | ||
3408 | fy += m_forceacc.Y; | ||
3409 | fz += m_forceacc.Z; | ||
3410 | |||
3411 | m_forceacc = Vector3.Zero; | ||
3412 | |||
3413 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
3414 | if (fx != 0 || fy != 0 || fz != 0) | ||
3415 | { | ||
3416 | d.BodyAddForce(Body, fx, fy, fz); | ||
3417 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | ||
3418 | } | ||
3419 | |||
3420 | Vector3 trq; | ||
3421 | |||
3422 | trq = m_torque; | ||
3423 | trq += m_angularForceacc; | ||
3424 | m_angularForceacc = Vector3.Zero; | ||
3425 | if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) | ||
3426 | { | ||
3427 | d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); | ||
3428 | } | ||
3429 | } | ||
3430 | else | ||
3431 | { // is not physical, or is not a body or is selected | ||
3432 | // _zeroPosition = d.BodyGetPosition(Body); | ||
3433 | return; | ||
3434 | //Console.WriteLine("Nothing " + Name); | ||
3435 | |||
3436 | } | ||
3437 | } | ||
3438 | |||
3439 | public void UpdatePositionAndVelocity(int frame) | ||
3440 | { | ||
3441 | if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) | ||
3442 | { | ||
3443 | bool bodyenabled = d.BodyIsEnabled(Body); | ||
3444 | if (bodyenabled || !_zeroFlag) | ||
3445 | { | ||
3446 | bool lastZeroFlag = _zeroFlag; | ||
3447 | |||
3448 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
3449 | |||
3450 | // check outside region | ||
3451 | if (lpos.Z < -100 || lpos.Z > 100000f) | ||
3452 | { | ||
3453 | m_outbounds = true; | ||
3454 | |||
3455 | lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); | ||
3456 | _acceleration.X = 0; | ||
3457 | _acceleration.Y = 0; | ||
3458 | _acceleration.Z = 0; | ||
3459 | |||
3460 | _velocity.X = 0; | ||
3461 | _velocity.Y = 0; | ||
3462 | _velocity.Z = 0; | ||
3463 | m_rotationalVelocity.X = 0; | ||
3464 | m_rotationalVelocity.Y = 0; | ||
3465 | m_rotationalVelocity.Z = 0; | ||
3466 | |||
3467 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
3468 | d.BodySetAngularVel(Body, 0, 0, 0); // stop it | ||
3469 | d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere | ||
3470 | m_lastposition = _position; | ||
3471 | m_lastorientation = _orientation; | ||
3472 | |||
3473 | base.RequestPhysicsterseUpdate(); | ||
3474 | |||
3475 | // throttleCounter = 0; | ||
3476 | _zeroFlag = true; | ||
3477 | |||
3478 | disableBodySoft(); // disable it and colisions | ||
3479 | base.RaiseOutOfBounds(_position); | ||
3480 | return; | ||
3481 | } | ||
3482 | |||
3483 | if (lpos.X < 0f) | ||
3484 | { | ||
3485 | _position.X = Util.Clip(lpos.X, -2f, -0.1f); | ||
3486 | m_outbounds = true; | ||
3487 | } | ||
3488 | else if (lpos.X > _parent_scene.WorldExtents.X) | ||
3489 | { | ||
3490 | _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); | ||
3491 | m_outbounds = true; | ||
3492 | } | ||
3493 | if (lpos.Y < 0f) | ||
3494 | { | ||
3495 | _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); | ||
3496 | m_outbounds = true; | ||
3497 | } | ||
3498 | else if (lpos.Y > _parent_scene.WorldExtents.Y) | ||
3499 | { | ||
3500 | _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); | ||
3501 | m_outbounds = true; | ||
3502 | } | ||
3503 | |||
3504 | if (m_outbounds) | ||
3505 | { | ||
3506 | m_lastposition = _position; | ||
3507 | m_lastorientation = _orientation; | ||
3508 | |||
3509 | d.Vector3 dtmp = d.BodyGetAngularVel(Body); | ||
3510 | m_rotationalVelocity.X = dtmp.X; | ||
3511 | m_rotationalVelocity.Y = dtmp.Y; | ||
3512 | m_rotationalVelocity.Z = dtmp.Z; | ||
3513 | |||
3514 | dtmp = d.BodyGetLinearVel(Body); | ||
3515 | _velocity.X = dtmp.X; | ||
3516 | _velocity.Y = dtmp.Y; | ||
3517 | _velocity.Z = dtmp.Z; | ||
3518 | |||
3519 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
3520 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
3521 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
3522 | disableBodySoft(); // stop collisions | ||
3523 | UnSubscribeEvents(); | ||
3524 | |||
3525 | base.RequestPhysicsterseUpdate(); | ||
3526 | return; | ||
3527 | } | ||
3528 | |||
3529 | d.Quaternion ori; | ||
3530 | d.GeomCopyQuaternion(prim_geom, out ori); | ||
3531 | |||
3532 | // decide if moving | ||
3533 | // use positions since this are integrated quantities | ||
3534 | // tolerance values depende a lot on simulation noise... | ||
3535 | // use simple math.abs since we dont need to be exact | ||
3536 | |||
3537 | if (!bodyenabled || | ||
3538 | (Math.Abs(_position.X - lpos.X) < 0.005f) | ||
3539 | && (Math.Abs(_position.Y - lpos.Y) < 0.005f) | ||
3540 | && (Math.Abs(_position.Z - lpos.Z) < 0.005f) | ||
3541 | && (Math.Abs(_orientation.X - ori.X) < 0.0005f) | ||
3542 | && (Math.Abs(_orientation.Y - ori.Y) < 0.0005f) | ||
3543 | && (Math.Abs(_orientation.Z - ori.Z) < 0.0005f) // ignore W | ||
3544 | ) | ||
3545 | { | ||
3546 | _zeroFlag = true; | ||
3547 | } | ||
3548 | else | ||
3549 | _zeroFlag = false; | ||
3550 | |||
3551 | // update velocities and aceleration | ||
3552 | if (!(_zeroFlag && lastZeroFlag)) | ||
3553 | { | ||
3554 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3555 | |||
3556 | _acceleration = _velocity; | ||
3557 | |||
3558 | if ((Math.Abs(vel.X) < 0.005f) && | ||
3559 | (Math.Abs(vel.Y) < 0.005f) && | ||
3560 | (Math.Abs(vel.Z) < 0.005f)) | ||
3561 | { | ||
3562 | _velocity = Vector3.Zero; | ||
3563 | float t = -m_invTimeStep; | ||
3564 | _acceleration = _acceleration * t; | ||
3565 | } | ||
3566 | else | ||
3567 | { | ||
3568 | _velocity.X = vel.X; | ||
3569 | _velocity.Y = vel.Y; | ||
3570 | _velocity.Z = vel.Z; | ||
3571 | _acceleration = (_velocity - _acceleration) * m_invTimeStep; | ||
3572 | } | ||
3573 | |||
3574 | if ((Math.Abs(_acceleration.X) < 0.01f) && | ||
3575 | (Math.Abs(_acceleration.Y) < 0.01f) && | ||
3576 | (Math.Abs(_acceleration.Z) < 0.01f)) | ||
3577 | { | ||
3578 | _acceleration = Vector3.Zero; | ||
3579 | } | ||
3580 | |||
3581 | if ((Math.Abs(_orientation.X - ori.X) < 0.0001) && | ||
3582 | (Math.Abs(_orientation.Y - ori.Y) < 0.0001) && | ||
3583 | (Math.Abs(_orientation.Z - ori.Z) < 0.0001) | ||
3584 | ) | ||
3585 | { | ||
3586 | m_rotationalVelocity = Vector3.Zero; | ||
3587 | } | ||
3588 | else | ||
3589 | { | ||
3590 | vel = d.BodyGetAngularVel(Body); | ||
3591 | m_rotationalVelocity.X = vel.X; | ||
3592 | m_rotationalVelocity.Y = vel.Y; | ||
3593 | m_rotationalVelocity.Z = vel.Z; | ||
3594 | } | ||
3595 | // } | ||
3596 | |||
3597 | _position.X = lpos.X; | ||
3598 | _position.Y = lpos.Y; | ||
3599 | _position.Z = lpos.Z; | ||
3600 | |||
3601 | _orientation.X = ori.X; | ||
3602 | _orientation.Y = ori.Y; | ||
3603 | _orientation.Z = ori.Z; | ||
3604 | _orientation.W = ori.W; | ||
3605 | } | ||
3606 | if (_zeroFlag) | ||
3607 | { | ||
3608 | if (lastZeroFlag) | ||
3609 | { | ||
3610 | _velocity = Vector3.Zero; | ||
3611 | _acceleration = Vector3.Zero; | ||
3612 | m_rotationalVelocity = Vector3.Zero; | ||
3613 | } | ||
3614 | |||
3615 | if (!m_lastUpdateSent) | ||
3616 | { | ||
3617 | base.RequestPhysicsterseUpdate(); | ||
3618 | if (lastZeroFlag) | ||
3619 | m_lastUpdateSent = true; | ||
3620 | } | ||
3621 | return; | ||
3622 | } | ||
3623 | |||
3624 | base.RequestPhysicsterseUpdate(); | ||
3625 | m_lastUpdateSent = false; | ||
3626 | } | ||
3627 | } | ||
3628 | } | ||
3629 | |||
3630 | internal static bool QuaternionIsFinite(Quaternion q) | ||
3631 | { | ||
3632 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
3633 | return false; | ||
3634 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
3635 | return false; | ||
3636 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
3637 | return false; | ||
3638 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
3639 | return false; | ||
3640 | return true; | ||
3641 | } | ||
3642 | |||
3643 | internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) | ||
3644 | { | ||
3645 | // assumes object center of mass is zero | ||
3646 | float smass = part.mass; | ||
3647 | theobj.mass -= smass; | ||
3648 | |||
3649 | smass *= 1.0f / (theobj.mass); ; | ||
3650 | |||
3651 | theobj.c.X -= part.c.X * smass; | ||
3652 | theobj.c.Y -= part.c.Y * smass; | ||
3653 | theobj.c.Z -= part.c.Z * smass; | ||
3654 | |||
3655 | theobj.I.M00 -= part.I.M00; | ||
3656 | theobj.I.M01 -= part.I.M01; | ||
3657 | theobj.I.M02 -= part.I.M02; | ||
3658 | theobj.I.M10 -= part.I.M10; | ||
3659 | theobj.I.M11 -= part.I.M11; | ||
3660 | theobj.I.M12 -= part.I.M12; | ||
3661 | theobj.I.M20 -= part.I.M20; | ||
3662 | theobj.I.M21 -= part.I.M21; | ||
3663 | theobj.I.M22 -= part.I.M22; | ||
3664 | } | ||
3665 | |||
3666 | private void donullchange() | ||
3667 | { | ||
3668 | } | ||
3669 | |||
3670 | public bool DoAChange(changes what, object arg) | ||
3671 | { | ||
3672 | if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) | ||
3673 | { | ||
3674 | return false; | ||
3675 | } | ||
3676 | |||
3677 | // nasty switch | ||
3678 | switch (what) | ||
3679 | { | ||
3680 | case changes.Add: | ||
3681 | changeadd(); | ||
3682 | break; | ||
3683 | |||
3684 | case changes.AddPhysRep: | ||
3685 | changeAddPhysRep((ODEPhysRepData)arg); | ||
3686 | break; | ||
3687 | |||
3688 | case changes.Remove: | ||
3689 | //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... | ||
3690 | //When we return true, it destroys all of the prims in the linkset anyway | ||
3691 | if (_parent != null) | ||
3692 | { | ||
3693 | OdePrim parent = (OdePrim)_parent; | ||
3694 | parent.ChildRemove(this, false); | ||
3695 | } | ||
3696 | else | ||
3697 | ChildRemove(this, false); | ||
3698 | |||
3699 | m_vehicle = null; | ||
3700 | RemoveGeom(); | ||
3701 | m_targetSpace = IntPtr.Zero; | ||
3702 | UnSubscribeEvents(); | ||
3703 | return true; | ||
3704 | |||
3705 | case changes.Link: | ||
3706 | OdePrim tmp = (OdePrim)arg; | ||
3707 | changeLink(tmp); | ||
3708 | break; | ||
3709 | |||
3710 | case changes.DeLink: | ||
3711 | changeLink(null); | ||
3712 | break; | ||
3713 | |||
3714 | case changes.Position: | ||
3715 | changePosition((Vector3)arg); | ||
3716 | break; | ||
3717 | |||
3718 | case changes.Orientation: | ||
3719 | changeOrientation((Quaternion)arg); | ||
3720 | break; | ||
3721 | |||
3722 | case changes.PosOffset: | ||
3723 | donullchange(); | ||
3724 | break; | ||
3725 | |||
3726 | case changes.OriOffset: | ||
3727 | donullchange(); | ||
3728 | break; | ||
3729 | |||
3730 | case changes.Velocity: | ||
3731 | changevelocity((Vector3)arg); | ||
3732 | break; | ||
3733 | |||
3734 | // case changes.Acceleration: | ||
3735 | // changeacceleration((Vector3)arg); | ||
3736 | // break; | ||
3737 | |||
3738 | case changes.AngVelocity: | ||
3739 | changeangvelocity((Vector3)arg); | ||
3740 | break; | ||
3741 | |||
3742 | case changes.Force: | ||
3743 | changeForce((Vector3)arg); | ||
3744 | break; | ||
3745 | |||
3746 | case changes.Torque: | ||
3747 | changeSetTorque((Vector3)arg); | ||
3748 | break; | ||
3749 | |||
3750 | case changes.AddForce: | ||
3751 | changeAddForce((Vector3)arg); | ||
3752 | break; | ||
3753 | |||
3754 | case changes.AddAngForce: | ||
3755 | changeAddAngularImpulse((Vector3)arg); | ||
3756 | break; | ||
3757 | |||
3758 | case changes.AngLock: | ||
3759 | changeAngularLock((Vector3)arg); | ||
3760 | break; | ||
3761 | |||
3762 | case changes.Size: | ||
3763 | changeSize((Vector3)arg); | ||
3764 | break; | ||
3765 | |||
3766 | case changes.Shape: | ||
3767 | changeShape((PrimitiveBaseShape)arg); | ||
3768 | break; | ||
3769 | |||
3770 | case changes.PhysRepData: | ||
3771 | changePhysRepData((ODEPhysRepData) arg); | ||
3772 | break; | ||
3773 | |||
3774 | case changes.CollidesWater: | ||
3775 | changeFloatOnWater((bool)arg); | ||
3776 | break; | ||
3777 | |||
3778 | case changes.VolumeDtc: | ||
3779 | changeVolumedetetion((bool)arg); | ||
3780 | break; | ||
3781 | |||
3782 | case changes.Phantom: | ||
3783 | changePhantomStatus((bool)arg); | ||
3784 | break; | ||
3785 | |||
3786 | case changes.Physical: | ||
3787 | changePhysicsStatus((bool)arg); | ||
3788 | break; | ||
3789 | |||
3790 | case changes.Selected: | ||
3791 | changeSelectedStatus((bool)arg); | ||
3792 | break; | ||
3793 | |||
3794 | case changes.disabled: | ||
3795 | changeDisable((bool)arg); | ||
3796 | break; | ||
3797 | |||
3798 | case changes.building: | ||
3799 | changeBuilding((bool)arg); | ||
3800 | break; | ||
3801 | |||
3802 | case changes.VehicleType: | ||
3803 | changeVehicleType((int)arg); | ||
3804 | break; | ||
3805 | |||
3806 | case changes.VehicleFlags: | ||
3807 | changeVehicleFlags((strVehicleBoolParam) arg); | ||
3808 | break; | ||
3809 | |||
3810 | case changes.VehicleFloatParam: | ||
3811 | changeVehicleFloatParam((strVehicleFloatParam) arg); | ||
3812 | break; | ||
3813 | |||
3814 | case changes.VehicleVectorParam: | ||
3815 | changeVehicleVectorParam((strVehicleVectorParam) arg); | ||
3816 | break; | ||
3817 | |||
3818 | case changes.VehicleRotationParam: | ||
3819 | changeVehicleRotationParam((strVehicleQuatParam) arg); | ||
3820 | break; | ||
3821 | |||
3822 | case changes.SetVehicle: | ||
3823 | changeSetVehicle((VehicleData) arg); | ||
3824 | break; | ||
3825 | |||
3826 | case changes.Buoyancy: | ||
3827 | changeBuoyancy((float)arg); | ||
3828 | break; | ||
3829 | |||
3830 | case changes.PIDTarget: | ||
3831 | changePIDTarget((Vector3)arg); | ||
3832 | break; | ||
3833 | |||
3834 | case changes.PIDTau: | ||
3835 | changePIDTau((float)arg); | ||
3836 | break; | ||
3837 | |||
3838 | case changes.PIDActive: | ||
3839 | changePIDActive((bool)arg); | ||
3840 | break; | ||
3841 | |||
3842 | case changes.PIDHoverHeight: | ||
3843 | changePIDHoverHeight((float)arg); | ||
3844 | break; | ||
3845 | |||
3846 | case changes.PIDHoverType: | ||
3847 | changePIDHoverType((PIDHoverType)arg); | ||
3848 | break; | ||
3849 | |||
3850 | case changes.PIDHoverTau: | ||
3851 | changePIDHoverTau((float)arg); | ||
3852 | break; | ||
3853 | |||
3854 | case changes.PIDHoverActive: | ||
3855 | changePIDHoverActive((bool)arg); | ||
3856 | break; | ||
3857 | |||
3858 | case changes.Null: | ||
3859 | donullchange(); | ||
3860 | break; | ||
3861 | |||
3862 | |||
3863 | |||
3864 | default: | ||
3865 | donullchange(); | ||
3866 | break; | ||
3867 | } | ||
3868 | return false; | ||
3869 | } | ||
3870 | |||
3871 | public void AddChange(changes what, object arg) | ||
3872 | { | ||
3873 | _parent_scene.AddChange((PhysicsActor) this, what, arg); | ||
3874 | } | ||
3875 | |||
3876 | |||
3877 | private struct strVehicleBoolParam | ||
3878 | { | ||
3879 | public int param; | ||
3880 | public bool value; | ||
3881 | } | ||
3882 | |||
3883 | private struct strVehicleFloatParam | ||
3884 | { | ||
3885 | public int param; | ||
3886 | public float value; | ||
3887 | } | ||
3888 | |||
3889 | private struct strVehicleQuatParam | ||
3890 | { | ||
3891 | public int param; | ||
3892 | public Quaternion value; | ||
3893 | } | ||
3894 | |||
3895 | private struct strVehicleVectorParam | ||
3896 | { | ||
3897 | public int param; | ||
3898 | public Vector3 value; | ||
3899 | } | ||
3900 | } | ||
3901 | } | ||