diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 3825 |
1 files changed, 3825 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs new file mode 100644 index 0000000..16e90ff --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | |||
@@ -0,0 +1,3825 @@ | |||
1 | /* Copyright (c) Contributors, http://opensimulator.org/ | ||
2 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
3 | * Redistribution and use in source and binary forms, with or without | ||
4 | * modification, are permitted provided that the following conditions are met: | ||
5 | * * Redistributions of source code must retain the above copyright | ||
6 | * notice, this list of conditions and the following disclaimer. | ||
7 | * * Redistributions in binary form must reproduce the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer in the | ||
9 | * documentation and/or other materials provided with the distribution. | ||
10 | * * Neither the name of the OpenSimulator Project nor the | ||
11 | * names of its contributors may be used to endorse or promote products | ||
12 | * derived from this software without specific prior written permission. | ||
13 | * | ||
14 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
17 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
24 | * | ||
25 | * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs | ||
26 | * rolled into ODEPrim.cs | ||
27 | */ | ||
28 | |||
29 | using System; | ||
30 | using System.Collections.Generic; | ||
31 | using System.Reflection; | ||
32 | using System.Runtime.InteropServices; | ||
33 | using System.Threading; | ||
34 | using log4net; | ||
35 | using OpenMetaverse; | ||
36 | using Ode.NET; | ||
37 | using OpenSim.Framework; | ||
38 | using OpenSim.Region.Physics.Manager; | ||
39 | |||
40 | |||
41 | namespace OpenSim.Region.Physics.OdePlugin | ||
42 | { | ||
43 | /// <summary> | ||
44 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
45 | /// </summary> | ||
46 | |||
47 | public class OdePrim : PhysicsActor | ||
48 | { | ||
49 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
50 | |||
51 | private Vector3 _position; | ||
52 | private Vector3 _velocity; | ||
53 | private Vector3 _torque; | ||
54 | private Vector3 m_lastVelocity; | ||
55 | private Vector3 m_lastposition; | ||
56 | private Quaternion m_lastorientation = new Quaternion(); | ||
57 | private Vector3 m_rotationalVelocity; | ||
58 | private Vector3 _size; | ||
59 | private Vector3 _acceleration; | ||
60 | // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); | ||
61 | private Quaternion _orientation; | ||
62 | private Vector3 m_taintposition; | ||
63 | private Vector3 m_taintsize; | ||
64 | private Vector3 m_taintVelocity; | ||
65 | private Vector3 m_taintTorque; | ||
66 | private Quaternion m_taintrot; | ||
67 | private Vector3 m_rotateEnable = Vector3.One; // Current setting | ||
68 | private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL | ||
69 | private bool m_rotateEnableUpdate = false; | ||
70 | private Vector3 m_lockX; | ||
71 | private Vector3 m_lockY; | ||
72 | private Vector3 m_lockZ; | ||
73 | private IntPtr Amotor = IntPtr.Zero; | ||
74 | private IntPtr AmotorX = IntPtr.Zero; | ||
75 | private IntPtr AmotorY = IntPtr.Zero; | ||
76 | private IntPtr AmotorZ = IntPtr.Zero; | ||
77 | |||
78 | private Vector3 m_PIDTarget; | ||
79 | private float m_PIDTau; | ||
80 | private float PID_D = 35f; | ||
81 | private float PID_G = 25f; | ||
82 | private bool m_usePID = false; | ||
83 | |||
84 | private Quaternion m_APIDTarget = new Quaternion(); | ||
85 | private float m_APIDStrength = 0.5f; | ||
86 | private float m_APIDDamping = 0.5f; | ||
87 | private bool m_useAPID = false; | ||
88 | |||
89 | // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), | ||
90 | // do not confuse with VEHICLE HOVER | ||
91 | |||
92 | private float m_PIDHoverHeight; | ||
93 | private float m_PIDHoverTau; | ||
94 | private bool m_useHoverPID; | ||
95 | private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; | ||
96 | private float m_targetHoverHeight; | ||
97 | private float m_groundHeight; | ||
98 | private float m_waterHeight; | ||
99 | private float m_buoyancy; //m_buoyancy set by llSetBuoyancy() | ||
100 | |||
101 | // private float m_tensor = 5f; | ||
102 | private int body_autodisable_frames = 20; | ||
103 | |||
104 | |||
105 | private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom | ||
106 | | CollisionCategories.Space | ||
107 | | CollisionCategories.Body | ||
108 | | CollisionCategories.Character | ||
109 | ); | ||
110 | private bool m_taintshape; | ||
111 | private bool m_taintPhysics; | ||
112 | private bool m_collidesLand = true; | ||
113 | private bool m_collidesWater; | ||
114 | public bool m_returnCollisions; | ||
115 | |||
116 | // Default we're a Geometry | ||
117 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | ||
118 | |||
119 | // Default, Collide with Other Geometries, spaces and Bodies | ||
120 | private CollisionCategories m_collisionFlags = m_default_collisionFlags; | ||
121 | |||
122 | public bool m_taintremove; | ||
123 | public bool m_taintdisable; | ||
124 | public bool m_disabled; | ||
125 | public bool m_taintadd; | ||
126 | public bool m_taintselected; | ||
127 | public bool m_taintCollidesWater; | ||
128 | |||
129 | public uint m_localID; | ||
130 | |||
131 | //public GCHandle gc; | ||
132 | private CollisionLocker ode; | ||
133 | |||
134 | private bool m_meshfailed = false; | ||
135 | private bool m_taintforce = false; | ||
136 | private bool m_taintaddangularforce = false; | ||
137 | private Vector3 m_force; | ||
138 | private List<Vector3> m_forcelist = new List<Vector3>(); | ||
139 | private List<Vector3> m_angularforcelist = new List<Vector3>(); | ||
140 | |||
141 | private IMesh _mesh; | ||
142 | private PrimitiveBaseShape _pbs; | ||
143 | private OdeScene _parent_scene; | ||
144 | public IntPtr m_targetSpace = IntPtr.Zero; | ||
145 | public IntPtr prim_geom; | ||
146 | // public IntPtr prev_geom; | ||
147 | public IntPtr _triMeshData; | ||
148 | |||
149 | private IntPtr _linkJointGroup = IntPtr.Zero; | ||
150 | private PhysicsActor _parent; | ||
151 | private PhysicsActor m_taintparent; | ||
152 | |||
153 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | ||
154 | |||
155 | private bool iscolliding; | ||
156 | private bool m_isphysical; | ||
157 | private bool m_isSelected; | ||
158 | |||
159 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
160 | |||
161 | private bool m_throttleUpdates; | ||
162 | private int throttleCounter; | ||
163 | public int m_interpenetrationcount; | ||
164 | public float m_collisionscore; | ||
165 | public int m_roundsUnderMotionThreshold; | ||
166 | private int m_crossingfailures; | ||
167 | |||
168 | public bool outofBounds; | ||
169 | private float m_density = 10.000006836f; // Aluminum g/cm3; | ||
170 | |||
171 | public bool _zeroFlag; // if body has been stopped | ||
172 | private bool m_lastUpdateSent; | ||
173 | |||
174 | public IntPtr Body = IntPtr.Zero; | ||
175 | public String m_primName; | ||
176 | private Vector3 _target_velocity; | ||
177 | public d.Mass pMass; | ||
178 | |||
179 | public int m_eventsubscription; | ||
180 | private CollisionEventUpdate CollisionEventsThisFrame; | ||
181 | |||
182 | private IntPtr m_linkJoint = IntPtr.Zero; | ||
183 | |||
184 | public volatile bool childPrim; | ||
185 | |||
186 | internal int m_material = (int)Material.Wood; | ||
187 | |||
188 | private int frcount = 0; // Used to limit dynamics debug output to | ||
189 | private int revcount = 0; // Reverse motion while > 0 | ||
190 | |||
191 | private IntPtr m_body = IntPtr.Zero; | ||
192 | |||
193 | // Vehicle properties ============================================================================================ | ||
194 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind | ||
195 | // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier | ||
196 | private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: | ||
197 | // HOVER_TERRAIN_ONLY | ||
198 | // HOVER_GLOBAL_HEIGHT | ||
199 | // NO_DEFLECTION_UP | ||
200 | // HOVER_WATER_ONLY | ||
201 | // HOVER_UP_ONLY | ||
202 | // LIMIT_MOTOR_UP | ||
203 | // LIMIT_ROLL_ONLY | ||
204 | |||
205 | // Linear properties | ||
206 | private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity | ||
207 | //requested by LSL | ||
208 | private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL | ||
209 | private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL | ||
210 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL | ||
211 | |||
212 | private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor | ||
213 | private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity | ||
214 | private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity | ||
215 | |||
216 | //Angular properties | ||
217 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
218 | |||
219 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL | ||
220 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL | ||
221 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL | ||
222 | |||
223 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor | ||
224 | // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity | ||
225 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | ||
226 | |||
227 | //Deflection properties | ||
228 | // private float m_angularDeflectionEfficiency = 0; | ||
229 | // private float m_angularDeflectionTimescale = 0; | ||
230 | // private float m_linearDeflectionEfficiency = 0; | ||
231 | // private float m_linearDeflectionTimescale = 0; | ||
232 | |||
233 | //Banking properties | ||
234 | // private float m_bankingEfficiency = 0; | ||
235 | // private float m_bankingMix = 0; | ||
236 | // private float m_bankingTimescale = 0; | ||
237 | |||
238 | //Hover and Buoyancy properties | ||
239 | private float m_VhoverHeight = 0f; | ||
240 | // private float m_VhoverEfficiency = 0f; | ||
241 | private float m_VhoverTimescale = 0f; | ||
242 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height | ||
243 | private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. | ||
244 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | ||
245 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | ||
246 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
247 | |||
248 | //Attractor properties | ||
249 | private float m_verticalAttractionEfficiency = 1.0f; // damped | ||
250 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. | ||
251 | |||
252 | |||
253 | |||
254 | |||
255 | |||
256 | |||
257 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
258 | Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) | ||
259 | { | ||
260 | ode = dode; | ||
261 | if (!pos.IsFinite()) | ||
262 | { | ||
263 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | ||
264 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | ||
265 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); | ||
266 | } | ||
267 | |||
268 | _position = pos; | ||
269 | m_taintposition = pos; | ||
270 | PID_D = parent_scene.bodyPIDD; | ||
271 | PID_G = parent_scene.bodyPIDG; | ||
272 | m_density = parent_scene.geomDefaultDensity; | ||
273 | // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; | ||
274 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
275 | |||
276 | |||
277 | prim_geom = IntPtr.Zero; | ||
278 | // prev_geom = IntPtr.Zero; | ||
279 | |||
280 | if (!pos.IsFinite()) | ||
281 | { | ||
282 | size = new Vector3(0.5f, 0.5f, 0.5f); | ||
283 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Size"); | ||
284 | } | ||
285 | |||
286 | if (size.X <= 0) size.X = 0.01f; | ||
287 | if (size.Y <= 0) size.Y = 0.01f; | ||
288 | if (size.Z <= 0) size.Z = 0.01f; | ||
289 | |||
290 | _size = size; | ||
291 | m_taintsize = _size; | ||
292 | |||
293 | if (!QuaternionIsFinite(rotation)) | ||
294 | { | ||
295 | rotation = Quaternion.Identity; | ||
296 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation"); | ||
297 | } | ||
298 | |||
299 | _orientation = rotation; | ||
300 | m_taintrot = _orientation; | ||
301 | _mesh = mesh; | ||
302 | _pbs = pbs; | ||
303 | |||
304 | _parent_scene = parent_scene; | ||
305 | m_targetSpace = (IntPtr)0; | ||
306 | |||
307 | // if (pos.Z < 0) | ||
308 | if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) | ||
309 | m_isphysical = false; | ||
310 | else | ||
311 | { | ||
312 | m_isphysical = pisPhysical; | ||
313 | // If we're physical, we need to be in the master space for now. | ||
314 | // linksets *should* be in a space together.. but are not currently | ||
315 | if (m_isphysical) | ||
316 | m_targetSpace = _parent_scene.space; | ||
317 | } | ||
318 | m_primName = primName; | ||
319 | m_taintadd = true; | ||
320 | _parent_scene.AddPhysicsActorTaint(this); | ||
321 | // don't do .add() here; old geoms get recycled with the same hash | ||
322 | } | ||
323 | |||
324 | public override int PhysicsActorType | ||
325 | { | ||
326 | get { return (int) ActorTypes.Prim; } | ||
327 | set { return; } | ||
328 | } | ||
329 | |||
330 | public override bool SetAlwaysRun | ||
331 | { | ||
332 | get { return false; } | ||
333 | set { return; } | ||
334 | } | ||
335 | |||
336 | public override uint LocalID | ||
337 | { | ||
338 | set { | ||
339 | //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); | ||
340 | m_localID = value; } | ||
341 | } | ||
342 | |||
343 | public override bool Grabbed | ||
344 | { | ||
345 | set { return; } | ||
346 | } | ||
347 | |||
348 | public override bool Selected | ||
349 | { | ||
350 | set { | ||
351 | |||
352 | //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); | ||
353 | // This only makes the object not collidable if the object | ||
354 | // is physical or the object is modified somehow *IN THE FUTURE* | ||
355 | // without this, if an avatar selects prim, they can walk right | ||
356 | // through it while it's selected | ||
357 | m_collisionscore = 0; | ||
358 | if ((m_isphysical && !_zeroFlag) || !value) | ||
359 | { | ||
360 | m_taintselected = value; | ||
361 | _parent_scene.AddPhysicsActorTaint(this); | ||
362 | } | ||
363 | else | ||
364 | { | ||
365 | m_taintselected = value; | ||
366 | m_isSelected = value; | ||
367 | } | ||
368 | if(m_isSelected) disableBodySoft(); | ||
369 | } | ||
370 | } | ||
371 | |||
372 | public override bool IsPhysical | ||
373 | { | ||
374 | get { return m_isphysical; } | ||
375 | set | ||
376 | { | ||
377 | m_isphysical = value; | ||
378 | if (!m_isphysical) | ||
379 | { // Zero the remembered last velocity | ||
380 | m_lastVelocity = Vector3.Zero; | ||
381 | if (m_type != Vehicle.TYPE_NONE) Halt(); | ||
382 | } | ||
383 | } | ||
384 | } | ||
385 | |||
386 | public void setPrimForRemoval() | ||
387 | { | ||
388 | m_taintremove = true; | ||
389 | } | ||
390 | |||
391 | public override bool Flying | ||
392 | { | ||
393 | // no flying prims for you | ||
394 | get { return false; } | ||
395 | set { } | ||
396 | } | ||
397 | |||
398 | public override bool IsColliding | ||
399 | { | ||
400 | get { return iscolliding; } | ||
401 | set { iscolliding = value; } | ||
402 | } | ||
403 | |||
404 | public override bool CollidingGround | ||
405 | { | ||
406 | get { return false; } | ||
407 | set { return; } | ||
408 | } | ||
409 | |||
410 | public override bool CollidingObj | ||
411 | { | ||
412 | get { return false; } | ||
413 | set { return; } | ||
414 | } | ||
415 | |||
416 | public override bool ThrottleUpdates | ||
417 | { | ||
418 | get { return m_throttleUpdates; } | ||
419 | set { m_throttleUpdates = value; } | ||
420 | } | ||
421 | |||
422 | public override bool Stopped | ||
423 | { | ||
424 | get { return _zeroFlag; } | ||
425 | } | ||
426 | |||
427 | public override Vector3 Position | ||
428 | { | ||
429 | get { return _position; } | ||
430 | |||
431 | set { _position = value; | ||
432 | //m_log.Info("[PHYSICS]: " + _position.ToString()); | ||
433 | } | ||
434 | } | ||
435 | |||
436 | public override Vector3 Size | ||
437 | { | ||
438 | get { return _size; } | ||
439 | set | ||
440 | { | ||
441 | if (value.IsFinite()) | ||
442 | { | ||
443 | _size = value; | ||
444 | } | ||
445 | else | ||
446 | { | ||
447 | m_log.Warn("[PHYSICS]: Got NaN Size on object"); | ||
448 | } | ||
449 | } | ||
450 | } | ||
451 | |||
452 | public override float Mass | ||
453 | { | ||
454 | get { return CalculateMass(); } | ||
455 | } | ||
456 | |||
457 | public override Vector3 Force | ||
458 | { | ||
459 | //get { return Vector3.Zero; } | ||
460 | get { return m_force; } | ||
461 | set | ||
462 | { | ||
463 | if (value.IsFinite()) | ||
464 | { | ||
465 | m_force = value; | ||
466 | } | ||
467 | else | ||
468 | { | ||
469 | m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object"); | ||
470 | } | ||
471 | } | ||
472 | } | ||
473 | |||
474 | public override int VehicleType | ||
475 | { | ||
476 | get { return (int)m_type; } | ||
477 | set { ProcessTypeChange((Vehicle)value); } | ||
478 | } | ||
479 | |||
480 | public override void VehicleFloatParam(int param, float value) | ||
481 | { | ||
482 | ProcessFloatVehicleParam((Vehicle) param, value); | ||
483 | } | ||
484 | |||
485 | public override void VehicleVectorParam(int param, Vector3 value) | ||
486 | { | ||
487 | ProcessVectorVehicleParam((Vehicle) param, value); | ||
488 | } | ||
489 | |||
490 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
491 | { | ||
492 | ProcessRotationVehicleParam((Vehicle) param, rotation); | ||
493 | } | ||
494 | |||
495 | public override void VehicleFlags(int param, bool remove) | ||
496 | { | ||
497 | ProcessVehicleFlags(param, remove); | ||
498 | } | ||
499 | |||
500 | public override void SetVolumeDetect(int param) | ||
501 | { | ||
502 | lock (_parent_scene.OdeLock) | ||
503 | { | ||
504 | m_isVolumeDetect = (param!=0); | ||
505 | } | ||
506 | } | ||
507 | |||
508 | public override Vector3 CenterOfMass | ||
509 | { | ||
510 | get { return Vector3.Zero; } | ||
511 | } | ||
512 | |||
513 | public override Vector3 GeometricCenter | ||
514 | { | ||
515 | get { return Vector3.Zero; } | ||
516 | } | ||
517 | |||
518 | public override PrimitiveBaseShape Shape | ||
519 | { | ||
520 | set | ||
521 | { | ||
522 | _pbs = value; | ||
523 | m_taintshape = true; | ||
524 | } | ||
525 | } | ||
526 | |||
527 | public override Vector3 Velocity | ||
528 | { | ||
529 | get | ||
530 | { | ||
531 | // Averate previous velocity with the new one so | ||
532 | // client object interpolation works a 'little' better | ||
533 | if (_zeroFlag) | ||
534 | return Vector3.Zero; | ||
535 | |||
536 | Vector3 returnVelocity = Vector3.Zero; | ||
537 | returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; | ||
538 | returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; | ||
539 | returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; | ||
540 | return returnVelocity; | ||
541 | } | ||
542 | set | ||
543 | { | ||
544 | if (value.IsFinite()) | ||
545 | { | ||
546 | _velocity = value; | ||
547 | |||
548 | m_taintVelocity = value; | ||
549 | _parent_scene.AddPhysicsActorTaint(this); | ||
550 | } | ||
551 | else | ||
552 | { | ||
553 | m_log.Warn("[PHYSICS]: Got NaN Velocity in Object"); | ||
554 | } | ||
555 | |||
556 | } | ||
557 | } | ||
558 | |||
559 | public override Vector3 Torque | ||
560 | { | ||
561 | get | ||
562 | { | ||
563 | if (!m_isphysical || Body == IntPtr.Zero) | ||
564 | return Vector3.Zero; | ||
565 | |||
566 | return _torque; | ||
567 | } | ||
568 | |||
569 | set | ||
570 | { | ||
571 | if (value.IsFinite()) | ||
572 | { | ||
573 | m_taintTorque = value; | ||
574 | _parent_scene.AddPhysicsActorTaint(this); | ||
575 | } | ||
576 | else | ||
577 | { | ||
578 | m_log.Warn("[PHYSICS]: Got NaN Torque in Object"); | ||
579 | } | ||
580 | } | ||
581 | } | ||
582 | |||
583 | public override float CollisionScore | ||
584 | { | ||
585 | get { return m_collisionscore; } | ||
586 | set { m_collisionscore = value; } | ||
587 | } | ||
588 | |||
589 | public override bool Kinematic | ||
590 | { | ||
591 | get { return false; } | ||
592 | set { } | ||
593 | } | ||
594 | |||
595 | public override Quaternion Orientation | ||
596 | { | ||
597 | get { return _orientation; } | ||
598 | set | ||
599 | { | ||
600 | if (QuaternionIsFinite(value)) | ||
601 | { | ||
602 | _orientation = value; | ||
603 | } | ||
604 | else | ||
605 | m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); | ||
606 | |||
607 | } | ||
608 | } | ||
609 | |||
610 | |||
611 | public override bool FloatOnWater | ||
612 | { | ||
613 | set { | ||
614 | m_taintCollidesWater = value; | ||
615 | _parent_scene.AddPhysicsActorTaint(this); | ||
616 | } | ||
617 | } | ||
618 | |||
619 | public override void SetMomentum(Vector3 momentum) | ||
620 | { | ||
621 | } | ||
622 | |||
623 | public override Vector3 PIDTarget | ||
624 | { | ||
625 | set | ||
626 | { | ||
627 | if (value.IsFinite()) | ||
628 | { | ||
629 | m_PIDTarget = value; | ||
630 | } | ||
631 | else | ||
632 | m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); | ||
633 | } | ||
634 | } | ||
635 | public override bool PIDActive { set { m_usePID = value; } } | ||
636 | public override float PIDTau { set { m_PIDTau = value; } } | ||
637 | |||
638 | // For RotLookAt | ||
639 | public override Quaternion APIDTarget { set { m_APIDTarget = value; } } | ||
640 | public override bool APIDActive { set { m_useAPID = value; } } | ||
641 | public override float APIDStrength { set { m_APIDStrength = value; } } | ||
642 | public override float APIDDamping { set { m_APIDDamping = value; } } | ||
643 | |||
644 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | ||
645 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | ||
646 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | ||
647 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } | ||
648 | |||
649 | internal static bool QuaternionIsFinite(Quaternion q) | ||
650 | { | ||
651 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
652 | return false; | ||
653 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
654 | return false; | ||
655 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
656 | return false; | ||
657 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
658 | return false; | ||
659 | return true; | ||
660 | } | ||
661 | |||
662 | public override Vector3 Acceleration // client updates read data via here | ||
663 | { | ||
664 | get { return _acceleration; } | ||
665 | } | ||
666 | |||
667 | |||
668 | public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything. | ||
669 | { | ||
670 | _acceleration = accel; | ||
671 | } | ||
672 | |||
673 | public override void AddForce(Vector3 force, bool pushforce) | ||
674 | { | ||
675 | if (force.IsFinite()) | ||
676 | { | ||
677 | lock (m_forcelist) | ||
678 | m_forcelist.Add(force); | ||
679 | |||
680 | m_taintforce = true; | ||
681 | } | ||
682 | else | ||
683 | { | ||
684 | m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object"); | ||
685 | } | ||
686 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
687 | } | ||
688 | |||
689 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
690 | { | ||
691 | if (force.IsFinite()) | ||
692 | { | ||
693 | m_angularforcelist.Add(force); | ||
694 | m_taintaddangularforce = true; | ||
695 | } | ||
696 | else | ||
697 | { | ||
698 | m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object"); | ||
699 | } | ||
700 | } | ||
701 | |||
702 | public override Vector3 RotationalVelocity | ||
703 | { | ||
704 | get | ||
705 | { | ||
706 | return m_rotationalVelocity; | ||
707 | } | ||
708 | set | ||
709 | { | ||
710 | if (value.IsFinite()) | ||
711 | { | ||
712 | m_rotationalVelocity = value; | ||
713 | } | ||
714 | else | ||
715 | { | ||
716 | m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); | ||
717 | } | ||
718 | } | ||
719 | } | ||
720 | |||
721 | public override void CrossingFailure() | ||
722 | { | ||
723 | m_crossingfailures++; | ||
724 | if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
725 | { | ||
726 | base.RaiseOutOfBounds(_position); | ||
727 | return; | ||
728 | } | ||
729 | else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
730 | { | ||
731 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); | ||
732 | } | ||
733 | } | ||
734 | |||
735 | public override float Buoyancy | ||
736 | { | ||
737 | get { return m_buoyancy; } | ||
738 | // set { m_buoyancy = value; } | ||
739 | set { | ||
740 | m_buoyancy = value; | ||
741 | |||
742 | Console.WriteLine("m_buoyancy={0}", m_buoyancy); | ||
743 | } | ||
744 | } | ||
745 | |||
746 | public override void link(PhysicsActor obj) | ||
747 | { | ||
748 | m_taintparent = obj; | ||
749 | } | ||
750 | |||
751 | public override void delink() | ||
752 | { | ||
753 | m_taintparent = null; | ||
754 | } | ||
755 | |||
756 | public override void LockAngularMotion(Vector3 axis) | ||
757 | { | ||
758 | // This is actually ROTATION ENABLE, not a lock. | ||
759 | // default is <1,1,1> which is all enabled. | ||
760 | // The lock value is updated inside Move(), no point in using the taint system. | ||
761 | // OS 'm_taintAngularLock' etc change to m_rotateEnable. | ||
762 | if (axis.IsFinite()) | ||
763 | { | ||
764 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
765 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
766 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
767 | m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
768 | m_rotateEnableRequest = axis; | ||
769 | m_rotateEnableUpdate = true; | ||
770 | } | ||
771 | else | ||
772 | { | ||
773 | m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object"); | ||
774 | } | ||
775 | } | ||
776 | |||
777 | |||
778 | public void SetGeom(IntPtr geom) | ||
779 | { | ||
780 | if(prim_geom != IntPtr.Zero) | ||
781 | { | ||
782 | // Remove any old entries | ||
783 | //string tPA; | ||
784 | //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); | ||
785 | //Console.WriteLine("**** Remove {0}", tPA); | ||
786 | if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); | ||
787 | if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); | ||
788 | d.GeomDestroy(prim_geom); | ||
789 | } | ||
790 | |||
791 | prim_geom = geom; | ||
792 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); | ||
793 | if (prim_geom != IntPtr.Zero) | ||
794 | { | ||
795 | _parent_scene.geom_name_map[prim_geom] = this.m_primName; | ||
796 | _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; | ||
797 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
798 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
799 | //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); | ||
800 | } | ||
801 | |||
802 | if (childPrim) | ||
803 | { | ||
804 | if (_parent != null && _parent is OdePrim) | ||
805 | { | ||
806 | OdePrim parent = (OdePrim)_parent; | ||
807 | //Console.WriteLine("SetGeom calls ChildSetGeom"); | ||
808 | parent.ChildSetGeom(this); | ||
809 | } | ||
810 | } | ||
811 | //m_log.Warn("Setting Geom to: " + prim_geom); | ||
812 | } | ||
813 | |||
814 | public void enableBodySoft() | ||
815 | { | ||
816 | if (!childPrim) | ||
817 | { | ||
818 | if (m_isphysical && Body != IntPtr.Zero) | ||
819 | { | ||
820 | d.BodyEnable(Body); | ||
821 | if (m_type != Vehicle.TYPE_NONE) | ||
822 | Enable(Body, _parent_scene); | ||
823 | } | ||
824 | |||
825 | m_disabled = false; | ||
826 | } | ||
827 | } | ||
828 | |||
829 | public void disableBodySoft() | ||
830 | { | ||
831 | m_disabled = true; | ||
832 | |||
833 | if (m_isphysical && Body != IntPtr.Zero) | ||
834 | { | ||
835 | d.BodyDisable(Body); | ||
836 | Halt(); | ||
837 | } | ||
838 | } | ||
839 | |||
840 | public void enableBody() | ||
841 | { | ||
842 | // Don't enable this body if we're a child prim | ||
843 | // this should be taken care of in the parent function not here | ||
844 | if (!childPrim) | ||
845 | { | ||
846 | // Sets the geom to a body | ||
847 | Body = d.BodyCreate(_parent_scene.world); | ||
848 | |||
849 | setMass(); | ||
850 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
851 | d.Quaternion myrot = new d.Quaternion(); | ||
852 | myrot.X = _orientation.X; | ||
853 | myrot.Y = _orientation.Y; | ||
854 | myrot.Z = _orientation.Z; | ||
855 | myrot.W = _orientation.W; | ||
856 | d.BodySetQuaternion(Body, ref myrot); | ||
857 | d.GeomSetBody(prim_geom, Body); | ||
858 | m_collisionCategories |= CollisionCategories.Body; | ||
859 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
860 | |||
861 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
862 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
863 | |||
864 | d.BodySetAutoDisableFlag(Body, true); | ||
865 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
866 | |||
867 | // disconnect from world gravity so we can apply buoyancy | ||
868 | d.BodySetGravityMode (Body, false); | ||
869 | |||
870 | m_interpenetrationcount = 0; | ||
871 | m_collisionscore = 0; | ||
872 | m_disabled = false; | ||
873 | |||
874 | if (m_type != Vehicle.TYPE_NONE) | ||
875 | { | ||
876 | Enable(Body, _parent_scene); | ||
877 | } | ||
878 | |||
879 | _parent_scene.addActivePrim(this); | ||
880 | } | ||
881 | } | ||
882 | |||
883 | #region Mass Calculation | ||
884 | |||
885 | private float CalculateMass() | ||
886 | { | ||
887 | float volume = _size.X * _size.Y * _size.Z; // default | ||
888 | float tmp; | ||
889 | |||
890 | float returnMass = 0; | ||
891 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | ||
892 | float hollowVolume = hollowAmount * hollowAmount; | ||
893 | |||
894 | switch (_pbs.ProfileShape) | ||
895 | { | ||
896 | case ProfileShape.Square: | ||
897 | // default box | ||
898 | |||
899 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
900 | { | ||
901 | if (hollowAmount > 0.0) | ||
902 | { | ||
903 | switch (_pbs.HollowShape) | ||
904 | { | ||
905 | case HollowShape.Square: | ||
906 | case HollowShape.Same: | ||
907 | break; | ||
908 | |||
909 | case HollowShape.Circle: | ||
910 | |||
911 | hollowVolume *= 0.78539816339f; | ||
912 | break; | ||
913 | |||
914 | case HollowShape.Triangle: | ||
915 | |||
916 | hollowVolume *= (0.5f * .5f); | ||
917 | break; | ||
918 | |||
919 | default: | ||
920 | hollowVolume = 0; | ||
921 | break; | ||
922 | } | ||
923 | volume *= (1.0f - hollowVolume); | ||
924 | } | ||
925 | } | ||
926 | |||
927 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
928 | { | ||
929 | //a tube | ||
930 | |||
931 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
932 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
933 | volume -= volume*tmp*tmp; | ||
934 | |||
935 | if (hollowAmount > 0.0) | ||
936 | { | ||
937 | hollowVolume *= hollowAmount; | ||
938 | |||
939 | switch (_pbs.HollowShape) | ||
940 | { | ||
941 | case HollowShape.Square: | ||
942 | case HollowShape.Same: | ||
943 | break; | ||
944 | |||
945 | case HollowShape.Circle: | ||
946 | hollowVolume *= 0.78539816339f;; | ||
947 | break; | ||
948 | |||
949 | case HollowShape.Triangle: | ||
950 | hollowVolume *= 0.5f * 0.5f; | ||
951 | break; | ||
952 | default: | ||
953 | hollowVolume = 0; | ||
954 | break; | ||
955 | } | ||
956 | volume *= (1.0f - hollowVolume); | ||
957 | } | ||
958 | } | ||
959 | |||
960 | break; | ||
961 | |||
962 | case ProfileShape.Circle: | ||
963 | |||
964 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
965 | { | ||
966 | volume *= 0.78539816339f; // elipse base | ||
967 | |||
968 | if (hollowAmount > 0.0) | ||
969 | { | ||
970 | switch (_pbs.HollowShape) | ||
971 | { | ||
972 | case HollowShape.Same: | ||
973 | case HollowShape.Circle: | ||
974 | break; | ||
975 | |||
976 | case HollowShape.Square: | ||
977 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
978 | break; | ||
979 | |||
980 | case HollowShape.Triangle: | ||
981 | hollowVolume *= .5f * 1.27323954473516f; | ||
982 | break; | ||
983 | |||
984 | default: | ||
985 | hollowVolume = 0; | ||
986 | break; | ||
987 | } | ||
988 | volume *= (1.0f - hollowVolume); | ||
989 | } | ||
990 | } | ||
991 | |||
992 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
993 | { | ||
994 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
995 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
996 | volume *= (1.0f - tmp * tmp); | ||
997 | |||
998 | if (hollowAmount > 0.0) | ||
999 | { | ||
1000 | |||
1001 | // calculate the hollow volume by it's shape compared to the prim shape | ||
1002 | hollowVolume *= hollowAmount; | ||
1003 | |||
1004 | switch (_pbs.HollowShape) | ||
1005 | { | ||
1006 | case HollowShape.Same: | ||
1007 | case HollowShape.Circle: | ||
1008 | break; | ||
1009 | |||
1010 | case HollowShape.Square: | ||
1011 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
1012 | break; | ||
1013 | |||
1014 | case HollowShape.Triangle: | ||
1015 | hollowVolume *= .5f * 1.27323954473516f; | ||
1016 | break; | ||
1017 | |||
1018 | default: | ||
1019 | hollowVolume = 0; | ||
1020 | break; | ||
1021 | } | ||
1022 | volume *= (1.0f - hollowVolume); | ||
1023 | } | ||
1024 | } | ||
1025 | break; | ||
1026 | |||
1027 | case ProfileShape.HalfCircle: | ||
1028 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1029 | { | ||
1030 | volume *= 0.52359877559829887307710723054658f; | ||
1031 | } | ||
1032 | break; | ||
1033 | |||
1034 | case ProfileShape.EquilateralTriangle: | ||
1035 | |||
1036 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
1037 | { | ||
1038 | volume *= 0.32475953f; | ||
1039 | |||
1040 | if (hollowAmount > 0.0) | ||
1041 | { | ||
1042 | |||
1043 | // calculate the hollow volume by it's shape compared to the prim shape | ||
1044 | switch (_pbs.HollowShape) | ||
1045 | { | ||
1046 | case HollowShape.Same: | ||
1047 | case HollowShape.Triangle: | ||
1048 | hollowVolume *= .25f; | ||
1049 | break; | ||
1050 | |||
1051 | case HollowShape.Square: | ||
1052 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
1053 | break; | ||
1054 | |||
1055 | case HollowShape.Circle: | ||
1056 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
1057 | // Cyllinder hollow volume calculation | ||
1058 | |||
1059 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
1060 | break; | ||
1061 | |||
1062 | default: | ||
1063 | hollowVolume = 0; | ||
1064 | break; | ||
1065 | } | ||
1066 | volume *= (1.0f - hollowVolume); | ||
1067 | } | ||
1068 | } | ||
1069 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1070 | { | ||
1071 | volume *= 0.32475953f; | ||
1072 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
1073 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
1074 | volume *= (1.0f - tmp * tmp); | ||
1075 | |||
1076 | if (hollowAmount > 0.0) | ||
1077 | { | ||
1078 | |||
1079 | hollowVolume *= hollowAmount; | ||
1080 | |||
1081 | switch (_pbs.HollowShape) | ||
1082 | { | ||
1083 | case HollowShape.Same: | ||
1084 | case HollowShape.Triangle: | ||
1085 | hollowVolume *= .25f; | ||
1086 | break; | ||
1087 | |||
1088 | case HollowShape.Square: | ||
1089 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
1090 | break; | ||
1091 | |||
1092 | case HollowShape.Circle: | ||
1093 | |||
1094 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
1095 | break; | ||
1096 | |||
1097 | default: | ||
1098 | hollowVolume = 0; | ||
1099 | break; | ||
1100 | } | ||
1101 | volume *= (1.0f - hollowVolume); | ||
1102 | } | ||
1103 | } | ||
1104 | break; | ||
1105 | |||
1106 | default: | ||
1107 | break; | ||
1108 | } | ||
1109 | |||
1110 | |||
1111 | |||
1112 | float taperX1; | ||
1113 | float taperY1; | ||
1114 | float taperX; | ||
1115 | float taperY; | ||
1116 | float pathBegin; | ||
1117 | float pathEnd; | ||
1118 | float profileBegin; | ||
1119 | float profileEnd; | ||
1120 | |||
1121 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | ||
1122 | { | ||
1123 | taperX1 = _pbs.PathScaleX * 0.01f; | ||
1124 | if (taperX1 > 1.0f) | ||
1125 | taperX1 = 2.0f - taperX1; | ||
1126 | taperX = 1.0f - taperX1; | ||
1127 | |||
1128 | taperY1 = _pbs.PathScaleY * 0.01f; | ||
1129 | if (taperY1 > 1.0f) | ||
1130 | taperY1 = 2.0f - taperY1; | ||
1131 | taperY = 1.0f - taperY1; | ||
1132 | } | ||
1133 | else | ||
1134 | { | ||
1135 | taperX = _pbs.PathTaperX * 0.01f; | ||
1136 | if (taperX < 0.0f) | ||
1137 | taperX = -taperX; | ||
1138 | taperX1 = 1.0f - taperX; | ||
1139 | |||
1140 | taperY = _pbs.PathTaperY * 0.01f; | ||
1141 | if (taperY < 0.0f) | ||
1142 | taperY = -taperY; | ||
1143 | taperY1 = 1.0f - taperY; | ||
1144 | |||
1145 | } | ||
1146 | |||
1147 | |||
1148 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | ||
1149 | |||
1150 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | ||
1151 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | ||
1152 | volume *= (pathEnd - pathBegin); | ||
1153 | |||
1154 | // this is crude aproximation | ||
1155 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | ||
1156 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | ||
1157 | volume *= (profileEnd - profileBegin); | ||
1158 | |||
1159 | returnMass = m_density * volume; | ||
1160 | |||
1161 | if (returnMass <= 0) | ||
1162 | returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
1163 | // else if (returnMass > _parent_scene.maximumMassObject) | ||
1164 | // returnMass = _parent_scene.maximumMassObject; | ||
1165 | |||
1166 | |||
1167 | |||
1168 | |||
1169 | // Recursively calculate mass | ||
1170 | bool HasChildPrim = false; | ||
1171 | lock (childrenPrim) | ||
1172 | { | ||
1173 | if (childrenPrim.Count > 0) | ||
1174 | { | ||
1175 | HasChildPrim = true; | ||
1176 | } | ||
1177 | |||
1178 | } | ||
1179 | if (HasChildPrim) | ||
1180 | { | ||
1181 | OdePrim[] childPrimArr = new OdePrim[0]; | ||
1182 | |||
1183 | lock (childrenPrim) | ||
1184 | childPrimArr = childrenPrim.ToArray(); | ||
1185 | |||
1186 | for (int i = 0; i < childPrimArr.Length; i++) | ||
1187 | { | ||
1188 | if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) | ||
1189 | returnMass += childPrimArr[i].CalculateMass(); | ||
1190 | // failsafe, this shouldn't happen but with OpenSim, you never know :) | ||
1191 | if (i > 256) | ||
1192 | break; | ||
1193 | } | ||
1194 | } | ||
1195 | if (returnMass > _parent_scene.maximumMassObject) | ||
1196 | returnMass = _parent_scene.maximumMassObject; | ||
1197 | return returnMass; | ||
1198 | }// end CalculateMass | ||
1199 | |||
1200 | #endregion | ||
1201 | |||
1202 | public void setMass() | ||
1203 | { | ||
1204 | if (Body != (IntPtr) 0) | ||
1205 | { | ||
1206 | float newmass = CalculateMass(); | ||
1207 | |||
1208 | //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); | ||
1209 | |||
1210 | d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); | ||
1211 | d.BodySetMass(Body, ref pMass); | ||
1212 | } | ||
1213 | } | ||
1214 | |||
1215 | public void disableBody() | ||
1216 | { | ||
1217 | //this kills the body so things like 'mesh' can re-create it. | ||
1218 | lock (this) | ||
1219 | { | ||
1220 | if (!childPrim) | ||
1221 | { | ||
1222 | if (Body != IntPtr.Zero) | ||
1223 | { | ||
1224 | _parent_scene.remActivePrim(this); | ||
1225 | m_collisionCategories &= ~CollisionCategories.Body; | ||
1226 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
1227 | |||
1228 | if (prim_geom != IntPtr.Zero) | ||
1229 | { | ||
1230 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1231 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1232 | } | ||
1233 | |||
1234 | |||
1235 | d.BodyDestroy(Body); | ||
1236 | lock (childrenPrim) | ||
1237 | { | ||
1238 | if (childrenPrim.Count > 0) | ||
1239 | { | ||
1240 | foreach (OdePrim prm in childrenPrim) | ||
1241 | { | ||
1242 | _parent_scene.remActivePrim(prm); | ||
1243 | prm.Body = IntPtr.Zero; | ||
1244 | } | ||
1245 | } | ||
1246 | } | ||
1247 | Body = IntPtr.Zero; | ||
1248 | } | ||
1249 | } | ||
1250 | else | ||
1251 | { | ||
1252 | _parent_scene.remActivePrim(this); | ||
1253 | |||
1254 | m_collisionCategories &= ~CollisionCategories.Body; | ||
1255 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
1256 | |||
1257 | if (prim_geom != IntPtr.Zero) | ||
1258 | { | ||
1259 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1260 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1261 | } | ||
1262 | |||
1263 | |||
1264 | Body = IntPtr.Zero; | ||
1265 | } | ||
1266 | } | ||
1267 | m_disabled = true; | ||
1268 | m_collisionscore = 0; | ||
1269 | } | ||
1270 | |||
1271 | private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); | ||
1272 | |||
1273 | public void setMesh(OdeScene parent_scene, IMesh mesh) | ||
1274 | { | ||
1275 | // This sleeper is there to moderate how long it takes between | ||
1276 | // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object | ||
1277 | |||
1278 | //Thread.Sleep(10); | ||
1279 | |||
1280 | //Kill Body so that mesh can re-make the geom | ||
1281 | if (IsPhysical && Body != IntPtr.Zero) | ||
1282 | { | ||
1283 | if (childPrim) | ||
1284 | { | ||
1285 | if (_parent != null) | ||
1286 | { | ||
1287 | OdePrim parent = (OdePrim)_parent; | ||
1288 | parent.ChildDelink(this); | ||
1289 | } | ||
1290 | } | ||
1291 | else | ||
1292 | { | ||
1293 | disableBody(); | ||
1294 | } | ||
1295 | } | ||
1296 | |||
1297 | IntPtr vertices, indices; | ||
1298 | int vertexCount, indexCount; | ||
1299 | int vertexStride, triStride; | ||
1300 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap | ||
1301 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage | ||
1302 | |||
1303 | mesh.releaseSourceMeshData(); // free up the original mesh data to save memory | ||
1304 | if (m_MeshToTriMeshMap.ContainsKey(mesh)) | ||
1305 | { | ||
1306 | _triMeshData = m_MeshToTriMeshMap[mesh]; | ||
1307 | } | ||
1308 | else | ||
1309 | { | ||
1310 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
1311 | |||
1312 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | ||
1313 | d.GeomTriMeshDataPreprocess(_triMeshData); | ||
1314 | m_MeshToTriMeshMap[mesh] = _triMeshData; | ||
1315 | } | ||
1316 | |||
1317 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1318 | try | ||
1319 | { | ||
1320 | // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer | ||
1321 | // { | ||
1322 | SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); | ||
1323 | // } | ||
1324 | } | ||
1325 | catch (AccessViolationException) | ||
1326 | { | ||
1327 | m_log.Error("[PHYSICS]: MESH LOCKED"); | ||
1328 | return; | ||
1329 | } | ||
1330 | |||
1331 | |||
1332 | // if (IsPhysical && Body == (IntPtr) 0) | ||
1333 | // { | ||
1334 | // Recreate the body | ||
1335 | // m_interpenetrationcount = 0; | ||
1336 | // m_collisionscore = 0; | ||
1337 | |||
1338 | // enableBody(); | ||
1339 | // } | ||
1340 | } | ||
1341 | |||
1342 | public void ProcessTaints(float timestep) //============================================================================= | ||
1343 | { | ||
1344 | if (m_taintadd) | ||
1345 | { | ||
1346 | changeadd(timestep); | ||
1347 | } | ||
1348 | |||
1349 | if (prim_geom != IntPtr.Zero) | ||
1350 | { | ||
1351 | if (!_position.ApproxEquals(m_taintposition, 0f)) | ||
1352 | changemove(timestep); | ||
1353 | |||
1354 | if (m_taintrot != _orientation) | ||
1355 | { | ||
1356 | if(childPrim && IsPhysical) // For physical child prim... | ||
1357 | { | ||
1358 | rotate(timestep); | ||
1359 | // KF: ODE will also rotate the parent prim! | ||
1360 | // so rotate the root back to where it was | ||
1361 | OdePrim parent = (OdePrim)_parent; | ||
1362 | parent.rotate(timestep); | ||
1363 | } | ||
1364 | else | ||
1365 | { | ||
1366 | //Just rotate the prim | ||
1367 | rotate(timestep); | ||
1368 | } | ||
1369 | } | ||
1370 | // | ||
1371 | |||
1372 | if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) | ||
1373 | changePhysicsStatus(timestep); | ||
1374 | // | ||
1375 | |||
1376 | if (!_size.ApproxEquals(m_taintsize,0f)) | ||
1377 | changesize(timestep); | ||
1378 | // | ||
1379 | |||
1380 | if (m_taintshape) | ||
1381 | changeshape(timestep); | ||
1382 | // | ||
1383 | |||
1384 | if (m_taintforce) | ||
1385 | changeAddForce(timestep); | ||
1386 | |||
1387 | if (m_taintaddangularforce) | ||
1388 | changeAddAngularForce(timestep); | ||
1389 | |||
1390 | if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) | ||
1391 | changeSetTorque(timestep); | ||
1392 | |||
1393 | if (m_taintdisable) | ||
1394 | changedisable(timestep); | ||
1395 | |||
1396 | if (m_taintselected != m_isSelected) | ||
1397 | changeSelectedStatus(timestep); | ||
1398 | |||
1399 | if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) | ||
1400 | changevelocity(timestep); | ||
1401 | |||
1402 | if (m_taintparent != _parent) | ||
1403 | changelink(timestep); | ||
1404 | |||
1405 | if (m_taintCollidesWater != m_collidesWater) | ||
1406 | changefloatonwater(timestep); | ||
1407 | /* obsolete | ||
1408 | if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) | ||
1409 | changeAngularLock(timestep); | ||
1410 | */ | ||
1411 | } | ||
1412 | else | ||
1413 | { | ||
1414 | m_log.Error("[PHYSICS]: The scene reused a disposed PhysActor! *waves finger*, Don't be evil. A couple of things can cause this. An improper prim breakdown(be sure to set prim_geom to zero after d.GeomDestroy! An improper buildup (creating the geom failed). Or, the Scene Reused a physics actor after disposing it.)"); | ||
1415 | } | ||
1416 | } | ||
1417 | |||
1418 | /* obsolete | ||
1419 | private void changeAngularLock(float timestep) | ||
1420 | { | ||
1421 | if (_parent == null) | ||
1422 | { | ||
1423 | m_angularLock = m_taintAngularLock; | ||
1424 | m_angularLockSet = true; | ||
1425 | } | ||
1426 | } | ||
1427 | */ | ||
1428 | private void changelink(float timestep) | ||
1429 | { | ||
1430 | // If the newly set parent is not null | ||
1431 | // create link | ||
1432 | if (_parent == null && m_taintparent != null) | ||
1433 | { | ||
1434 | if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) | ||
1435 | { | ||
1436 | OdePrim obj = (OdePrim)m_taintparent; | ||
1437 | //obj.disableBody(); | ||
1438 | obj.ParentPrim(this); | ||
1439 | |||
1440 | /* | ||
1441 | if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) | ||
1442 | { | ||
1443 | _linkJointGroup = d.JointGroupCreate(0); | ||
1444 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1445 | d.JointAttach(m_linkJoint, obj.Body, Body); | ||
1446 | d.JointSetFixed(m_linkJoint); | ||
1447 | } | ||
1448 | */ | ||
1449 | } | ||
1450 | } | ||
1451 | // If the newly set parent is null | ||
1452 | // destroy link | ||
1453 | else if (_parent != null && m_taintparent == null) | ||
1454 | { | ||
1455 | if (_parent is OdePrim) | ||
1456 | { | ||
1457 | OdePrim obj = (OdePrim)_parent; | ||
1458 | obj.ChildDelink(this); | ||
1459 | childPrim = false; | ||
1460 | //_parent = null; | ||
1461 | } | ||
1462 | |||
1463 | /* | ||
1464 | if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) | ||
1465 | d.JointGroupDestroy(_linkJointGroup); | ||
1466 | |||
1467 | _linkJointGroup = (IntPtr)0; | ||
1468 | m_linkJoint = (IntPtr)0; | ||
1469 | */ | ||
1470 | } | ||
1471 | |||
1472 | _parent = m_taintparent; | ||
1473 | m_taintPhysics = m_isphysical; | ||
1474 | } | ||
1475 | |||
1476 | // I'm the parent | ||
1477 | // prim is the child | ||
1478 | public void ParentPrim(OdePrim prim) | ||
1479 | { | ||
1480 | if (this.m_localID != prim.m_localID) | ||
1481 | { | ||
1482 | if (Body == IntPtr.Zero) | ||
1483 | { | ||
1484 | Body = d.BodyCreate(_parent_scene.world); | ||
1485 | setMass(); | ||
1486 | } | ||
1487 | if (Body != IntPtr.Zero) | ||
1488 | { | ||
1489 | lock (childrenPrim) | ||
1490 | { | ||
1491 | if (!childrenPrim.Contains(prim)) | ||
1492 | { | ||
1493 | childrenPrim.Add(prim); | ||
1494 | |||
1495 | foreach (OdePrim prm in childrenPrim) | ||
1496 | { | ||
1497 | d.Mass m2; | ||
1498 | d.MassSetZero(out m2); | ||
1499 | d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); | ||
1500 | |||
1501 | |||
1502 | d.Quaternion quat = new d.Quaternion(); | ||
1503 | quat.W = prm._orientation.W; | ||
1504 | quat.X = prm._orientation.X; | ||
1505 | quat.Y = prm._orientation.Y; | ||
1506 | quat.Z = prm._orientation.Z; | ||
1507 | |||
1508 | d.Matrix3 mat = new d.Matrix3(); | ||
1509 | d.RfromQ(out mat, ref quat); | ||
1510 | d.MassRotate(ref m2, ref mat); | ||
1511 | d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); | ||
1512 | d.MassAdd(ref pMass, ref m2); | ||
1513 | } | ||
1514 | foreach (OdePrim prm in childrenPrim) | ||
1515 | { | ||
1516 | |||
1517 | prm.m_collisionCategories |= CollisionCategories.Body; | ||
1518 | prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1519 | |||
1520 | if (prm.prim_geom == IntPtr.Zero) | ||
1521 | { | ||
1522 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); | ||
1523 | continue; | ||
1524 | } | ||
1525 | //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); | ||
1526 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | ||
1527 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | ||
1528 | |||
1529 | |||
1530 | d.Quaternion quat = new d.Quaternion(); | ||
1531 | quat.W = prm._orientation.W; | ||
1532 | quat.X = prm._orientation.X; | ||
1533 | quat.Y = prm._orientation.Y; | ||
1534 | quat.Z = prm._orientation.Z; | ||
1535 | |||
1536 | d.Matrix3 mat = new d.Matrix3(); | ||
1537 | d.RfromQ(out mat, ref quat); | ||
1538 | if (Body != IntPtr.Zero) | ||
1539 | { | ||
1540 | d.GeomSetBody(prm.prim_geom, Body); | ||
1541 | prm.childPrim = true; | ||
1542 | d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); | ||
1543 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1544 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1545 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1546 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1547 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); | ||
1548 | //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); | ||
1549 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1550 | d.BodySetMass(Body, ref pMass); | ||
1551 | } | ||
1552 | else | ||
1553 | { | ||
1554 | m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body"); | ||
1555 | } | ||
1556 | |||
1557 | |||
1558 | prm.m_interpenetrationcount = 0; | ||
1559 | prm.m_collisionscore = 0; | ||
1560 | prm.m_disabled = false; | ||
1561 | |||
1562 | prm.Body = Body; | ||
1563 | _parent_scene.addActivePrim(prm); | ||
1564 | } | ||
1565 | m_collisionCategories |= CollisionCategories.Body; | ||
1566 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1567 | |||
1568 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); | ||
1569 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1570 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); | ||
1571 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1572 | |||
1573 | |||
1574 | d.Quaternion quat2 = new d.Quaternion(); | ||
1575 | quat2.W = _orientation.W; | ||
1576 | quat2.X = _orientation.X; | ||
1577 | quat2.Y = _orientation.Y; | ||
1578 | quat2.Z = _orientation.Z; | ||
1579 | |||
1580 | d.Matrix3 mat2 = new d.Matrix3(); | ||
1581 | d.RfromQ(out mat2, ref quat2); | ||
1582 | d.GeomSetBody(prim_geom, Body); | ||
1583 | d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); | ||
1584 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1585 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1586 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1587 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1588 | //d.GeomSetOffsetRotation(prim_geom, ref mat2); | ||
1589 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1590 | d.BodySetMass(Body, ref pMass); | ||
1591 | |||
1592 | d.BodySetAutoDisableFlag(Body, true); | ||
1593 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1594 | |||
1595 | |||
1596 | m_interpenetrationcount = 0; | ||
1597 | m_collisionscore = 0; | ||
1598 | m_disabled = false; | ||
1599 | |||
1600 | d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); | ||
1601 | if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene); | ||
1602 | _parent_scene.addActivePrim(this); | ||
1603 | } | ||
1604 | } | ||
1605 | } | ||
1606 | } | ||
1607 | |||
1608 | } | ||
1609 | |||
1610 | private void ChildSetGeom(OdePrim odePrim) | ||
1611 | { | ||
1612 | //if (m_isphysical && Body != IntPtr.Zero) | ||
1613 | lock (childrenPrim) | ||
1614 | { | ||
1615 | foreach (OdePrim prm in childrenPrim) | ||
1616 | { | ||
1617 | //prm.childPrim = true; | ||
1618 | prm.disableBody(); | ||
1619 | //prm.m_taintparent = null; | ||
1620 | //prm._parent = null; | ||
1621 | //prm.m_taintPhysics = false; | ||
1622 | //prm.m_disabled = true; | ||
1623 | //prm.childPrim = false; | ||
1624 | } | ||
1625 | } | ||
1626 | disableBody(); | ||
1627 | |||
1628 | |||
1629 | if (Body != IntPtr.Zero) | ||
1630 | { | ||
1631 | _parent_scene.remActivePrim(this); | ||
1632 | } | ||
1633 | |||
1634 | lock (childrenPrim) | ||
1635 | { | ||
1636 | foreach (OdePrim prm in childrenPrim) | ||
1637 | { | ||
1638 | ParentPrim(prm); | ||
1639 | } | ||
1640 | } | ||
1641 | |||
1642 | } | ||
1643 | |||
1644 | private void ChildDelink(OdePrim odePrim) | ||
1645 | { | ||
1646 | // Okay, we have a delinked child.. need to rebuild the body. | ||
1647 | lock (childrenPrim) | ||
1648 | { | ||
1649 | foreach (OdePrim prm in childrenPrim) | ||
1650 | { | ||
1651 | prm.childPrim = true; | ||
1652 | prm.disableBody(); | ||
1653 | //prm.m_taintparent = null; | ||
1654 | //prm._parent = null; | ||
1655 | //prm.m_taintPhysics = false; | ||
1656 | //prm.m_disabled = true; | ||
1657 | //prm.childPrim = false; | ||
1658 | } | ||
1659 | } | ||
1660 | disableBody(); | ||
1661 | |||
1662 | lock (childrenPrim) | ||
1663 | { | ||
1664 | childrenPrim.Remove(odePrim); | ||
1665 | } | ||
1666 | |||
1667 | if (Body != IntPtr.Zero) | ||
1668 | { | ||
1669 | _parent_scene.remActivePrim(this); | ||
1670 | } | ||
1671 | |||
1672 | lock (childrenPrim) | ||
1673 | { | ||
1674 | foreach (OdePrim prm in childrenPrim) | ||
1675 | { | ||
1676 | ParentPrim(prm); | ||
1677 | } | ||
1678 | } | ||
1679 | } | ||
1680 | |||
1681 | private void changeSelectedStatus(float timestep) | ||
1682 | { | ||
1683 | if (m_taintselected) | ||
1684 | { | ||
1685 | m_collisionCategories = CollisionCategories.Selected; | ||
1686 | m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); | ||
1687 | |||
1688 | // We do the body disable soft twice because 'in theory' a collision could have happened | ||
1689 | // in between the disabling and the collision properties setting | ||
1690 | // which would wake the physical body up from a soft disabling and potentially cause it to fall | ||
1691 | // through the ground. | ||
1692 | |||
1693 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select | ||
1694 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, | ||
1695 | // so that causes the selected part to wake up and continue moving. | ||
1696 | |||
1697 | // even if you select all parts of a jointed assembly, it is not guaranteed that the entire | ||
1698 | // assembly will stop simulating during the selection, because of the lack of atomicity | ||
1699 | // of select operations (their processing could be interrupted by a thread switch, causing | ||
1700 | // simulation to continue before all of the selected object notifications trickle down to | ||
1701 | // the physics engine). | ||
1702 | |||
1703 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are | ||
1704 | // selected and disabled. then, due to a thread switch, the selection processing is | ||
1705 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose | ||
1706 | // selection was not yet processed, continues to simulate. this wakes up ALL of the | ||
1707 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken | ||
1708 | // up, start simulating again, which in turn wakes up the last 50. | ||
1709 | |||
1710 | if (m_isphysical) | ||
1711 | { | ||
1712 | disableBodySoft(); | ||
1713 | } | ||
1714 | |||
1715 | if (prim_geom != IntPtr.Zero) | ||
1716 | { | ||
1717 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1718 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1719 | } | ||
1720 | |||
1721 | if (m_isphysical) | ||
1722 | { | ||
1723 | disableBodySoft(); | ||
1724 | |||
1725 | if (Body != IntPtr.Zero) | ||
1726 | { | ||
1727 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1728 | d.BodySetForce(Body, 0, 0, 0); | ||
1729 | enableBodySoft(); | ||
1730 | } | ||
1731 | } | ||
1732 | } | ||
1733 | else | ||
1734 | { | ||
1735 | m_collisionCategories = CollisionCategories.Geom; | ||
1736 | |||
1737 | if (m_isphysical) | ||
1738 | m_collisionCategories |= CollisionCategories.Body; | ||
1739 | |||
1740 | m_collisionFlags = m_default_collisionFlags; | ||
1741 | |||
1742 | if (m_collidesLand) | ||
1743 | m_collisionFlags |= CollisionCategories.Land; | ||
1744 | if (m_collidesWater) | ||
1745 | m_collisionFlags |= CollisionCategories.Water; | ||
1746 | |||
1747 | if (prim_geom != IntPtr.Zero) | ||
1748 | { | ||
1749 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1750 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1751 | } | ||
1752 | /* Uhhh - stop the motion if the object is _selected_!! | ||
1753 | if (m_isphysical) | ||
1754 | { | ||
1755 | if (Body != IntPtr.Zero) | ||
1756 | { | ||
1757 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1758 | d.BodySetForce(Body, 0, 0, 0); | ||
1759 | enableBodySoft(); | ||
1760 | } | ||
1761 | } | ||
1762 | */ | ||
1763 | } | ||
1764 | |||
1765 | resetCollisionAccounting(); | ||
1766 | m_isSelected = m_taintselected; | ||
1767 | }//end changeSelectedStatus | ||
1768 | |||
1769 | public void ResetTaints() | ||
1770 | { | ||
1771 | m_taintposition = _position; | ||
1772 | m_taintrot = _orientation; | ||
1773 | m_taintPhysics = m_isphysical; | ||
1774 | m_taintselected = m_isSelected; | ||
1775 | m_taintsize = _size; | ||
1776 | m_taintshape = false; | ||
1777 | m_taintforce = false; | ||
1778 | m_taintdisable = false; | ||
1779 | m_taintVelocity = Vector3.Zero; | ||
1780 | } | ||
1781 | |||
1782 | public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) | ||
1783 | { | ||
1784 | //Console.WriteLine("CreateGeom:"); | ||
1785 | if (_mesh != null) // Special - make mesh | ||
1786 | { | ||
1787 | setMesh(_parent_scene, _mesh); | ||
1788 | } | ||
1789 | else // not a mesh | ||
1790 | { | ||
1791 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) // special profile?? | ||
1792 | { | ||
1793 | if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) // Equi-size | ||
1794 | { | ||
1795 | if (((_size.X / 2f) > 0f)) // Has size | ||
1796 | { | ||
1797 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1798 | try | ||
1799 | { | ||
1800 | //Console.WriteLine(" CreateGeom 1"); | ||
1801 | SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); | ||
1802 | } | ||
1803 | catch (AccessViolationException) | ||
1804 | { | ||
1805 | m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); | ||
1806 | ode.dunlock(_parent_scene.world); | ||
1807 | return; | ||
1808 | } | ||
1809 | } | ||
1810 | else | ||
1811 | { | ||
1812 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1813 | try | ||
1814 | { | ||
1815 | //Console.WriteLine(" CreateGeom 2"); | ||
1816 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1817 | } | ||
1818 | catch (AccessViolationException) | ||
1819 | { | ||
1820 | m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); | ||
1821 | ode.dunlock(_parent_scene.world); | ||
1822 | return; | ||
1823 | } | ||
1824 | } | ||
1825 | } | ||
1826 | else // not equi-size | ||
1827 | { | ||
1828 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1829 | try | ||
1830 | { | ||
1831 | //Console.WriteLine(" CreateGeom 3"); | ||
1832 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1833 | } | ||
1834 | catch (AccessViolationException) | ||
1835 | { | ||
1836 | m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); | ||
1837 | ode.dunlock(_parent_scene.world); | ||
1838 | return; | ||
1839 | } | ||
1840 | } | ||
1841 | } | ||
1842 | |||
1843 | else // not special profile | ||
1844 | { | ||
1845 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1846 | try | ||
1847 | { | ||
1848 | //Console.WriteLine(" CreateGeom 4"); | ||
1849 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1850 | } | ||
1851 | catch (AccessViolationException) | ||
1852 | { | ||
1853 | m_log.Warn("[PHYSICS]: Unable to create physics proxy for object"); | ||
1854 | ode.dunlock(_parent_scene.world); | ||
1855 | return; | ||
1856 | } | ||
1857 | } | ||
1858 | } | ||
1859 | } | ||
1860 | |||
1861 | public void changeadd(float timestep) | ||
1862 | { | ||
1863 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
1864 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); | ||
1865 | |||
1866 | if (targetspace == IntPtr.Zero) | ||
1867 | targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); | ||
1868 | |||
1869 | m_targetSpace = targetspace; | ||
1870 | |||
1871 | if (_mesh == null && m_meshfailed == false) | ||
1872 | { | ||
1873 | if (_parent_scene.needsMeshing(_pbs)) | ||
1874 | { | ||
1875 | // Don't need to re-enable body.. it's done in SetMesh | ||
1876 | try | ||
1877 | { | ||
1878 | _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); | ||
1879 | } | ||
1880 | catch | ||
1881 | { | ||
1882 | //Don't continuously try to mesh prims when meshing has failed | ||
1883 | m_meshfailed = true; | ||
1884 | } | ||
1885 | // createmesh returns null when it's a shape that isn't a cube. | ||
1886 | // m_log.Debug(m_localID); | ||
1887 | } | ||
1888 | } | ||
1889 | |||
1890 | |||
1891 | lock (_parent_scene.OdeLock) | ||
1892 | { | ||
1893 | //Console.WriteLine("changeadd 1"); | ||
1894 | CreateGeom(m_targetSpace, _mesh); | ||
1895 | |||
1896 | if (prim_geom != IntPtr.Zero) | ||
1897 | { | ||
1898 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1899 | d.Quaternion myrot = new d.Quaternion(); | ||
1900 | myrot.X = _orientation.X; | ||
1901 | myrot.Y = _orientation.Y; | ||
1902 | myrot.Z = _orientation.Z; | ||
1903 | myrot.W = _orientation.W; | ||
1904 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
1905 | } | ||
1906 | |||
1907 | if (m_isphysical && Body == IntPtr.Zero) | ||
1908 | { | ||
1909 | enableBody(); | ||
1910 | } | ||
1911 | } | ||
1912 | |||
1913 | changeSelectedStatus(timestep); | ||
1914 | |||
1915 | m_taintadd = false; | ||
1916 | } | ||
1917 | |||
1918 | public void changemove(float timestep) | ||
1919 | { | ||
1920 | //Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position ); | ||
1921 | if (m_isphysical) | ||
1922 | { | ||
1923 | //Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim); | ||
1924 | // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! | ||
1925 | if (!m_taintremove && !childPrim) | ||
1926 | { | ||
1927 | if (Body == IntPtr.Zero) | ||
1928 | enableBody(); | ||
1929 | //Prim auto disable after 20 frames, | ||
1930 | //if you move it, re-enable the prim manually. | ||
1931 | if (_parent != null) | ||
1932 | { | ||
1933 | if (m_linkJoint != IntPtr.Zero) | ||
1934 | { | ||
1935 | d.JointDestroy(m_linkJoint); | ||
1936 | m_linkJoint = IntPtr.Zero; | ||
1937 | } | ||
1938 | } | ||
1939 | if (Body != IntPtr.Zero) | ||
1940 | { | ||
1941 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
1942 | |||
1943 | if (_parent != null) | ||
1944 | { | ||
1945 | OdePrim odParent = (OdePrim)_parent; | ||
1946 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) | ||
1947 | { | ||
1948 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? | ||
1949 | Console.WriteLine(" JointCreateFixed"); | ||
1950 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1951 | d.JointAttach(m_linkJoint, Body, odParent.Body); | ||
1952 | d.JointSetFixed(m_linkJoint); | ||
1953 | } | ||
1954 | } | ||
1955 | d.BodyEnable(Body); | ||
1956 | if (m_type != Vehicle.TYPE_NONE) | ||
1957 | { | ||
1958 | Enable(Body, _parent_scene); | ||
1959 | } | ||
1960 | } | ||
1961 | else | ||
1962 | { | ||
1963 | m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario."); | ||
1964 | } | ||
1965 | } | ||
1966 | //else | ||
1967 | // { | ||
1968 | //m_log.Debug("[BUG]: race!"); | ||
1969 | //} | ||
1970 | } | ||
1971 | else | ||
1972 | { | ||
1973 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
1974 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
1975 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1976 | |||
1977 | IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); | ||
1978 | m_targetSpace = tempspace; | ||
1979 | |||
1980 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1981 | if (prim_geom != IntPtr.Zero) | ||
1982 | { | ||
1983 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1984 | |||
1985 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1986 | d.SpaceAdd(m_targetSpace, prim_geom); | ||
1987 | } | ||
1988 | } | ||
1989 | |||
1990 | changeSelectedStatus(timestep); | ||
1991 | |||
1992 | resetCollisionAccounting(); | ||
1993 | m_taintposition = _position; | ||
1994 | } | ||
1995 | |||
1996 | |||
1997 | |||
1998 | public void rotate(float timestep) | ||
1999 | { | ||
2000 | d.Quaternion myrot = new d.Quaternion(); | ||
2001 | myrot.X = _orientation.X; | ||
2002 | myrot.Y = _orientation.Y; | ||
2003 | myrot.Z = _orientation.Z; | ||
2004 | myrot.W = _orientation.W; | ||
2005 | if (Body != IntPtr.Zero) | ||
2006 | { | ||
2007 | // KF: If this is a root prim do BodySet | ||
2008 | d.BodySetQuaternion(Body, ref myrot); | ||
2009 | } | ||
2010 | else | ||
2011 | { | ||
2012 | // daughter prim, do Geom set | ||
2013 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2014 | } | ||
2015 | |||
2016 | resetCollisionAccounting(); | ||
2017 | m_taintrot = _orientation; | ||
2018 | } | ||
2019 | |||
2020 | private void resetCollisionAccounting() | ||
2021 | { | ||
2022 | m_collisionscore = 0; | ||
2023 | m_interpenetrationcount = 0; | ||
2024 | m_disabled = false; | ||
2025 | } | ||
2026 | |||
2027 | public void changedisable(float timestep) | ||
2028 | { | ||
2029 | m_disabled = true; | ||
2030 | if (Body != IntPtr.Zero) | ||
2031 | { | ||
2032 | d.BodyDisable(Body); | ||
2033 | Body = IntPtr.Zero; | ||
2034 | } | ||
2035 | |||
2036 | m_taintdisable = false; | ||
2037 | } | ||
2038 | |||
2039 | public void changePhysicsStatus(float timestep) | ||
2040 | { | ||
2041 | if (m_isphysical == true) | ||
2042 | { | ||
2043 | if (Body == IntPtr.Zero) | ||
2044 | { | ||
2045 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
2046 | { | ||
2047 | changeshape(2f); | ||
2048 | } | ||
2049 | else | ||
2050 | { | ||
2051 | enableBody(); | ||
2052 | } | ||
2053 | } | ||
2054 | } | ||
2055 | else | ||
2056 | { | ||
2057 | if (Body != IntPtr.Zero) | ||
2058 | { | ||
2059 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
2060 | { | ||
2061 | _mesh = null; | ||
2062 | //Console.WriteLine("changePhysicsStatus for " + m_primName ); | ||
2063 | changeadd(2f); | ||
2064 | } | ||
2065 | if (childPrim) | ||
2066 | { | ||
2067 | if (_parent != null) | ||
2068 | { | ||
2069 | OdePrim parent = (OdePrim)_parent; | ||
2070 | parent.ChildDelink(this); | ||
2071 | } | ||
2072 | } | ||
2073 | else | ||
2074 | { | ||
2075 | disableBody(); | ||
2076 | } | ||
2077 | } | ||
2078 | } | ||
2079 | |||
2080 | changeSelectedStatus(timestep); | ||
2081 | |||
2082 | resetCollisionAccounting(); | ||
2083 | m_taintPhysics = m_isphysical; | ||
2084 | } | ||
2085 | |||
2086 | public void changesize(float timestamp) | ||
2087 | { | ||
2088 | |||
2089 | string oldname = _parent_scene.geom_name_map[prim_geom]; | ||
2090 | |||
2091 | if (_size.X <= 0) _size.X = 0.01f; | ||
2092 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2093 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2094 | |||
2095 | // Cleanup of old prim geometry | ||
2096 | if (_mesh != null) | ||
2097 | { | ||
2098 | // Cleanup meshing here | ||
2099 | } | ||
2100 | //kill body to rebuild | ||
2101 | if (IsPhysical && Body != IntPtr.Zero) | ||
2102 | { | ||
2103 | if (childPrim) | ||
2104 | { | ||
2105 | if (_parent != null) | ||
2106 | { | ||
2107 | OdePrim parent = (OdePrim)_parent; | ||
2108 | parent.ChildDelink(this); | ||
2109 | } | ||
2110 | } | ||
2111 | else | ||
2112 | { | ||
2113 | disableBody(); | ||
2114 | } | ||
2115 | } | ||
2116 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
2117 | { | ||
2118 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2119 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
2120 | } | ||
2121 | // we don't need to do space calculation because the client sends a position update also. | ||
2122 | |||
2123 | // Construction of new prim | ||
2124 | if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) | ||
2125 | { | ||
2126 | float meshlod = _parent_scene.meshSculptLOD; | ||
2127 | |||
2128 | if (IsPhysical) | ||
2129 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2130 | // Don't need to re-enable body.. it's done in SetMesh | ||
2131 | |||
2132 | IMesh mesh = null; | ||
2133 | |||
2134 | try | ||
2135 | { | ||
2136 | if (_parent_scene.needsMeshing(_pbs)) | ||
2137 | mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); | ||
2138 | } | ||
2139 | catch | ||
2140 | { | ||
2141 | m_meshfailed = true; | ||
2142 | } | ||
2143 | |||
2144 | //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); | ||
2145 | //Console.WriteLine("changesize 1"); | ||
2146 | CreateGeom(m_targetSpace, mesh); | ||
2147 | |||
2148 | |||
2149 | } | ||
2150 | else | ||
2151 | { | ||
2152 | _mesh = null; | ||
2153 | //Console.WriteLine("changesize 2"); | ||
2154 | CreateGeom(m_targetSpace, _mesh); | ||
2155 | } | ||
2156 | |||
2157 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2158 | d.Quaternion myrot = new d.Quaternion(); | ||
2159 | myrot.X = _orientation.X; | ||
2160 | myrot.Y = _orientation.Y; | ||
2161 | myrot.Z = _orientation.Z; | ||
2162 | myrot.W = _orientation.W; | ||
2163 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2164 | |||
2165 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2166 | if (IsPhysical && Body == IntPtr.Zero && !childPrim) | ||
2167 | { | ||
2168 | // Re creates body on size. | ||
2169 | // EnableBody also does setMass() | ||
2170 | enableBody(); | ||
2171 | d.BodyEnable(Body); | ||
2172 | } | ||
2173 | |||
2174 | _parent_scene.geom_name_map[prim_geom] = oldname; | ||
2175 | |||
2176 | changeSelectedStatus(timestamp); | ||
2177 | if (childPrim) | ||
2178 | { | ||
2179 | if (_parent is OdePrim) | ||
2180 | { | ||
2181 | OdePrim parent = (OdePrim)_parent; | ||
2182 | parent.ChildSetGeom(this); | ||
2183 | } | ||
2184 | } | ||
2185 | resetCollisionAccounting(); | ||
2186 | m_taintsize = _size; | ||
2187 | } | ||
2188 | |||
2189 | |||
2190 | |||
2191 | public void changefloatonwater(float timestep) | ||
2192 | { | ||
2193 | m_collidesWater = m_taintCollidesWater; | ||
2194 | |||
2195 | if (prim_geom != IntPtr.Zero) | ||
2196 | { | ||
2197 | if (m_collidesWater) | ||
2198 | { | ||
2199 | m_collisionFlags |= CollisionCategories.Water; | ||
2200 | } | ||
2201 | else | ||
2202 | { | ||
2203 | m_collisionFlags &= ~CollisionCategories.Water; | ||
2204 | } | ||
2205 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2206 | } | ||
2207 | } | ||
2208 | |||
2209 | public void changeshape(float timestamp) | ||
2210 | { | ||
2211 | string oldname = _parent_scene.geom_name_map[prim_geom]; | ||
2212 | |||
2213 | // Cleanup of old prim geometry and Bodies | ||
2214 | if (IsPhysical && Body != IntPtr.Zero) | ||
2215 | { | ||
2216 | if (childPrim) | ||
2217 | { | ||
2218 | if (_parent != null) | ||
2219 | { | ||
2220 | OdePrim parent = (OdePrim)_parent; | ||
2221 | parent.ChildDelink(this); | ||
2222 | } | ||
2223 | } | ||
2224 | else | ||
2225 | { | ||
2226 | disableBody(); | ||
2227 | } | ||
2228 | } | ||
2229 | |||
2230 | |||
2231 | // we don't need to do space calculation because the client sends a position update also. | ||
2232 | if (_size.X <= 0) _size.X = 0.01f; | ||
2233 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2234 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2235 | // Construction of new prim | ||
2236 | |||
2237 | if (_parent_scene.needsMeshing(_pbs) && m_meshfailed == false) | ||
2238 | { | ||
2239 | // Don't need to re-enable body.. it's done in SetMesh | ||
2240 | float meshlod = _parent_scene.meshSculptLOD; | ||
2241 | |||
2242 | if (IsPhysical) | ||
2243 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2244 | try | ||
2245 | { | ||
2246 | IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); | ||
2247 | CreateGeom(m_targetSpace, mesh); | ||
2248 | } | ||
2249 | catch | ||
2250 | { | ||
2251 | m_meshfailed = true; | ||
2252 | } | ||
2253 | // createmesh returns null when it doesn't mesh. | ||
2254 | } | ||
2255 | else | ||
2256 | { | ||
2257 | _mesh = null; | ||
2258 | //Console.WriteLine("changeshape"); | ||
2259 | CreateGeom(m_targetSpace, null); | ||
2260 | } | ||
2261 | |||
2262 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2263 | d.Quaternion myrot = new d.Quaternion(); | ||
2264 | //myrot.W = _orientation.w; | ||
2265 | myrot.W = _orientation.W; | ||
2266 | myrot.X = _orientation.X; | ||
2267 | myrot.Y = _orientation.Y; | ||
2268 | myrot.Z = _orientation.Z; | ||
2269 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2270 | |||
2271 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2272 | if (IsPhysical && Body == IntPtr.Zero) | ||
2273 | { | ||
2274 | // Re creates body on size. | ||
2275 | // EnableBody also does setMass() | ||
2276 | enableBody(); | ||
2277 | if (Body != IntPtr.Zero) | ||
2278 | { | ||
2279 | d.BodyEnable(Body); | ||
2280 | } | ||
2281 | } | ||
2282 | _parent_scene.geom_name_map[prim_geom] = oldname; | ||
2283 | |||
2284 | changeSelectedStatus(timestamp); | ||
2285 | if (childPrim) | ||
2286 | { | ||
2287 | if (_parent is OdePrim) | ||
2288 | { | ||
2289 | OdePrim parent = (OdePrim)_parent; | ||
2290 | parent.ChildSetGeom(this); | ||
2291 | } | ||
2292 | } | ||
2293 | resetCollisionAccounting(); | ||
2294 | m_taintshape = false; | ||
2295 | } | ||
2296 | |||
2297 | public void changeAddForce(float timestamp) | ||
2298 | { | ||
2299 | if (!m_isSelected) | ||
2300 | { | ||
2301 | lock (m_forcelist) | ||
2302 | { | ||
2303 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2304 | if (IsPhysical) | ||
2305 | { | ||
2306 | Vector3 iforce = Vector3.Zero; | ||
2307 | int i = 0; | ||
2308 | try | ||
2309 | { | ||
2310 | for (i = 0; i < m_forcelist.Count; i++) | ||
2311 | { | ||
2312 | |||
2313 | iforce = iforce + (m_forcelist[i] * 100); | ||
2314 | } | ||
2315 | } | ||
2316 | catch (IndexOutOfRangeException) | ||
2317 | { | ||
2318 | m_forcelist = new List<Vector3>(); | ||
2319 | m_collisionscore = 0; | ||
2320 | m_interpenetrationcount = 0; | ||
2321 | m_taintforce = false; | ||
2322 | return; | ||
2323 | } | ||
2324 | catch (ArgumentOutOfRangeException) | ||
2325 | { | ||
2326 | m_forcelist = new List<Vector3>(); | ||
2327 | m_collisionscore = 0; | ||
2328 | m_interpenetrationcount = 0; | ||
2329 | m_taintforce = false; | ||
2330 | return; | ||
2331 | } | ||
2332 | d.BodyEnable(Body); | ||
2333 | d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); | ||
2334 | } | ||
2335 | m_forcelist.Clear(); | ||
2336 | } | ||
2337 | |||
2338 | m_collisionscore = 0; | ||
2339 | m_interpenetrationcount = 0; | ||
2340 | } | ||
2341 | |||
2342 | m_taintforce = false; | ||
2343 | |||
2344 | } | ||
2345 | |||
2346 | |||
2347 | |||
2348 | public void changeSetTorque(float timestamp) | ||
2349 | { | ||
2350 | if (!m_isSelected) | ||
2351 | { | ||
2352 | if (IsPhysical && Body != IntPtr.Zero) | ||
2353 | { | ||
2354 | d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); | ||
2355 | } | ||
2356 | } | ||
2357 | |||
2358 | m_taintTorque = Vector3.Zero; | ||
2359 | } | ||
2360 | |||
2361 | public void changeAddAngularForce(float timestamp) | ||
2362 | { | ||
2363 | if (!m_isSelected) | ||
2364 | { | ||
2365 | lock (m_angularforcelist) | ||
2366 | { | ||
2367 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2368 | if (IsPhysical) | ||
2369 | { | ||
2370 | Vector3 iforce = Vector3.Zero; | ||
2371 | for (int i = 0; i < m_angularforcelist.Count; i++) | ||
2372 | { | ||
2373 | iforce = iforce + (m_angularforcelist[i] * 100); | ||
2374 | } | ||
2375 | d.BodyEnable(Body); | ||
2376 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); | ||
2377 | |||
2378 | } | ||
2379 | m_angularforcelist.Clear(); | ||
2380 | } | ||
2381 | |||
2382 | m_collisionscore = 0; | ||
2383 | m_interpenetrationcount = 0; | ||
2384 | } | ||
2385 | |||
2386 | m_taintaddangularforce = false; | ||
2387 | } | ||
2388 | |||
2389 | private void changevelocity(float timestep) | ||
2390 | { | ||
2391 | if (!m_isSelected) | ||
2392 | { | ||
2393 | Thread.Sleep(20); | ||
2394 | if (IsPhysical) | ||
2395 | { | ||
2396 | if (Body != IntPtr.Zero) | ||
2397 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); | ||
2398 | } | ||
2399 | |||
2400 | //resetCollisionAccounting(); | ||
2401 | } | ||
2402 | m_taintVelocity = Vector3.Zero; | ||
2403 | } | ||
2404 | |||
2405 | public void UpdatePositionAndVelocity() | ||
2406 | { | ||
2407 | return; // moved to the Move () method | ||
2408 | } | ||
2409 | |||
2410 | public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | ||
2411 | { | ||
2412 | obj.I.M00 = pMat[0, 0]; | ||
2413 | obj.I.M01 = pMat[0, 1]; | ||
2414 | obj.I.M02 = pMat[0, 2]; | ||
2415 | obj.I.M10 = pMat[1, 0]; | ||
2416 | obj.I.M11 = pMat[1, 1]; | ||
2417 | obj.I.M12 = pMat[1, 2]; | ||
2418 | obj.I.M20 = pMat[2, 0]; | ||
2419 | obj.I.M21 = pMat[2, 1]; | ||
2420 | obj.I.M22 = pMat[2, 2]; | ||
2421 | return obj; | ||
2422 | } | ||
2423 | |||
2424 | public override void SubscribeEvents(int ms) | ||
2425 | { | ||
2426 | m_eventsubscription = ms; | ||
2427 | _parent_scene.addCollisionEventReporting(this); | ||
2428 | } | ||
2429 | |||
2430 | public override void UnSubscribeEvents() | ||
2431 | { | ||
2432 | _parent_scene.remCollisionEventReporting(this); | ||
2433 | m_eventsubscription = 0; | ||
2434 | } | ||
2435 | |||
2436 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
2437 | { | ||
2438 | if (CollisionEventsThisFrame == null) | ||
2439 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
2440 | CollisionEventsThisFrame.addCollider(CollidedWith, contact); | ||
2441 | } | ||
2442 | |||
2443 | public void SendCollisions() | ||
2444 | { | ||
2445 | if (CollisionEventsThisFrame == null) | ||
2446 | return; | ||
2447 | |||
2448 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
2449 | |||
2450 | if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) | ||
2451 | CollisionEventsThisFrame = null; | ||
2452 | else | ||
2453 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
2454 | } | ||
2455 | |||
2456 | public override bool SubscribedEvents() | ||
2457 | { | ||
2458 | if (m_eventsubscription > 0) | ||
2459 | return true; | ||
2460 | return false; | ||
2461 | } | ||
2462 | |||
2463 | public static Matrix4 Inverse(Matrix4 pMat) | ||
2464 | { | ||
2465 | if (determinant3x3(pMat) == 0) | ||
2466 | { | ||
2467 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
2468 | } | ||
2469 | |||
2470 | |||
2471 | |||
2472 | return (Adjoint(pMat) / determinant3x3(pMat)); | ||
2473 | } | ||
2474 | |||
2475 | public static Matrix4 Adjoint(Matrix4 pMat) | ||
2476 | { | ||
2477 | Matrix4 adjointMatrix = new Matrix4(); | ||
2478 | for (int i=0; i<4; i++) | ||
2479 | { | ||
2480 | for (int j=0; j<4; j++) | ||
2481 | { | ||
2482 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
2483 | } | ||
2484 | } | ||
2485 | |||
2486 | adjointMatrix = Transpose(adjointMatrix); | ||
2487 | return adjointMatrix; | ||
2488 | } | ||
2489 | |||
2490 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | ||
2491 | { | ||
2492 | Matrix4 minor = new Matrix4(); | ||
2493 | int m = 0, n = 0; | ||
2494 | for (int i = 0; i < 4; i++) | ||
2495 | { | ||
2496 | if (i == iRow) | ||
2497 | continue; | ||
2498 | n = 0; | ||
2499 | for (int j = 0; j < 4; j++) | ||
2500 | { | ||
2501 | if (j == iCol) | ||
2502 | continue; | ||
2503 | Matrix4SetValue(ref minor, m,n, matrix[i, j]); | ||
2504 | n++; | ||
2505 | } | ||
2506 | m++; | ||
2507 | } | ||
2508 | return minor; | ||
2509 | } | ||
2510 | |||
2511 | public static Matrix4 Transpose(Matrix4 pMat) | ||
2512 | { | ||
2513 | Matrix4 transposeMatrix = new Matrix4(); | ||
2514 | for (int i = 0; i < 4; i++) | ||
2515 | for (int j = 0; j < 4; j++) | ||
2516 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
2517 | return transposeMatrix; | ||
2518 | } | ||
2519 | |||
2520 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | ||
2521 | { | ||
2522 | switch (r) | ||
2523 | { | ||
2524 | case 0: | ||
2525 | switch (c) | ||
2526 | { | ||
2527 | case 0: | ||
2528 | pMat.M11 = val; | ||
2529 | break; | ||
2530 | case 1: | ||
2531 | pMat.M12 = val; | ||
2532 | break; | ||
2533 | case 2: | ||
2534 | pMat.M13 = val; | ||
2535 | break; | ||
2536 | case 3: | ||
2537 | pMat.M14 = val; | ||
2538 | break; | ||
2539 | } | ||
2540 | |||
2541 | break; | ||
2542 | case 1: | ||
2543 | switch (c) | ||
2544 | { | ||
2545 | case 0: | ||
2546 | pMat.M21 = val; | ||
2547 | break; | ||
2548 | case 1: | ||
2549 | pMat.M22 = val; | ||
2550 | break; | ||
2551 | case 2: | ||
2552 | pMat.M23 = val; | ||
2553 | break; | ||
2554 | case 3: | ||
2555 | pMat.M24 = val; | ||
2556 | break; | ||
2557 | } | ||
2558 | |||
2559 | break; | ||
2560 | case 2: | ||
2561 | switch (c) | ||
2562 | { | ||
2563 | case 0: | ||
2564 | pMat.M31 = val; | ||
2565 | break; | ||
2566 | case 1: | ||
2567 | pMat.M32 = val; | ||
2568 | break; | ||
2569 | case 2: | ||
2570 | pMat.M33 = val; | ||
2571 | break; | ||
2572 | case 3: | ||
2573 | pMat.M34 = val; | ||
2574 | break; | ||
2575 | } | ||
2576 | |||
2577 | break; | ||
2578 | case 3: | ||
2579 | switch (c) | ||
2580 | { | ||
2581 | case 0: | ||
2582 | pMat.M41 = val; | ||
2583 | break; | ||
2584 | case 1: | ||
2585 | pMat.M42 = val; | ||
2586 | break; | ||
2587 | case 2: | ||
2588 | pMat.M43 = val; | ||
2589 | break; | ||
2590 | case 3: | ||
2591 | pMat.M44 = val; | ||
2592 | break; | ||
2593 | } | ||
2594 | |||
2595 | break; | ||
2596 | } | ||
2597 | } | ||
2598 | private static float determinant3x3(Matrix4 pMat) | ||
2599 | { | ||
2600 | float det = 0; | ||
2601 | float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; | ||
2602 | float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; | ||
2603 | float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; | ||
2604 | float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; | ||
2605 | float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; | ||
2606 | float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; | ||
2607 | |||
2608 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
2609 | return det; | ||
2610 | |||
2611 | } | ||
2612 | |||
2613 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
2614 | { | ||
2615 | dst.c.W = src.c.W; | ||
2616 | dst.c.X = src.c.X; | ||
2617 | dst.c.Y = src.c.Y; | ||
2618 | dst.c.Z = src.c.Z; | ||
2619 | dst.mass = src.mass; | ||
2620 | dst.I.M00 = src.I.M00; | ||
2621 | dst.I.M01 = src.I.M01; | ||
2622 | dst.I.M02 = src.I.M02; | ||
2623 | dst.I.M10 = src.I.M10; | ||
2624 | dst.I.M11 = src.I.M11; | ||
2625 | dst.I.M12 = src.I.M12; | ||
2626 | dst.I.M20 = src.I.M20; | ||
2627 | dst.I.M21 = src.I.M21; | ||
2628 | dst.I.M22 = src.I.M22; | ||
2629 | } | ||
2630 | |||
2631 | public override void SetMaterial(int pMaterial) | ||
2632 | { | ||
2633 | m_material = pMaterial; | ||
2634 | } | ||
2635 | |||
2636 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
2637 | { | ||
2638 | switch (pParam) | ||
2639 | { | ||
2640 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
2641 | if (pValue < 0.01f) pValue = 0.01f; | ||
2642 | // m_angularDeflectionEfficiency = pValue; | ||
2643 | break; | ||
2644 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
2645 | if (pValue < 0.1f) pValue = 0.1f; | ||
2646 | // m_angularDeflectionTimescale = pValue; | ||
2647 | break; | ||
2648 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
2649 | if (pValue < 0.3f) pValue = 0.3f; | ||
2650 | m_angularMotorDecayTimescale = pValue; | ||
2651 | break; | ||
2652 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
2653 | if (pValue < 0.3f) pValue = 0.3f; | ||
2654 | m_angularMotorTimescale = pValue; | ||
2655 | break; | ||
2656 | case Vehicle.BANKING_EFFICIENCY: | ||
2657 | if (pValue < 0.01f) pValue = 0.01f; | ||
2658 | // m_bankingEfficiency = pValue; | ||
2659 | break; | ||
2660 | case Vehicle.BANKING_MIX: | ||
2661 | if (pValue < 0.01f) pValue = 0.01f; | ||
2662 | // m_bankingMix = pValue; | ||
2663 | break; | ||
2664 | case Vehicle.BANKING_TIMESCALE: | ||
2665 | if (pValue < 0.01f) pValue = 0.01f; | ||
2666 | // m_bankingTimescale = pValue; | ||
2667 | break; | ||
2668 | case Vehicle.BUOYANCY: | ||
2669 | if (pValue < -1f) pValue = -1f; | ||
2670 | if (pValue > 1f) pValue = 1f; | ||
2671 | m_VehicleBuoyancy = pValue; | ||
2672 | break; | ||
2673 | // case Vehicle.HOVER_EFFICIENCY: | ||
2674 | // if (pValue < 0f) pValue = 0f; | ||
2675 | // if (pValue > 1f) pValue = 1f; | ||
2676 | // m_VhoverEfficiency = pValue; | ||
2677 | // break; | ||
2678 | case Vehicle.HOVER_HEIGHT: | ||
2679 | m_VhoverHeight = pValue; | ||
2680 | break; | ||
2681 | case Vehicle.HOVER_TIMESCALE: | ||
2682 | if (pValue < 0.1f) pValue = 0.1f; | ||
2683 | m_VhoverTimescale = pValue; | ||
2684 | break; | ||
2685 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
2686 | if (pValue < 0.01f) pValue = 0.01f; | ||
2687 | // m_linearDeflectionEfficiency = pValue; | ||
2688 | break; | ||
2689 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
2690 | if (pValue < 0.01f) pValue = 0.01f; | ||
2691 | // m_linearDeflectionTimescale = pValue; | ||
2692 | break; | ||
2693 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
2694 | if (pValue < 0.3f) pValue = 0.3f; | ||
2695 | m_linearMotorDecayTimescale = pValue; | ||
2696 | break; | ||
2697 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
2698 | if (pValue < 0.1f) pValue = 0.1f; | ||
2699 | m_linearMotorTimescale = pValue; | ||
2700 | break; | ||
2701 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
2702 | if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable | ||
2703 | if (pValue > 1.0f) pValue = 1.0f; | ||
2704 | m_verticalAttractionEfficiency = pValue; | ||
2705 | break; | ||
2706 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
2707 | if (pValue < 0.1f) pValue = 0.1f; | ||
2708 | m_verticalAttractionTimescale = pValue; | ||
2709 | break; | ||
2710 | |||
2711 | // These are vector properties but the engine lets you use a single float value to | ||
2712 | // set all of the components to the same value | ||
2713 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
2714 | if (pValue > 30f) pValue = 30f; | ||
2715 | if (pValue < 0.1f) pValue = 0.1f; | ||
2716 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
2717 | break; | ||
2718 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
2719 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
2720 | UpdateAngDecay(); | ||
2721 | break; | ||
2722 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
2723 | if (pValue < 0.1f) pValue = 0.1f; | ||
2724 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
2725 | break; | ||
2726 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
2727 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
2728 | UpdateLinDecay(); | ||
2729 | break; | ||
2730 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
2731 | // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
2732 | break; | ||
2733 | |||
2734 | } | ||
2735 | |||
2736 | }//end ProcessFloatVehicleParam | ||
2737 | |||
2738 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
2739 | { | ||
2740 | switch (pParam) | ||
2741 | { | ||
2742 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
2743 | if (pValue.X > 30f) pValue.X = 30f; | ||
2744 | if (pValue.X < 0.1f) pValue.X = 0.1f; | ||
2745 | if (pValue.Y > 30f) pValue.Y = 30f; | ||
2746 | if (pValue.Y < 0.1f) pValue.Y = 0.1f; | ||
2747 | if (pValue.Z > 30f) pValue.Z = 30f; | ||
2748 | if (pValue.Z < 0.1f) pValue.Z = 0.1f; | ||
2749 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
2750 | break; | ||
2751 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
2752 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
2753 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
2754 | if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; | ||
2755 | if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; | ||
2756 | if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; | ||
2757 | if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; | ||
2758 | if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; | ||
2759 | if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; | ||
2760 | UpdateAngDecay(); | ||
2761 | break; | ||
2762 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
2763 | if (pValue.X < 0.1f) pValue.X = 0.1f; | ||
2764 | if (pValue.Y < 0.1f) pValue.Y = 0.1f; | ||
2765 | if (pValue.Z < 0.1f) pValue.Z = 0.1f; | ||
2766 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
2767 | break; | ||
2768 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
2769 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting | ||
2770 | UpdateLinDecay(); | ||
2771 | break; | ||
2772 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
2773 | // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
2774 | break; | ||
2775 | } | ||
2776 | |||
2777 | }//end ProcessVectorVehicleParam | ||
2778 | |||
2779 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
2780 | { | ||
2781 | switch (pParam) | ||
2782 | { | ||
2783 | case Vehicle.REFERENCE_FRAME: | ||
2784 | // m_referenceFrame = pValue; | ||
2785 | break; | ||
2786 | } | ||
2787 | |||
2788 | }//end ProcessRotationVehicleParam | ||
2789 | |||
2790 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
2791 | { | ||
2792 | if (remove) | ||
2793 | { | ||
2794 | m_flags &= ~((VehicleFlag)pParam); | ||
2795 | } | ||
2796 | else | ||
2797 | { | ||
2798 | m_flags |= (VehicleFlag)pParam; | ||
2799 | } | ||
2800 | } | ||
2801 | |||
2802 | internal void ProcessTypeChange(Vehicle pType) | ||
2803 | { | ||
2804 | // Set Defaults For Type | ||
2805 | m_type = pType; | ||
2806 | switch (pType) | ||
2807 | { | ||
2808 | case Vehicle.TYPE_SLED: | ||
2809 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
2810 | m_angularFrictionTimescale = new Vector3(30, 30, 30); | ||
2811 | // m_lLinMotorVel = Vector3.Zero; | ||
2812 | m_linearMotorTimescale = 1000; | ||
2813 | m_linearMotorDecayTimescale = 120; | ||
2814 | m_angularMotorDirection = Vector3.Zero; | ||
2815 | m_angularMotorDVel = Vector3.Zero; | ||
2816 | m_angularMotorTimescale = 1000; | ||
2817 | m_angularMotorDecayTimescale = 120; | ||
2818 | m_VhoverHeight = 0; | ||
2819 | // m_VhoverEfficiency = 1; | ||
2820 | m_VhoverTimescale = 10; | ||
2821 | m_VehicleBuoyancy = 0; | ||
2822 | // m_linearDeflectionEfficiency = 1; | ||
2823 | // m_linearDeflectionTimescale = 1; | ||
2824 | // m_angularDeflectionEfficiency = 1; | ||
2825 | // m_angularDeflectionTimescale = 1000; | ||
2826 | // m_bankingEfficiency = 0; | ||
2827 | // m_bankingMix = 1; | ||
2828 | // m_bankingTimescale = 10; | ||
2829 | // m_referenceFrame = Quaternion.Identity; | ||
2830 | m_flags &= | ||
2831 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
2832 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
2833 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
2834 | break; | ||
2835 | case Vehicle.TYPE_CAR: | ||
2836 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
2837 | m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. | ||
2838 | // m_lLinMotorVel = Vector3.Zero; | ||
2839 | m_linearMotorTimescale = 1; | ||
2840 | m_linearMotorDecayTimescale = 60; | ||
2841 | m_angularMotorDirection = Vector3.Zero; | ||
2842 | m_angularMotorDVel = Vector3.Zero; | ||
2843 | m_angularMotorTimescale = 1; | ||
2844 | m_angularMotorDecayTimescale = 0.8f; | ||
2845 | m_VhoverHeight = 0; | ||
2846 | // m_VhoverEfficiency = 0; | ||
2847 | m_VhoverTimescale = 1000; | ||
2848 | m_VehicleBuoyancy = 0; | ||
2849 | // // m_linearDeflectionEfficiency = 1; | ||
2850 | // // m_linearDeflectionTimescale = 2; | ||
2851 | // // m_angularDeflectionEfficiency = 0; | ||
2852 | // m_angularDeflectionTimescale = 10; | ||
2853 | m_verticalAttractionEfficiency = 1f; | ||
2854 | m_verticalAttractionTimescale = 10f; | ||
2855 | // m_bankingEfficiency = -0.2f; | ||
2856 | // m_bankingMix = 1; | ||
2857 | // m_bankingTimescale = 1; | ||
2858 | // m_referenceFrame = Quaternion.Identity; | ||
2859 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
2860 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | | ||
2861 | VehicleFlag.LIMIT_MOTOR_UP); | ||
2862 | break; | ||
2863 | case Vehicle.TYPE_BOAT: | ||
2864 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
2865 | m_angularFrictionTimescale = new Vector3(10,10,10); | ||
2866 | // m_lLinMotorVel = Vector3.Zero; | ||
2867 | m_linearMotorTimescale = 5; | ||
2868 | m_linearMotorDecayTimescale = 60; | ||
2869 | m_angularMotorDirection = Vector3.Zero; | ||
2870 | m_angularMotorDVel = Vector3.Zero; | ||
2871 | m_angularMotorTimescale = 4; | ||
2872 | m_angularMotorDecayTimescale = 4; | ||
2873 | m_VhoverHeight = 0; | ||
2874 | // m_VhoverEfficiency = 0.5f; | ||
2875 | m_VhoverTimescale = 2; | ||
2876 | m_VehicleBuoyancy = 1; | ||
2877 | // m_linearDeflectionEfficiency = 0.5f; | ||
2878 | // m_linearDeflectionTimescale = 3; | ||
2879 | // m_angularDeflectionEfficiency = 0.5f; | ||
2880 | // m_angularDeflectionTimescale = 5; | ||
2881 | m_verticalAttractionEfficiency = 0.5f; | ||
2882 | m_verticalAttractionTimescale = 5f; | ||
2883 | // m_bankingEfficiency = -0.3f; | ||
2884 | // m_bankingMix = 0.8f; | ||
2885 | // m_bankingTimescale = 1; | ||
2886 | // m_referenceFrame = Quaternion.Identity; | ||
2887 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | | ||
2888 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
2889 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | | ||
2890 | VehicleFlag.LIMIT_MOTOR_UP); | ||
2891 | break; | ||
2892 | case Vehicle.TYPE_AIRPLANE: | ||
2893 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
2894 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
2895 | // m_lLinMotorVel = Vector3.Zero; | ||
2896 | m_linearMotorTimescale = 2; | ||
2897 | m_linearMotorDecayTimescale = 60; | ||
2898 | m_angularMotorDirection = Vector3.Zero; | ||
2899 | m_angularMotorDVel = Vector3.Zero; | ||
2900 | m_angularMotorTimescale = 4; | ||
2901 | m_angularMotorDecayTimescale = 4; | ||
2902 | m_VhoverHeight = 0; | ||
2903 | // m_VhoverEfficiency = 0.5f; | ||
2904 | m_VhoverTimescale = 1000; | ||
2905 | m_VehicleBuoyancy = 0; | ||
2906 | // m_linearDeflectionEfficiency = 0.5f; | ||
2907 | // m_linearDeflectionTimescale = 3; | ||
2908 | // m_angularDeflectionEfficiency = 1; | ||
2909 | // m_angularDeflectionTimescale = 2; | ||
2910 | m_verticalAttractionEfficiency = 0.9f; | ||
2911 | m_verticalAttractionTimescale = 2f; | ||
2912 | // m_bankingEfficiency = 1; | ||
2913 | // m_bankingMix = 0.7f; | ||
2914 | // m_bankingTimescale = 2; | ||
2915 | // m_referenceFrame = Quaternion.Identity; | ||
2916 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
2917 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
2918 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
2919 | break; | ||
2920 | case Vehicle.TYPE_BALLOON: | ||
2921 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
2922 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
2923 | m_linearMotorTimescale = 5; | ||
2924 | m_linearMotorDecayTimescale = 60; | ||
2925 | m_angularMotorDirection = Vector3.Zero; | ||
2926 | m_angularMotorDVel = Vector3.Zero; | ||
2927 | m_angularMotorTimescale = 6; | ||
2928 | m_angularMotorDecayTimescale = 10; | ||
2929 | m_VhoverHeight = 5; | ||
2930 | // m_VhoverEfficiency = 0.8f; | ||
2931 | m_VhoverTimescale = 10; | ||
2932 | m_VehicleBuoyancy = 1; | ||
2933 | // m_linearDeflectionEfficiency = 0; | ||
2934 | // m_linearDeflectionTimescale = 5; | ||
2935 | // m_angularDeflectionEfficiency = 0; | ||
2936 | // m_angularDeflectionTimescale = 5; | ||
2937 | m_verticalAttractionEfficiency = 1f; | ||
2938 | m_verticalAttractionTimescale = 100f; | ||
2939 | // m_bankingEfficiency = 0; | ||
2940 | // m_bankingMix = 0.7f; | ||
2941 | // m_bankingTimescale = 5; | ||
2942 | // m_referenceFrame = Quaternion.Identity; | ||
2943 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
2944 | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
2945 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
2946 | break; | ||
2947 | |||
2948 | } | ||
2949 | }//end SetDefaultsForType | ||
2950 | |||
2951 | internal void Enable(IntPtr pBody, OdeScene pParentScene) | ||
2952 | { | ||
2953 | if (m_type == Vehicle.TYPE_NONE) | ||
2954 | return; | ||
2955 | |||
2956 | m_body = pBody; | ||
2957 | } | ||
2958 | |||
2959 | |||
2960 | internal void Halt() | ||
2961 | { // Kill all motions, when non-physical | ||
2962 | // m_linearMotorDirection = Vector3.Zero; | ||
2963 | m_lLinMotorDVel = Vector3.Zero; | ||
2964 | m_lLinObjectVel = Vector3.Zero; | ||
2965 | m_wLinObjectVel = Vector3.Zero; | ||
2966 | m_angularMotorDirection = Vector3.Zero; | ||
2967 | m_lastAngularVelocity = Vector3.Zero; | ||
2968 | m_angularMotorDVel = Vector3.Zero; | ||
2969 | _acceleration = Vector3.Zero; | ||
2970 | } | ||
2971 | |||
2972 | private void UpdateLinDecay() | ||
2973 | { | ||
2974 | // if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; | ||
2975 | // if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; | ||
2976 | // if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; | ||
2977 | m_lLinMotorDVel.X = m_linearMotorDirection.X; | ||
2978 | m_lLinMotorDVel.Y = m_linearMotorDirection.Y; | ||
2979 | m_lLinMotorDVel.Z = m_linearMotorDirection.Z; | ||
2980 | } // else let the motor decay on its own | ||
2981 | |||
2982 | private void UpdateAngDecay() | ||
2983 | { | ||
2984 | // if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; | ||
2985 | // if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; | ||
2986 | // if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; | ||
2987 | m_angularMotorDVel.X = m_angularMotorDirection.X; | ||
2988 | m_angularMotorDVel.Y = m_angularMotorDirection.Y; | ||
2989 | m_angularMotorDVel.Z = m_angularMotorDirection.Z; | ||
2990 | } // else let the motor decay on its own | ||
2991 | |||
2992 | public void Move(float timestep) | ||
2993 | { | ||
2994 | float fx = 0; | ||
2995 | float fy = 0; | ||
2996 | float fz = 0; | ||
2997 | Vector3 linvel; // velocity applied, including any reversal | ||
2998 | int outside = 0; | ||
2999 | |||
3000 | // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. | ||
3001 | // This is a temp patch until proper region crossing is developed. | ||
3002 | |||
3003 | int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; | ||
3004 | float fence = _parent_scene.geomRegionFence; | ||
3005 | |||
3006 | frcount++; // used to limit debug comment output | ||
3007 | if (frcount > 50) | ||
3008 | frcount = 0; | ||
3009 | |||
3010 | if(revcount > 0) revcount--; | ||
3011 | |||
3012 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. | ||
3013 | { | ||
3014 | // Old public void UpdatePositionAndVelocity(), more accuratley calculated here | ||
3015 | bool lastZeroFlag = _zeroFlag; // was it stopped | ||
3016 | |||
3017 | d.Vector3 vec = d.BodyGetPosition(Body); | ||
3018 | Vector3 l_position = Vector3.Zero; | ||
3019 | l_position.X = vec.X; | ||
3020 | l_position.Y = vec.Y; | ||
3021 | l_position.Z = vec.Z; | ||
3022 | m_lastposition = _position; | ||
3023 | _position = l_position; | ||
3024 | |||
3025 | d.Quaternion ori = d.BodyGetQuaternion(Body); | ||
3026 | // Quaternion l_orientation = Quaternion.Identity; | ||
3027 | _orientation.X = ori.X; | ||
3028 | _orientation.Y = ori.Y; | ||
3029 | _orientation.Z = ori.Z; | ||
3030 | _orientation.W = ori.W; | ||
3031 | m_lastorientation = _orientation; | ||
3032 | |||
3033 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3034 | m_lastVelocity = _velocity; | ||
3035 | _velocity.X = vel.X; | ||
3036 | _velocity.Y = vel.Y; | ||
3037 | _velocity.Z = vel.Z; | ||
3038 | _acceleration = ((_velocity - m_lastVelocity) / timestep); | ||
3039 | |||
3040 | d.Vector3 torque = d.BodyGetTorque(Body); | ||
3041 | _torque = new Vector3(torque.X, torque.Y, torque.Z); | ||
3042 | |||
3043 | base.RequestPhysicsterseUpdate(); | ||
3044 | |||
3045 | //Console.WriteLine("Move {0} at {1}", m_primName, l_position); | ||
3046 | |||
3047 | // Check if outside region | ||
3048 | // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! | ||
3049 | if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) | ||
3050 | { | ||
3051 | l_position.X = ((float)_parent_scene.WorldExtents.X - fence); | ||
3052 | outside = 1; | ||
3053 | } | ||
3054 | |||
3055 | if (l_position.X < fence) | ||
3056 | { | ||
3057 | l_position.X = fence; | ||
3058 | outside = 2; | ||
3059 | } | ||
3060 | if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) | ||
3061 | { | ||
3062 | l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); | ||
3063 | outside = 3; | ||
3064 | } | ||
3065 | |||
3066 | if (l_position.Y < fence) | ||
3067 | { | ||
3068 | l_position.Y = fence; | ||
3069 | outside = 4; | ||
3070 | } | ||
3071 | |||
3072 | if (outside > 0) | ||
3073 | { | ||
3074 | |||
3075 | //Console.WriteLine("Border {0} fence={1}", l_position, fence); | ||
3076 | if (fence > 0.0f) // bounce object off boundary | ||
3077 | { | ||
3078 | if (revcount == 0) | ||
3079 | { | ||
3080 | if (outside < 3) | ||
3081 | { | ||
3082 | _velocity.X = -_velocity.X; | ||
3083 | } | ||
3084 | else | ||
3085 | { | ||
3086 | _velocity.Y = -_velocity.Y; | ||
3087 | } | ||
3088 | if (m_type != Vehicle.TYPE_NONE) Halt(); | ||
3089 | _position = l_position; | ||
3090 | m_taintposition = _position; | ||
3091 | m_lastVelocity = _velocity; | ||
3092 | _acceleration = Vector3.Zero; | ||
3093 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
3094 | d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); | ||
3095 | base.RequestPhysicsterseUpdate(); | ||
3096 | |||
3097 | revcount = 25; // wait for object to move away from border | ||
3098 | } | ||
3099 | } // else old crossing mode | ||
3100 | else if (m_crossingfailures < failureLimit) | ||
3101 | { // keep trying to cross? | ||
3102 | _position = l_position; | ||
3103 | //_parent_scene.remActivePrim(this); | ||
3104 | if (_parent == null) base.RequestPhysicsterseUpdate(); | ||
3105 | return; // Dont process any other motion? | ||
3106 | } | ||
3107 | else | ||
3108 | { // Too many tries | ||
3109 | if (_parent == null) base.RaiseOutOfBounds(l_position); | ||
3110 | //Console.WriteLine("ROOB 2"); | ||
3111 | |||
3112 | return; // Dont process any other motion? | ||
3113 | } // end various methods | ||
3114 | } // end outside region horizontally | ||
3115 | |||
3116 | |||
3117 | if (l_position.Z < 0) | ||
3118 | { | ||
3119 | // This is so prim that get lost underground don't fall forever and suck up | ||
3120 | // | ||
3121 | // Sim resources and memory. | ||
3122 | // Disables the prim's movement physics.... | ||
3123 | // It's a hack and will generate a console message if it fails. | ||
3124 | |||
3125 | //IsPhysical = false; | ||
3126 | if (_parent == null) base.RaiseOutOfBounds(_position); | ||
3127 | //Console.WriteLine("ROOB 3"); | ||
3128 | |||
3129 | |||
3130 | _acceleration.X = 0; // This stuff may stop client display but it has no | ||
3131 | _acceleration.Y = 0; // effect on the object in phys engine! | ||
3132 | _acceleration.Z = 0; | ||
3133 | |||
3134 | _velocity.X = 0; | ||
3135 | _velocity.Y = 0; | ||
3136 | _velocity.Z = 0; | ||
3137 | m_rotationalVelocity.X = 0; | ||
3138 | m_rotationalVelocity.Y = 0; | ||
3139 | m_rotationalVelocity.Z = 0; | ||
3140 | |||
3141 | if (_parent == null) base.RequestPhysicsterseUpdate(); | ||
3142 | |||
3143 | m_throttleUpdates = false; | ||
3144 | throttleCounter = 0; | ||
3145 | _zeroFlag = true; | ||
3146 | //outofBounds = true; | ||
3147 | } // end neg Z check | ||
3148 | |||
3149 | // Is it moving? | ||
3150 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | ||
3151 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | ||
3152 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) | ||
3153 | && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large | ||
3154 | { | ||
3155 | _zeroFlag = true; | ||
3156 | m_throttleUpdates = false; | ||
3157 | } | ||
3158 | else | ||
3159 | { | ||
3160 | //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); | ||
3161 | _zeroFlag = false; | ||
3162 | m_lastUpdateSent = false; | ||
3163 | //m_throttleUpdates = false; | ||
3164 | } | ||
3165 | |||
3166 | if (_zeroFlag) | ||
3167 | { // Its stopped | ||
3168 | _velocity.X = 0.0f; | ||
3169 | _velocity.Y = 0.0f; | ||
3170 | _velocity.Z = 0.0f; | ||
3171 | |||
3172 | _acceleration.X = 0; | ||
3173 | _acceleration.Y = 0; | ||
3174 | _acceleration.Z = 0; | ||
3175 | |||
3176 | m_rotationalVelocity.X = 0; | ||
3177 | m_rotationalVelocity.Y = 0; | ||
3178 | m_rotationalVelocity.Z = 0; | ||
3179 | if (!m_lastUpdateSent) | ||
3180 | { | ||
3181 | m_throttleUpdates = false; | ||
3182 | throttleCounter = 0; | ||
3183 | if (_parent == null) | ||
3184 | { | ||
3185 | base.RequestPhysicsterseUpdate(); | ||
3186 | } | ||
3187 | |||
3188 | m_lastUpdateSent = true; | ||
3189 | } | ||
3190 | } | ||
3191 | else | ||
3192 | { // Its moving | ||
3193 | if (lastZeroFlag != _zeroFlag) | ||
3194 | { | ||
3195 | if (_parent == null) | ||
3196 | { | ||
3197 | base.RequestPhysicsterseUpdate(); | ||
3198 | } | ||
3199 | } | ||
3200 | m_lastUpdateSent = false; | ||
3201 | if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) | ||
3202 | { | ||
3203 | if (_parent == null) | ||
3204 | { | ||
3205 | base.RequestPhysicsterseUpdate(); | ||
3206 | } | ||
3207 | } | ||
3208 | else | ||
3209 | { | ||
3210 | throttleCounter++; | ||
3211 | } | ||
3212 | } | ||
3213 | m_lastposition = l_position; | ||
3214 | |||
3215 | /// End UpdatePositionAndVelocity insert | ||
3216 | |||
3217 | |||
3218 | // Rotation lock ===================================== | ||
3219 | if(m_rotateEnableUpdate) | ||
3220 | { | ||
3221 | // Snapshot current angles, set up Amotor(s) | ||
3222 | m_rotateEnableUpdate = false; | ||
3223 | m_rotateEnable = m_rotateEnableRequest; | ||
3224 | Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); | ||
3225 | |||
3226 | if (Amotor != IntPtr.Zero) | ||
3227 | { | ||
3228 | d.JointDestroy(Amotor); | ||
3229 | Amotor = IntPtr.Zero; | ||
3230 | Console.WriteLine("Old Amotor Destroyed"); | ||
3231 | } | ||
3232 | |||
3233 | if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) | ||
3234 | { // not all are enabled | ||
3235 | d.Quaternion r = d.BodyGetQuaternion(Body); | ||
3236 | Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); | ||
3237 | // extract the axes vectors | ||
3238 | Vector3 vX = new Vector3(1f,0f,0f); | ||
3239 | Vector3 vY = new Vector3(0f,1f,0f); | ||
3240 | Vector3 vZ = new Vector3(0f,0f,1f); | ||
3241 | vX = vX * locrot; | ||
3242 | vY = vY * locrot; | ||
3243 | vZ = vZ * locrot; | ||
3244 | // snapshot the current angle vectors | ||
3245 | m_lockX = vX; | ||
3246 | m_lockY = vY; | ||
3247 | m_lockZ = vZ; | ||
3248 | // m_lockRot = locrot; | ||
3249 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
3250 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
3251 | d.JointSetAMotorMode(Amotor, 0); // User mode?? | ||
3252 | Console.WriteLine("New Amotor Created for {0}", m_primName); | ||
3253 | |||
3254 | float axisnum = 3; // how many to lock | ||
3255 | axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); | ||
3256 | d.JointSetAMotorNumAxes(Amotor,(int)axisnum); | ||
3257 | Console.WriteLine("AxisNum={0}",(int)axisnum); | ||
3258 | |||
3259 | int i = 0; | ||
3260 | |||
3261 | if (m_rotateEnable.X == 0) | ||
3262 | { | ||
3263 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); | ||
3264 | Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); | ||
3265 | i++; | ||
3266 | } | ||
3267 | |||
3268 | if (m_rotateEnable.Y == 0) | ||
3269 | { | ||
3270 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); | ||
3271 | Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); | ||
3272 | i++; | ||
3273 | } | ||
3274 | |||
3275 | if (m_rotateEnable.Z == 0) | ||
3276 | { | ||
3277 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); | ||
3278 | Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); | ||
3279 | i++; | ||
3280 | } | ||
3281 | |||
3282 | // These lowstops and high stops are effectively (no wiggle room) | ||
3283 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f); | ||
3284 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); | ||
3285 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f); | ||
3286 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
3287 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
3288 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3289 | d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); | ||
3290 | d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); | ||
3291 | d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); | ||
3292 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); | ||
3293 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); | ||
3294 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); | ||
3295 | } // else none are locked | ||
3296 | } // end Rotation Update | ||
3297 | |||
3298 | |||
3299 | // VEHICLE processing ========================================== | ||
3300 | if (m_type != Vehicle.TYPE_NONE) | ||
3301 | { | ||
3302 | // get body attitude | ||
3303 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
3304 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
3305 | Quaternion irotq = Quaternion.Inverse(rotq); | ||
3306 | |||
3307 | // VEHICLE Linear Motion | ||
3308 | d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame | ||
3309 | Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); | ||
3310 | m_lLinObjectVel = vel_now * irotq; | ||
3311 | |||
3312 | if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate | ||
3313 | { | ||
3314 | if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) | ||
3315 | { | ||
3316 | float decayfactor = m_linearMotorDecayTimescale/timestep; | ||
3317 | Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); | ||
3318 | m_lLinMotorDVel -= decayAmount; | ||
3319 | } | ||
3320 | else | ||
3321 | { | ||
3322 | float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); | ||
3323 | Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; | ||
3324 | m_lLinMotorDVel -= decel; | ||
3325 | } | ||
3326 | if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) | ||
3327 | { | ||
3328 | m_lLinMotorDVel = Vector3.Zero; | ||
3329 | } | ||
3330 | |||
3331 | /* else | ||
3332 | { | ||
3333 | if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; | ||
3334 | if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; | ||
3335 | if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; | ||
3336 | } */ | ||
3337 | } // end linear motor decay | ||
3338 | |||
3339 | if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) | ||
3340 | { | ||
3341 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); | ||
3342 | if (m_linearMotorTimescale < 300.0f) | ||
3343 | { | ||
3344 | Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; | ||
3345 | float linfactor = m_linearMotorTimescale/timestep; | ||
3346 | Vector3 attackAmount = (attack_error/linfactor) * 1.3f; | ||
3347 | m_lLinObjectVel += attackAmount; | ||
3348 | } | ||
3349 | if (m_linearFrictionTimescale.X < 300.0f) | ||
3350 | { | ||
3351 | float fricfactor = m_linearFrictionTimescale.X / timestep; | ||
3352 | float fricX = m_lLinObjectVel.X / fricfactor; | ||
3353 | m_lLinObjectVel.X -= fricX; | ||
3354 | } | ||
3355 | if (m_linearFrictionTimescale.Y < 300.0f) | ||
3356 | { | ||
3357 | float fricfactor = m_linearFrictionTimescale.Y / timestep; | ||
3358 | float fricY = m_lLinObjectVel.Y / fricfactor; | ||
3359 | m_lLinObjectVel.Y -= fricY; | ||
3360 | } | ||
3361 | if (m_linearFrictionTimescale.Z < 300.0f) | ||
3362 | { | ||
3363 | float fricfactor = m_linearFrictionTimescale.Z / timestep; | ||
3364 | float fricZ = m_lLinObjectVel.Z / fricfactor; | ||
3365 | m_lLinObjectVel.Z -= fricZ; | ||
3366 | } | ||
3367 | } | ||
3368 | m_wLinObjectVel = m_lLinObjectVel * rotq; | ||
3369 | |||
3370 | // Gravity and Buoyancy | ||
3371 | Vector3 grav = Vector3.Zero; | ||
3372 | if(m_VehicleBuoyancy < 1.0f) | ||
3373 | { | ||
3374 | // There is some gravity, make a gravity force vector | ||
3375 | // that is applied after object velocity. | ||
3376 | d.Mass objMass; | ||
3377 | d.BodyGetMass(Body, out objMass); | ||
3378 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | ||
3379 | grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force | ||
3380 | } // else its 1.0, no gravity. | ||
3381 | |||
3382 | // Hovering | ||
3383 | if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | ||
3384 | { | ||
3385 | // We should hover, get the target height | ||
3386 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3387 | if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) | ||
3388 | { | ||
3389 | m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; | ||
3390 | } | ||
3391 | else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) | ||
3392 | { | ||
3393 | m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; | ||
3394 | } | ||
3395 | else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) | ||
3396 | { | ||
3397 | m_VhoverTargetHeight = m_VhoverHeight; | ||
3398 | } | ||
3399 | |||
3400 | if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) | ||
3401 | { | ||
3402 | // If body is aready heigher, use its height as target height | ||
3403 | if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; | ||
3404 | } | ||
3405 | |||
3406 | // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped | ||
3407 | // m_VhoverTimescale = 0f; // time to acheive height | ||
3408 | // timestep is time since last frame,in secs | ||
3409 | float herr0 = pos.Z - m_VhoverTargetHeight; | ||
3410 | // Replace Vertical speed with correction figure if significant | ||
3411 | if(Math.Abs(herr0) > 0.01f ) | ||
3412 | { | ||
3413 | //? d.Mass objMass; | ||
3414 | //? d.BodyGetMass(Body, out objMass); | ||
3415 | m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); | ||
3416 | //KF: m_VhoverEfficiency is not yet implemented | ||
3417 | } | ||
3418 | else | ||
3419 | { | ||
3420 | m_wLinObjectVel.Z = 0f; | ||
3421 | } | ||
3422 | } | ||
3423 | else | ||
3424 | { // not hovering | ||
3425 | if (m_wLinObjectVel.Z == 0f) | ||
3426 | { // Gravity rules | ||
3427 | m_wLinObjectVel.Z = vel_now.Z; | ||
3428 | } // else the motor has it | ||
3429 | } | ||
3430 | linvel = m_wLinObjectVel; | ||
3431 | |||
3432 | // Vehicle Linear Motion done ======================================= | ||
3433 | // Apply velocity | ||
3434 | d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); | ||
3435 | // apply gravity force | ||
3436 | d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); | ||
3437 | //if(frcount == 0) Console.WriteLine("Grav {0}", grav); | ||
3438 | // end MoveLinear() | ||
3439 | |||
3440 | |||
3441 | // MoveAngular | ||
3442 | /* | ||
3443 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
3444 | |||
3445 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL | ||
3446 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL | ||
3447 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL | ||
3448 | |||
3449 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor | ||
3450 | private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body | ||
3451 | */ | ||
3452 | //if(frcount == 0) Console.WriteLine("MoveAngular "); | ||
3453 | |||
3454 | d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); | ||
3455 | Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); | ||
3456 | angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation | ||
3457 | |||
3458 | //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); | ||
3459 | |||
3460 | // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. | ||
3461 | float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); | ||
3462 | m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; | ||
3463 | // Decay Angular Motor 2. | ||
3464 | if (m_angularMotorDecayTimescale < 300.0f) | ||
3465 | { | ||
3466 | if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) | ||
3467 | { | ||
3468 | float decayfactor = (m_angularMotorDecayTimescale)/timestep; | ||
3469 | Vector3 decayAmount = (m_angularMotorDVel/decayfactor); | ||
3470 | m_angularMotorDVel -= decayAmount; | ||
3471 | } | ||
3472 | else | ||
3473 | { | ||
3474 | Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; | ||
3475 | m_angularMotorDVel -= decel; | ||
3476 | } | ||
3477 | |||
3478 | if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) | ||
3479 | { | ||
3480 | m_angularMotorDVel = Vector3.Zero; | ||
3481 | } | ||
3482 | else | ||
3483 | { | ||
3484 | if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; | ||
3485 | if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; | ||
3486 | if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; | ||
3487 | } | ||
3488 | } // end decay angular motor | ||
3489 | //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); | ||
3490 | |||
3491 | //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); | ||
3492 | |||
3493 | if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) | ||
3494 | { // if motor or object have motion | ||
3495 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); | ||
3496 | |||
3497 | if (m_angularMotorTimescale < 300.0f) | ||
3498 | { | ||
3499 | Vector3 attack_error = m_angularMotorDVel - angObjectVel; | ||
3500 | float angfactor = m_angularMotorTimescale/timestep; | ||
3501 | Vector3 attackAmount = (attack_error/angfactor); | ||
3502 | angObjectVel += attackAmount; | ||
3503 | //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); | ||
3504 | //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); | ||
3505 | } | ||
3506 | |||
3507 | angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); | ||
3508 | angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); | ||
3509 | angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); | ||
3510 | } // else no signif. motion | ||
3511 | |||
3512 | //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); | ||
3513 | // Bank section tba | ||
3514 | // Deflection section tba | ||
3515 | //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); | ||
3516 | |||
3517 | |||
3518 | /* // Rotation Axis Disables: | ||
3519 | if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) | ||
3520 | { | ||
3521 | if (m_angularEnable.X == 0) | ||
3522 | angObjectVel.X = 0f; | ||
3523 | if (m_angularEnable.Y == 0) | ||
3524 | angObjectVel.Y = 0f; | ||
3525 | if (m_angularEnable.Z == 0) | ||
3526 | angObjectVel.Z = 0f; | ||
3527 | } | ||
3528 | */ | ||
3529 | angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation | ||
3530 | |||
3531 | // Vertical attractor section | ||
3532 | Vector3 vertattr = Vector3.Zero; | ||
3533 | |||
3534 | if(m_verticalAttractionTimescale < 300) | ||
3535 | { | ||
3536 | float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); | ||
3537 | // make a vector pointing up | ||
3538 | Vector3 verterr = Vector3.Zero; | ||
3539 | verterr.Z = 1.0f; | ||
3540 | // rotate it to Body Angle | ||
3541 | verterr = verterr * rotq; | ||
3542 | // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. | ||
3543 | // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go | ||
3544 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. | ||
3545 | |||
3546 | if (verterr.Z < 0.0f) | ||
3547 | { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to | ||
3548 | // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. | ||
3549 | //Console.WriteLine("InvertFlip"); | ||
3550 | verterr.X = 2.0f - verterr.X; | ||
3551 | verterr.Y = 2.0f - verterr.Y; | ||
3552 | } | ||
3553 | verterr *= 0.5f; | ||
3554 | // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) | ||
3555 | Vector3 xyav = angObjectVel; | ||
3556 | xyav.Z = 0.0f; | ||
3557 | if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) | ||
3558 | { | ||
3559 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so | ||
3560 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. | ||
3561 | vertattr.X = verterr.Y; | ||
3562 | vertattr.Y = - verterr.X; | ||
3563 | vertattr.Z = 0f; | ||
3564 | //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); | ||
3565 | |||
3566 | // scaling appears better usingsquare-law | ||
3567 | float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; | ||
3568 | float bounce = 1.0f - damped; | ||
3569 | // 0 = crit damp, 1 = bouncy | ||
3570 | float oavz = angObjectVel.Z; // retain z velocity | ||
3571 | // time-scaled correction, which sums, therefore is bouncy: | ||
3572 | angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; | ||
3573 | // damped, good @ < 90: | ||
3574 | angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); | ||
3575 | angObjectVel.Z = oavz; | ||
3576 | //if(frcount == 0) Console.WriteLine("VA+"); | ||
3577 | //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); | ||
3578 | } | ||
3579 | else | ||
3580 | { | ||
3581 | // else error is very small | ||
3582 | angObjectVel.X = 0f; | ||
3583 | angObjectVel.Y = 0f; | ||
3584 | //if(frcount == 0) Console.WriteLine("VA0"); | ||
3585 | } | ||
3586 | } // else vertical attractor is off | ||
3587 | //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); | ||
3588 | |||
3589 | |||
3590 | m_lastAngularVelocity = angObjectVel; | ||
3591 | // apply Angular Velocity to body | ||
3592 | d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); | ||
3593 | //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); | ||
3594 | |||
3595 | } // end VEHICLES | ||
3596 | else | ||
3597 | { | ||
3598 | // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ | ||
3599 | |||
3600 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | ||
3601 | |||
3602 | /// Dynamics Buoyancy | ||
3603 | //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. | ||
3604 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | ||
3605 | // NB Prims in ODE are no subject to global gravity | ||
3606 | // This should only affect gravity operations | ||
3607 | |||
3608 | float m_mass = CalculateMass(); | ||
3609 | // calculate z-force due togravity on object. | ||
3610 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass | ||
3611 | |||
3612 | if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. | ||
3613 | { | ||
3614 | fz = 0; // llMoveToTarget ignores gravity. | ||
3615 | // it also ignores mass of object, and any physical resting on it. | ||
3616 | // Vector3 m_PIDTarget is where we are going | ||
3617 | // float m_PIDTau is time to get there | ||
3618 | fx = 0; | ||
3619 | fy = 0; | ||
3620 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3621 | Vector3 error = new Vector3( | ||
3622 | (m_PIDTarget.X - pos.X), | ||
3623 | (m_PIDTarget.Y - pos.Y), | ||
3624 | (m_PIDTarget.Z - pos.Z)); | ||
3625 | if (error.ApproxEquals(Vector3.Zero,0.01f)) | ||
3626 | { // Very close, Jump there and quit move | ||
3627 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
3628 | _target_velocity = Vector3.Zero; | ||
3629 | d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3630 | } | ||
3631 | else | ||
3632 | { | ||
3633 | float scale = 50.0f * timestep / m_PIDTau; | ||
3634 | if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) | ||
3635 | { | ||
3636 | // Nearby, quit update of velocity | ||
3637 | } | ||
3638 | else | ||
3639 | { // Far, calc damped velocity | ||
3640 | _target_velocity = error * scale; | ||
3641 | } | ||
3642 | d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3643 | } | ||
3644 | } // end PID MoveToTarget | ||
3645 | |||
3646 | |||
3647 | /// Dynamics Hover =================================================================================== | ||
3648 | // Hover PID Controller can only run if the PIDcontroller is not in use. | ||
3649 | if (m_useHoverPID && !m_usePID) | ||
3650 | { | ||
3651 | //Console.WriteLine("Hover " + m_primName); | ||
3652 | |||
3653 | // If we're using the PID controller, then we have no gravity | ||
3654 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
3655 | |||
3656 | // no lock; for now it's only called from within Simulate() | ||
3657 | |||
3658 | // If the PID Controller isn't active then we set our force | ||
3659 | // calculating base velocity to the current position | ||
3660 | |||
3661 | if ((m_PIDTau < 1)) | ||
3662 | { | ||
3663 | PID_G = PID_G / m_PIDTau; | ||
3664 | } | ||
3665 | |||
3666 | if ((PID_G - m_PIDTau) <= 0) | ||
3667 | { | ||
3668 | PID_G = m_PIDTau + 1; | ||
3669 | } | ||
3670 | |||
3671 | |||
3672 | // Where are we, and where are we headed? | ||
3673 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3674 | // d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3675 | |||
3676 | |||
3677 | // Non-Vehicles have a limited set of Hover options. | ||
3678 | // determine what our target height really is based on HoverType | ||
3679 | switch (m_PIDHoverType) | ||
3680 | { | ||
3681 | case PIDHoverType.Ground: | ||
3682 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
3683 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3684 | break; | ||
3685 | case PIDHoverType.GroundAndWater: | ||
3686 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
3687 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
3688 | if (m_groundHeight > m_waterHeight) | ||
3689 | { | ||
3690 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3691 | } | ||
3692 | else | ||
3693 | { | ||
3694 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
3695 | } | ||
3696 | break; | ||
3697 | |||
3698 | } // end switch (m_PIDHoverType) | ||
3699 | |||
3700 | |||
3701 | _target_velocity = | ||
3702 | new Vector3(0.0f, 0.0f, | ||
3703 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) | ||
3704 | ); | ||
3705 | |||
3706 | // if velocity is zero, use position control; otherwise, velocity control | ||
3707 | |||
3708 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | ||
3709 | { | ||
3710 | // keep track of where we stopped. No more slippin' & slidin' | ||
3711 | |||
3712 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
3713 | // react to the physics scene by moving it's position. | ||
3714 | // Avatar to Avatar collisions | ||
3715 | // Prim to avatar collisions | ||
3716 | d.Vector3 dlinvel = vel; | ||
3717 | |||
3718 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | ||
3719 | d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); | ||
3720 | d.BodyAddForce(Body, 0, 0, fz); | ||
3721 | //KF this prevents furthur motions return; | ||
3722 | } | ||
3723 | else | ||
3724 | { | ||
3725 | _zeroFlag = false; | ||
3726 | |||
3727 | // We're flying and colliding with something | ||
3728 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | ||
3729 | } | ||
3730 | } // end m_useHoverPID && !m_usePID | ||
3731 | |||
3732 | |||
3733 | /// Dynamics Apply Forces =================================================================================== | ||
3734 | fx *= m_mass; | ||
3735 | fy *= m_mass; | ||
3736 | //fz *= m_mass; | ||
3737 | |||
3738 | fx += m_force.X; | ||
3739 | fy += m_force.Y; | ||
3740 | fz += m_force.Z; | ||
3741 | |||
3742 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
3743 | if (fx != 0 || fy != 0 || fz != 0) | ||
3744 | { | ||
3745 | //m_taintdisable = true; | ||
3746 | //base.RaiseOutOfBounds(Position); | ||
3747 | //d.BodySetLinearVel(Body, fx, fy, 0f); | ||
3748 | if (!d.BodyIsEnabled(Body)) | ||
3749 | { | ||
3750 | // A physical body at rest on a surface will auto-disable after a while, | ||
3751 | // this appears to re-enable it incase the surface it is upon vanishes, | ||
3752 | // and the body should fall again. | ||
3753 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
3754 | d.BodySetForce(Body, 0, 0, 0); | ||
3755 | enableBodySoft(); | ||
3756 | } | ||
3757 | |||
3758 | // 35x10 = 350n times the mass per second applied maximum. | ||
3759 | float nmax = 35f * m_mass; | ||
3760 | float nmin = -35f * m_mass; | ||
3761 | |||
3762 | |||
3763 | if (fx > nmax) | ||
3764 | fx = nmax; | ||
3765 | if (fx < nmin) | ||
3766 | fx = nmin; | ||
3767 | if (fy > nmax) | ||
3768 | fy = nmax; | ||
3769 | if (fy < nmin) | ||
3770 | fy = nmin; | ||
3771 | d.BodyAddForce(Body, fx, fy, fz); | ||
3772 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | ||
3773 | } // end apply forces | ||
3774 | } // end Vehicle/Dynamics | ||
3775 | |||
3776 | /// RotLookAt ================================================================================= | ||
3777 | if (m_useAPID) | ||
3778 | { | ||
3779 | // RotLookAt, apparently overrides all other rotation sources. Inputs: | ||
3780 | // Quaternion m_APIDTarget | ||
3781 | // float m_APIDStrength // From SL experiments, this is the time to get there | ||
3782 | // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly | ||
3783 | // Also in SL the mass of the object has no effect on time to get there. | ||
3784 | // Factors: | ||
3785 | // get present body rotation | ||
3786 | float limit = 1.0f; | ||
3787 | float scaler = 50f; // adjusts damping time | ||
3788 | float RLAservo = 0f; | ||
3789 | |||
3790 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
3791 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); | ||
3792 | Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; | ||
3793 | float diff_angle; | ||
3794 | Vector3 diff_axis; | ||
3795 | rot_diff.GetAxisAngle(out diff_axis, out diff_angle); | ||
3796 | diff_axis.Normalize(); | ||
3797 | if(diff_angle > 0.01f) // diff_angle is always +ve | ||
3798 | { | ||
3799 | // PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); | ||
3800 | Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); | ||
3801 | rotforce = rotforce * rotq; | ||
3802 | if(diff_angle > limit) diff_angle = limit; // cap the rotate rate | ||
3803 | // RLAservo = timestep / m_APIDStrength * m_mass * scaler; | ||
3804 | // rotforce = rotforce * RLAservo * diff_angle ; | ||
3805 | // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); | ||
3806 | RLAservo = timestep / m_APIDStrength * scaler; | ||
3807 | rotforce = rotforce * RLAservo * diff_angle ; | ||
3808 | /* | ||
3809 | if (m_angularEnable.X == 0) | ||
3810 | rotforce.X = 0; | ||
3811 | if (m_angularEnable.Y == 0) | ||
3812 | rotforce.Y = 0; | ||
3813 | if (m_angularEnable.Z == 0) | ||
3814 | rotforce.Z = 0; | ||
3815 | */ | ||
3816 | d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); | ||
3817 | //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); | ||
3818 | } | ||
3819 | //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); | ||
3820 | } // end m_useAPID | ||
3821 | |||
3822 | } // end root prims | ||
3823 | } // end Move() | ||
3824 | } // end class | ||
3825 | } | ||