aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2368
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs28
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs167
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsActor.cs6
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsScene.cs6
5 files changed, 1540 insertions, 1035 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index a68dcb7..6e28bfa 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -27,6 +27,7 @@
27 */ 27 */
28 28
29using System; 29using System;
30using System.IO;
30using System.Collections.Generic; 31using System.Collections.Generic;
31using System.Reflection; 32using System.Reflection;
32using System.Runtime.InteropServices; 33using System.Runtime.InteropServices;
@@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin
48 { 49 {
49 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 50 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
50 51
52 public class SerialControl
53 {
54 public object alock = new object();
55 public byte[] data = new byte[0];
56 }
51 private Vector3 _position; 57 private Vector3 _position;
52 private Vector3 _velocity; 58 private Vector3 _velocity;
53 private Vector3 _torque; 59 private Vector3 _torque;
@@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin
80 private float PID_D = 35f; 86 private float PID_D = 35f;
81 private float PID_G = 25f; 87 private float PID_G = 25f;
82 private bool m_usePID = false; 88 private bool m_usePID = false;
83 89
84 private Quaternion m_APIDTarget = new Quaternion(); 90 private Quaternion m_APIDTarget = new Quaternion();
85 private float m_APIDStrength = 0.5f; 91 private float m_APIDStrength = 0.5f;
86 private float m_APIDDamping = 0.5f; 92 private float m_APIDDamping = 0.5f;
87 private bool m_useAPID = false; 93 private bool m_useAPID = false;
88 private float m_APIDdamper = 1.0f; 94 private float m_APIDdamper = 1.0f;
89 95
90 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 96 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
91 // do not confuse with VEHICLE HOVER 97 // do not confuse with VEHICLE HOVER
@@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin
112 private bool m_taintPhysics; 118 private bool m_taintPhysics;
113 private bool m_collidesLand = true; 119 private bool m_collidesLand = true;
114 private bool m_collidesWater; 120 private bool m_collidesWater;
115 public bool m_returnCollisions; 121 // public bool m_returnCollisions;
116 122
117 // Default we're a Geometry 123 // Default we're a Geometry
118 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 124 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
@@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin
144 private OdeScene _parent_scene; 150 private OdeScene _parent_scene;
145 public IntPtr m_targetSpace = IntPtr.Zero; 151 public IntPtr m_targetSpace = IntPtr.Zero;
146 public IntPtr prim_geom; 152 public IntPtr prim_geom;
147// public IntPtr prev_geom; 153 // public IntPtr prev_geom;
148 public IntPtr _triMeshData; 154 public IntPtr _triMeshData;
149 155
150 private IntPtr _linkJointGroup = IntPtr.Zero; 156 private IntPtr _linkJointGroup = IntPtr.Zero;
@@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin
163 private int throttleCounter; 169 private int throttleCounter;
164 public int m_interpenetrationcount; 170 public int m_interpenetrationcount;
165 public float m_collisionscore; 171 public float m_collisionscore;
166 public int m_roundsUnderMotionThreshold; 172 // public int m_roundsUnderMotionThreshold;
167 private int m_crossingfailures; 173 // private int m_crossingfailures;
168 174
169 public bool m_outofBounds; 175 public bool m_outofBounds;
170 private float m_density = 10.000006836f; // Aluminum g/cm3; 176 private float m_density = 10.000006836f; // Aluminum g/cm3;
@@ -185,75 +191,470 @@ namespace OpenSim.Region.Physics.OdePlugin
185 public volatile bool childPrim; 191 public volatile bool childPrim;
186 192
187 internal int m_material = (int)Material.Wood; 193 internal int m_material = (int)Material.Wood;
188
189 private int frcount = 0; // Used to limit dynamics debug output to
190 private int revcount = 0; // Reverse motion while > 0
191 194
192 private IntPtr m_body = IntPtr.Zero; 195 private IntPtr m_body = IntPtr.Zero;
193 196
194 // Vehicle properties ============================================================================================ 197 // Vehicle properties ============================================================================================
195 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind 198 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
196 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier 199 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
197 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: 200 private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings:
198 // HOVER_TERRAIN_ONLY 201 // HOVER_TERRAIN_ONLY
199 // HOVER_GLOBAL_HEIGHT 202 // HOVER_GLOBAL_HEIGHT
200 // NO_DEFLECTION_UP 203 // NO_DEFLECTION_UP
201 // HOVER_WATER_ONLY 204 // HOVER_WATER_ONLY
202 // HOVER_UP_ONLY 205 // HOVER_UP_ONLY
203 // LIMIT_MOTOR_UP 206 // LIMIT_MOTOR_UP
204 // LIMIT_ROLL_ONLY 207 // LIMIT_ROLL_ONLY
205 208
206 // Linear properties 209 // Linear properties
207 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity 210 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
208 //requested by LSL 211 //requested by LSL
209 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL 212 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
210 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL 213 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
211 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL 214 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
212 215
213 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor 216 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
214 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity 217 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
215 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity 218 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
216 219
217 //Angular properties 220 //Angular properties
218 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 221 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
219 222
220 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL 223 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
221 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL 224 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
222 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL 225 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
223 226
224 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor 227 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
225// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity 228 // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
226 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 229 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
227 230
228 //Deflection properties 231 //Deflection properties
229 // private float m_angularDeflectionEfficiency = 0; 232 // private float m_angularDeflectionEfficiency = 0;
230 // private float m_angularDeflectionTimescale = 0; 233 // private float m_angularDeflectionTimescale = 0;
231 // private float m_linearDeflectionEfficiency = 0; 234 // private float m_linearDeflectionEfficiency = 0;
232 // private float m_linearDeflectionTimescale = 0; 235 // private float m_linearDeflectionTimescale = 0;
233 236
234 //Banking properties 237 //Banking properties
235 // private float m_bankingEfficiency = 0; 238 // private float m_bankingEfficiency = 0;
236 // private float m_bankingMix = 0; 239 // private float m_bankingMix = 0;
237 // private float m_bankingTimescale = 0; 240 // private float m_bankingTimescale = 0;
238 241
239 //Hover and Buoyancy properties 242 //Hover and Buoyancy properties
240 private float m_VhoverHeight = 0f; 243 private float m_VhoverHeight = 0f;
241// private float m_VhoverEfficiency = 0f; 244 // private float m_VhoverEfficiency = 0f;
242 private float m_VhoverTimescale = 0f; 245 private float m_VhoverTimescale = 0f;
243 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height 246 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
244 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. 247 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
245 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) 248 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
246 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. 249 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
247 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. 250 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
248 251
249 //Attractor properties 252 //Attractor properties
250 private float m_verticalAttractionEfficiency = 1.0f; // damped 253 private float m_verticalAttractionEfficiency = 1.0f; // damped
251 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. 254 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
252
253
254 255
256 SerialControl m_taintserial = null;
257
258 public override byte[] Serialize(bool PhysIsRunning)
259 {
260 SerialControl sc = new SerialControl();
261
262 lock (sc.alock)
263 {
264 if (PhysIsRunning)
265 {
266 m_taintserial = sc;
267
268 if (!Monitor.Wait(sc.alock, 1000))
269 {
270 m_log.Error("[chOde] prim data serialization timed out");
271 m_taintserial = null;
272 return new byte[0];
273 }
274 }
275 else
276 DoSerialize(sc);
277 }
278
279 return sc.data;
280 }
281
282 public void DoSerialize(SerialControl sc)
283 {
284 wstreamer st = new wstreamer();
285 Vector3 vtmp;
286
287 ushort version = 2;
288 if (!BitConverter.IsLittleEndian)
289 version |= 1;
290 st.Wushort(version); //version lower bit codes endian type for future use
291
292 // compact booleans in a ushort
293 ushort flags = 0;
294
295 if (m_isphysical) // this should be true for now
296 flags |= 1;
297 if (m_isSelected)
298 flags |= 2;
299 if (m_isVolumeDetect)
300 flags |= 4;
301 if (m_disabled)
302 flags |= 8;
303 if (m_collidesWater)
304 flags |= 16;
305 if (m_collidesLand)
306 flags |= 32;
307 if (m_usePID)
308 flags |= 64;
309 if (m_useAPID)
310 flags |= 128;
311 if (m_useHoverPID)
312 flags |= 256;
313 if (m_throttleUpdates)
314 flags |= 512;
315
316 st.Wushort(flags);
317
318 st.Wvector3(_size);
319 st.Wint(m_material);
320 st.Wfloat(m_density);
321 st.Wfloat(0); // future gravity mod V3
322 st.Wfloat(0); // future friction V3
323 st.Wfloat(0); // future bounce V3
324
325// st.Wuint((uint)m_collisionCategories);
326// st.Wuint((uint)m_collisionFlags);
327
328 if (_parent == null)
329 {
330 st.Wvector3(_position); // ??
331 st.Wquat(_orientation);
332 }
333 else // for childs save offsets
334 {
335 Quaternion to;
336 Quaternion ipo = Quaternion.Inverse(_parent.Orientation);
337
338 if (m_isphysical && prim_geom != IntPtr.Zero)
339 {
340 d.Vector3 dvt;
341 d.GeomCopyPosition(prim_geom, out dvt);
342
343 vtmp.X = dvt.X;
344 vtmp.Y = dvt.Y;
345 vtmp.Z = dvt.Z;
346
347 d.Quaternion dqt;
348 d.GeomCopyQuaternion(prim_geom, out dqt);
349
350 to.X = dqt.X;
351 to.Y = dqt.Y;
352 to.Z = dqt.Z;
353 to.W = dqt.W; // rotation in world
354 }
355 else
356 {
357 vtmp = _position;
358 to = _orientation;
359 }
360
361 vtmp -= _parent.Position; // offset in world
362 vtmp *= ipo; // offset in local
363 st.Wvector3(vtmp);
364
365 ipo *= to; // own rotation
366 st.Wquat(ipo);
367 }
368
369 st.Wvector3(_velocity);
370 st.Wvector3(m_rotationalVelocity);
371 st.Wvector3(_acceleration);
372 st.Wvector3(m_rotateEnable);
373
374 vtmp = Vector3.Zero;
375 for (int i = 0; i < m_forcelist.Count; i++)
376 {
377
378 vtmp += (m_forcelist[i] * 100);
379 }
380
381 st.Wvector3(vtmp); // force acc
382
383 vtmp = Vector3.Zero;
384 for (int i = 0; i < m_angularforcelist.Count; i++)
385 {
386 vtmp += (m_angularforcelist[i] * 100);
387 }
388
389 st.Wvector3(vtmp); // angular force acc
390
391 st.Wvector3(m_PIDTarget);
392 st.Wfloat(m_PIDTau);
393 st.Wfloat(PID_D);
394 st.Wfloat(PID_G);
395 st.Wquat(m_APIDTarget);
396 st.Wfloat(m_APIDStrength);
397 st.Wfloat(m_APIDDamping);
398 st.Wfloat(m_APIDdamper);
399
400 st.Wint((int)m_PIDHoverType);
401 st.Wfloat(m_PIDHoverHeight);
402 st.Wfloat(m_PIDHoverTau);
403 st.Wfloat(m_targetHoverHeight);
404
405 st.Wfloat(m_groundHeight);
406 st.Wfloat(m_waterHeight);
407
408 st.Wfloat(m_buoyancy);
409
410 // this must be last since type none ends stream
411 if (m_type == Vehicle.TYPE_NONE)
412 st.Wint((int)Vehicle.TYPE_NONE);
413 else
414 {
415 st.Wint((int)m_type);
416
417 st.Wquat(Quaternion.Identity); //m_referenceFrame
255 418
419 st.Wint((int)m_flags);
256 420
421 st.Wvector3(m_linearMotorDirection);
422 st.Wfloat(
423 (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared()));
424
425 st.Wvector3(m_linearFrictionTimescale);
426 st.Wfloat(m_linearMotorDecayTimescale);
427 st.Wfloat(m_linearMotorTimescale);
428 st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset);
429
430 st.Wvector3(m_angularMotorDirection);
431 st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared()));
432
433 st.Wvector3(m_angularFrictionTimescale);
434 st.Wfloat(m_angularMotorDecayTimescale);
435 st.Wfloat(m_angularMotorTimescale);
436
437 st.Wfloat(0); //m_linearDeflectionEfficiency);
438 st.Wfloat(1000); //m_linearDeflectionTimescale);
439
440 st.Wfloat(0); //m_angularDeflectionEfficiency);
441 st.Wfloat(120); //m_angularDeflectionTimescale);
442
443 st.Wfloat(0); // m_bankingEfficiency);
444 st.Wfloat(0); //m_bankingMix);
445 st.Wfloat(1000); //m_bankingTimescale);
446
447 st.Wfloat(m_VhoverHeight);
448 st.Wfloat(0.5f); //m_VhoverEfficiency);
449 st.Wfloat(m_VhoverTimescale);
450
451 st.Wfloat(m_VehicleBuoyancy);
452
453 st.Wfloat(m_verticalAttractionEfficiency);
454 st.Wfloat(m_verticalAttractionTimescale);
455 }
456 sc.data = st.close();
457 m_taintserial = null;
458 Monitor.PulseAll(sc.alock);
459 }
460
461 public bool DeSerialize(byte[] data)
462 {
463 rstreamer st = new rstreamer(data);
464
465 int version =st.Rushort(); //version
466
467 // merge booleans in a ushort
468 ushort flags = st.Rushort();
469 if ((flags & 1) != 0)
470 m_isphysical = true;
471 if ((flags & 2) != 0)
472 m_taintselected = true;
473 if ((flags & 4) != 0)
474 m_isVolumeDetect = true;
475 if ((flags & 8) != 0)
476 m_taintdisable = true;
477 if ((flags & 16) != 0)
478 m_taintCollidesWater = true;
479 if ((flags & 32) != 0)
480 m_collidesLand = true;
481 if ((flags & 64) != 0)
482 m_usePID = true;
483 if ((flags & 128) != 0)
484 m_useAPID = true;
485 if ((flags & 256) != 0)
486 m_useHoverPID = true;
487 if ((flags & 512) != 0)
488 m_throttleUpdates = true;
489
490 _size = st.Rvector3();
491 m_taintsize = _size;
492
493 m_material= st.Rint();
494 m_density = st.Rfloat();
495 st.Rfloat(); // future gravity mod V3
496 st.Rfloat(); // future friction V3
497 st.Rfloat(); // future bounce V3
498
499// m_collisionCategories = (CollisionCategories)st.Ruint();
500// m_collisionFlags = (CollisionCategories) st.Ruint();
501
502 if (m_taintparent == null)
503 {
504 st.Rvector3(); // ignore old position sop/sog as to tell the new one
505 m_taintrot = st.Rquat(); //
506 _orientation = m_taintrot;
507 }
508 else
509 {
510 m_taintrot = _parent.Orientation;
511 m_taintposition = st.Rvector3(); // ??
512 _position = m_taintposition;
513
514 m_taintposition *= m_taintrot;
515 m_taintposition += _parent.Position;
516
517 m_taintrot *= st.Rquat(); //
518 _orientation = m_taintrot;
519 }
520
521 m_taintVelocity = st.Rvector3();
522 m_rotationalVelocity = st.Rvector3();
523
524 _acceleration = st.Rvector3();
525 m_rotateEnableRequest = st.Rvector3();
526 m_rotateEnableUpdate = true;
527
528 Vector3 vtmp;
529
530 vtmp = st.Rvector3(); // forces acc
531 m_forcelist.Add(vtmp);
532 m_taintforce = true;
533
534 vtmp = st.Rvector3(); // angular forces acc
535 m_angularforcelist.Add(vtmp);
536 m_taintaddangularforce = true;
537
538 m_PIDTarget = st.Rvector3();
539 m_PIDTau = st.Rfloat();
540 PID_D = st.Rfloat();
541 PID_G = st.Rfloat();
542
543 m_APIDTarget = st.Rquat();
544 m_APIDStrength = st.Rfloat();
545 m_APIDDamping = st.Rfloat();
546 m_APIDdamper = st.Rfloat();
547
548 m_PIDHoverType = (PIDHoverType) st.Rint();
549 m_PIDHoverHeight = st.Rfloat();
550 m_PIDHoverTau = st.Rfloat();
551 m_targetHoverHeight = st.Rfloat();
552
553 m_groundHeight = st.Rfloat();
554 m_waterHeight = st.Rfloat();
555
556 m_buoyancy = st.Rfloat();
557
558
559 // this must be last since type none ends stream
560
561 m_type = (Vehicle) st.Rint();
562
563 if (m_type != Vehicle.TYPE_NONE)
564 {
565 float ftmp;
566
567 st.Rquat(); //m_referenceFrame
568
569 m_flags = (VehicleFlag) st.Rint();
570
571 m_linearMotorDirection = st.Rvector3();
572
573 ftmp = st.Rfloat();
574 m_lLinMotorDVel = m_linearMotorDirection * ftmp;
575
576 m_linearFrictionTimescale = st.Rvector3();
577 m_linearMotorDecayTimescale = st.Rfloat();
578 m_linearMotorTimescale = st.Rfloat();
579 st.Rvector3(); //m_linearMotorOffset);
580
581 m_angularMotorDirection = st.Rvector3();
582 ftmp = st.Rfloat();
583 m_angularMotorDVel = m_angularMotorDirection * ftmp;
584
585 m_angularFrictionTimescale = st.Rvector3();
586 m_angularMotorDecayTimescale = st.Rfloat();
587 m_angularMotorTimescale = st.Rfloat();
588
589 st.Rfloat(); //m_linearDeflectionEfficiency);
590 st.Rfloat(); //m_linearDeflectionTimescale);
591
592 st.Rfloat(); //m_angularDeflectionEfficiency);
593 st.Rfloat(); //m_angularDeflectionTimescale);
594
595 st.Rfloat(); // m_bankingEfficiency);
596 st.Rfloat(); //m_bankingMix);
597 st.Rfloat(); //m_bankingTimescale);
598
599 m_VhoverHeight = st.Rfloat();
600 st.Rfloat(); //m_VhoverEfficiency);
601 m_VhoverTimescale = st.Rfloat();
602
603 m_VehicleBuoyancy = st.Rfloat();
604
605 m_verticalAttractionEfficiency = st.Rfloat();
606 m_verticalAttractionTimescale = st.Rfloat();
607 }
608 st.close();
609 return true;
610 }
611
612 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent,
613 PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata)
614 {
615 m_localID = localid;
616 ode = dode;
617
618 if (parent == null)
619 {
620 m_taintparent = null;
621
622 if (!pos.IsFinite())
623 {
624 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
625 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
626 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
627 }
628
629 _position = pos;
630 m_taintposition = pos;
631 }
632 else
633 m_taintparent = parent;
634
635 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
636
637 prim_geom = IntPtr.Zero;
638
639 _mesh = null;
640 m_meshfailed = false;
641 _pbs = pbs;
642
643 _parent_scene = parent_scene;
644 m_targetSpace = (IntPtr)0;
645
646 if(sdata != null && sdata.Length > 1)
647 DeSerialize(sdata);
648
649 if (m_isphysical)
650 m_targetSpace = _parent_scene.space;
651
652 m_primName = primName;
653 m_taintserial = null;
654 m_taintadd = true;
655 _parent_scene.AddPhysicsActorTaint(this);
656 // don't do .add() here; old geoms get recycled with the same hash
657 }
257 658
258 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 659 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
259 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) 660 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid)
@@ -266,7 +667,7 @@ namespace OpenSim.Region.Physics.OdePlugin
266 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); 667 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
267 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); 668 m_log.Warn("[PHYSICS]: Got nonFinite Object create Position");
268 } 669 }
269 670
270 _position = pos; 671 _position = pos;
271 m_taintposition = pos; 672 m_taintposition = pos;
272 PID_D = parent_scene.bodyPIDD; 673 PID_D = parent_scene.bodyPIDD;
@@ -275,9 +676,8 @@ namespace OpenSim.Region.Physics.OdePlugin
275 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 676 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
276 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 677 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
277 678
278
279 prim_geom = IntPtr.Zero; 679 prim_geom = IntPtr.Zero;
280// prev_geom = IntPtr.Zero; 680 // prev_geom = IntPtr.Zero;
281 681
282 if (!pos.IsFinite()) 682 if (!pos.IsFinite())
283 { 683 {
@@ -306,7 +706,7 @@ namespace OpenSim.Region.Physics.OdePlugin
306 _parent_scene = parent_scene; 706 _parent_scene = parent_scene;
307 m_targetSpace = (IntPtr)0; 707 m_targetSpace = (IntPtr)0;
308 708
309// if (pos.Z < 0) 709 // if (pos.Z < 0)
310 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) 710 if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y))
311 m_isphysical = false; 711 m_isphysical = false;
312 else 712 else
@@ -317,6 +717,8 @@ namespace OpenSim.Region.Physics.OdePlugin
317 if (m_isphysical) 717 if (m_isphysical)
318 m_targetSpace = _parent_scene.space; 718 m_targetSpace = _parent_scene.space;
319 } 719 }
720
721 m_taintserial = null;
320 m_primName = primName; 722 m_primName = primName;
321 m_taintadd = true; 723 m_taintadd = true;
322 _parent_scene.AddPhysicsActorTaint(this); 724 _parent_scene.AddPhysicsActorTaint(this);
@@ -325,7 +727,7 @@ namespace OpenSim.Region.Physics.OdePlugin
325 727
326 public override int PhysicsActorType 728 public override int PhysicsActorType
327 { 729 {
328 get { return (int) ActorTypes.Prim; } 730 get { return (int)ActorTypes.Prim; }
329 set { return; } 731 set { return; }
330 } 732 }
331 733
@@ -337,9 +739,11 @@ namespace OpenSim.Region.Physics.OdePlugin
337 739
338 public override uint LocalID 740 public override uint LocalID
339 { 741 {
340 set { 742 set
743 {
341 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); 744 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
342 m_localID = value; } 745 m_localID = value;
746 }
343 } 747 }
344 748
345 public override bool Grabbed 749 public override bool Grabbed
@@ -349,9 +753,10 @@ namespace OpenSim.Region.Physics.OdePlugin
349 753
350 public override bool Selected 754 public override bool Selected
351 { 755 {
352 set { 756 set
353 757 {
354//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); 758
759 //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
355 // This only makes the object not collidable if the object 760 // This only makes the object not collidable if the object
356 // is physical or the object is modified somehow *IN THE FUTURE* 761 // is physical or the object is modified somehow *IN THE FUTURE*
357 // without this, if an avatar selects prim, they can walk right 762 // without this, if an avatar selects prim, they can walk right
@@ -367,21 +772,21 @@ namespace OpenSim.Region.Physics.OdePlugin
367 m_taintselected = value; 772 m_taintselected = value;
368 m_isSelected = value; 773 m_isSelected = value;
369 } 774 }
370 if(m_isSelected) disableBodySoft(); 775 if (m_isSelected) disableBodySoft();
371 } 776 }
372 } 777 }
373 778
374 public override bool IsPhysical 779 public override bool IsPhysical
375 { 780 {
376 get { return m_isphysical; } 781 get { return m_isphysical; }
377 set 782 set
378 { 783 {
379 m_isphysical = value; 784 m_isphysical = value;
380 if (!m_isphysical) 785 if (!m_isphysical)
381 { // Zero the remembered last velocity 786 { // Zero the remembered last velocity
382 m_lastVelocity = Vector3.Zero; 787 m_lastVelocity = Vector3.Zero;
383 if (m_type != Vehicle.TYPE_NONE) Halt(); 788 if (m_type != Vehicle.TYPE_NONE) Halt();
384 } 789 }
385 } 790 }
386 } 791 }
387 792
@@ -430,7 +835,9 @@ namespace OpenSim.Region.Physics.OdePlugin
430 { 835 {
431 get { return _position; } 836 get { return _position; }
432 837
433 set { _position = value; 838 set
839 {
840 _position = value;
434 //m_log.Info("[PHYSICS]: " + _position.ToString()); 841 //m_log.Info("[PHYSICS]: " + _position.ToString());
435 } 842 }
436 } 843 }
@@ -481,29 +888,29 @@ namespace OpenSim.Region.Physics.OdePlugin
481 888
482 public override void VehicleFloatParam(int param, float value) 889 public override void VehicleFloatParam(int param, float value)
483 { 890 {
484 ProcessFloatVehicleParam((Vehicle) param, value); 891 ProcessFloatVehicleParam((Vehicle)param, value);
485 } 892 }
486 893
487 public override void VehicleVectorParam(int param, Vector3 value) 894 public override void VehicleVectorParam(int param, Vector3 value)
488 { 895 {
489 ProcessVectorVehicleParam((Vehicle) param, value); 896 ProcessVectorVehicleParam((Vehicle)param, value);
490 } 897 }
491 898
492 public override void VehicleRotationParam(int param, Quaternion rotation) 899 public override void VehicleRotationParam(int param, Quaternion rotation)
493 { 900 {
494 ProcessRotationVehicleParam((Vehicle) param, rotation); 901 ProcessRotationVehicleParam((Vehicle)param, rotation);
495 } 902 }
496 903
497 public override void VehicleFlags(int param, bool remove) 904 public override void VehicleFlags(int param, bool remove)
498 { 905 {
499 ProcessVehicleFlags(param, remove); 906 ProcessVehicleFlags(param, remove);
500 } 907 }
501 908
502 public override void SetVolumeDetect(int param) 909 public override void SetVolumeDetect(int param)
503 { 910 {
504 lock (_parent_scene.OdeLock) 911 lock (_parent_scene.OdeLock)
505 { 912 {
506 m_isVolumeDetect = (param!=0); 913 m_isVolumeDetect = (param != 0);
507 } 914 }
508 } 915 }
509 916
@@ -536,9 +943,9 @@ namespace OpenSim.Region.Physics.OdePlugin
536 return Vector3.Zero; 943 return Vector3.Zero;
537 944
538 Vector3 returnVelocity = Vector3.Zero; 945 Vector3 returnVelocity = Vector3.Zero;
539 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; 946 returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
540 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; 947 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
541 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; 948 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
542 return returnVelocity; 949 return returnVelocity;
543 } 950 }
544 set 951 set
@@ -546,8 +953,8 @@ namespace OpenSim.Region.Physics.OdePlugin
546 if (value.IsFinite()) 953 if (value.IsFinite())
547 { 954 {
548 _velocity = value; 955 _velocity = value;
549 if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) 956 if (_velocity.ApproxEquals(Vector3.Zero, 0.001f))
550 _acceleration = Vector3.Zero; 957 _acceleration = Vector3.Zero;
551 958
552 m_taintVelocity = value; 959 m_taintVelocity = value;
553 _parent_scene.AddPhysicsActorTaint(this); 960 _parent_scene.AddPhysicsActorTaint(this);
@@ -604,17 +1011,18 @@ namespace OpenSim.Region.Physics.OdePlugin
604 if (QuaternionIsFinite(value)) 1011 if (QuaternionIsFinite(value))
605 { 1012 {
606 _orientation = value; 1013 _orientation = value;
607 } 1014 }
608 else 1015 else
609 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 1016 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
610 1017
611 } 1018 }
612 } 1019 }
613 1020
614 1021
615 public override bool FloatOnWater 1022 public override bool FloatOnWater
616 { 1023 {
617 set { 1024 set
1025 {
618 m_taintCollidesWater = value; 1026 m_taintCollidesWater = value;
619 _parent_scene.AddPhysicsActorTaint(this); 1027 _parent_scene.AddPhysicsActorTaint(this);
620 } 1028 }
@@ -624,8 +1032,8 @@ namespace OpenSim.Region.Physics.OdePlugin
624 { 1032 {
625 } 1033 }
626 1034
627 public override Vector3 PIDTarget 1035 public override Vector3 PIDTarget
628 { 1036 {
629 set 1037 set
630 { 1038 {
631 if (value.IsFinite()) 1039 if (value.IsFinite())
@@ -634,16 +1042,16 @@ namespace OpenSim.Region.Physics.OdePlugin
634 } 1042 }
635 else 1043 else
636 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); 1044 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
637 } 1045 }
638 } 1046 }
639 public override bool PIDActive { set { m_usePID = value; } } 1047 public override bool PIDActive { set { m_usePID = value; } }
640 public override float PIDTau { set { m_PIDTau = value; } } 1048 public override float PIDTau { set { m_PIDTau = value; } }
641 1049
642 // For RotLookAt 1050 // For RotLookAt
643 public override Quaternion APIDTarget { set { m_APIDTarget = value; } } 1051 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
644 public override bool APIDActive { set { m_useAPID = value; } } 1052 public override bool APIDActive { set { m_useAPID = value; } }
645 public override float APIDStrength { set { m_APIDStrength = value; } } 1053 public override float APIDStrength { set { m_APIDStrength = value; } }
646 public override float APIDDamping { set { m_APIDDamping = value; } } 1054 public override float APIDDamping { set { m_APIDDamping = value; } }
647 1055
648 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 1056 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
649 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 1057 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
@@ -665,13 +1073,13 @@ namespace OpenSim.Region.Physics.OdePlugin
665 1073
666 public override Vector3 Acceleration // client updates read data via here 1074 public override Vector3 Acceleration // client updates read data via here
667 { 1075 {
668 get 1076 get
669 { 1077 {
670 if (_zeroFlag) 1078 if (_zeroFlag)
671 { 1079 {
672 return Vector3.Zero; 1080 return Vector3.Zero;
673 } 1081 }
674 return _acceleration; 1082 return _acceleration;
675 } 1083 }
676 set { _acceleration = value; } 1084 set { _acceleration = value; }
677 } 1085 }
@@ -752,18 +1160,18 @@ namespace OpenSim.Region.Physics.OdePlugin
752 base.RequestPhysicsterseUpdate(); 1160 base.RequestPhysicsterseUpdate();
753 m_outofBounds = false; 1161 m_outofBounds = false;
754 } 1162 }
755/* 1163 /*
756 int tmp = Interlocked.Increment(ref m_crossingfailures); 1164 int tmp = Interlocked.Increment(ref m_crossingfailures);
757 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1165 if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
758 { 1166 {
759 base.RaiseOutOfBounds(_position); 1167 base.RaiseOutOfBounds(_position);
760 return; 1168 return;
761 } 1169 }
762 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) 1170 else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
763 { 1171 {
764 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); 1172 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
765 } 1173 }
766 */ 1174 */
767 } 1175 }
768 1176
769 public override float Buoyancy 1177 public override float Buoyancy
@@ -806,26 +1214,26 @@ namespace OpenSim.Region.Physics.OdePlugin
806 1214
807 public void SetGeom(IntPtr geom) 1215 public void SetGeom(IntPtr geom)
808 { 1216 {
809 if(prim_geom != IntPtr.Zero) 1217 if (prim_geom != IntPtr.Zero)
810 { 1218 {
811 // Remove any old entries 1219 // Remove any old entries
812//string tPA; 1220 //string tPA;
813//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); 1221 //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA);
814//Console.WriteLine("**** Remove {0}", tPA); 1222 //Console.WriteLine("**** Remove {0}", tPA);
815 if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); 1223 if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom);
816 if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); 1224 if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom);
817 d.GeomDestroy(prim_geom); 1225 d.GeomDestroy(prim_geom);
818 } 1226 }
819 1227
820 prim_geom = geom; 1228 prim_geom = geom;
821//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 1229 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
822 if (prim_geom != IntPtr.Zero) 1230 if (prim_geom != IntPtr.Zero)
823 { 1231 {
824 _parent_scene.geom_name_map[prim_geom] = this.m_primName; 1232 _parent_scene.geom_name_map[prim_geom] = this.m_primName;
825 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; 1233 _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
826 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1234 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
827 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1235 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
828//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); 1236 //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName);
829 } 1237 }
830 1238
831 if (childPrim) 1239 if (childPrim)
@@ -833,7 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin
833 if (_parent != null && _parent is OdePrim) 1241 if (_parent != null && _parent is OdePrim)
834 { 1242 {
835 OdePrim parent = (OdePrim)_parent; 1243 OdePrim parent = (OdePrim)_parent;
836//Console.WriteLine("SetGeom calls ChildSetGeom"); 1244 //Console.WriteLine("SetGeom calls ChildSetGeom");
837 parent.ChildSetGeom(this); 1245 parent.ChildSetGeom(this);
838 } 1246 }
839 } 1247 }
@@ -848,7 +1256,7 @@ namespace OpenSim.Region.Physics.OdePlugin
848 { 1256 {
849 d.BodyEnable(Body); 1257 d.BodyEnable(Body);
850 if (m_type != Vehicle.TYPE_NONE) 1258 if (m_type != Vehicle.TYPE_NONE)
851 Enable(Body, _parent_scene); 1259 Enable(Body, _parent_scene);
852 } 1260 }
853 1261
854 m_disabled = false; 1262 m_disabled = false;
@@ -892,9 +1300,9 @@ namespace OpenSim.Region.Physics.OdePlugin
892 1300
893 d.BodySetAutoDisableFlag(Body, true); 1301 d.BodySetAutoDisableFlag(Body, true);
894 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 1302 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
895 1303
896 // disconnect from world gravity so we can apply buoyancy 1304 // disconnect from world gravity so we can apply buoyancy
897 d.BodySetGravityMode (Body, false); 1305 d.BodySetGravityMode(Body, false);
898 1306
899 m_interpenetrationcount = 0; 1307 m_interpenetrationcount = 0;
900 m_collisionscore = 0; 1308 m_collisionscore = 0;
@@ -918,19 +1326,19 @@ namespace OpenSim.Region.Physics.OdePlugin
918 1326
919 float returnMass = 0; 1327 float returnMass = 0;
920 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; 1328 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
921 float hollowVolume = hollowAmount * hollowAmount; 1329 float hollowVolume = hollowAmount * hollowAmount;
922 1330
923 switch (_pbs.ProfileShape) 1331 switch (_pbs.ProfileShape)
924 { 1332 {
925 case ProfileShape.Square: 1333 case ProfileShape.Square:
926 // default box 1334 // default box
927 1335
928 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1336 if (_pbs.PathCurve == (byte)Extrusion.Straight)
929 { 1337 {
930 if (hollowAmount > 0.0) 1338 if (hollowAmount > 0.0)
931 { 1339 {
932 switch (_pbs.HollowShape) 1340 switch (_pbs.HollowShape)
933 { 1341 {
934 case HollowShape.Square: 1342 case HollowShape.Square:
935 case HollowShape.Same: 1343 case HollowShape.Same:
936 break; 1344 break;
@@ -948,31 +1356,31 @@ namespace OpenSim.Region.Physics.OdePlugin
948 default: 1356 default:
949 hollowVolume = 0; 1357 hollowVolume = 0;
950 break; 1358 break;
951 }
952 volume *= (1.0f - hollowVolume);
953 } 1359 }
1360 volume *= (1.0f - hollowVolume);
954 } 1361 }
1362 }
955 1363
956 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1364 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
957 { 1365 {
958 //a tube 1366 //a tube
959 1367
960 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); 1368 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
961 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); 1369 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
962 volume -= volume*tmp*tmp; 1370 volume -= volume * tmp * tmp;
963 1371
964 if (hollowAmount > 0.0) 1372 if (hollowAmount > 0.0)
965 { 1373 {
966 hollowVolume *= hollowAmount; 1374 hollowVolume *= hollowAmount;
967 1375
968 switch (_pbs.HollowShape) 1376 switch (_pbs.HollowShape)
969 { 1377 {
970 case HollowShape.Square: 1378 case HollowShape.Square:
971 case HollowShape.Same: 1379 case HollowShape.Same:
972 break; 1380 break;
973 1381
974 case HollowShape.Circle: 1382 case HollowShape.Circle:
975 hollowVolume *= 0.78539816339f;; 1383 hollowVolume *= 0.78539816339f; ;
976 break; 1384 break;
977 1385
978 case HollowShape.Triangle: 1386 case HollowShape.Triangle:
@@ -981,23 +1389,23 @@ namespace OpenSim.Region.Physics.OdePlugin
981 default: 1389 default:
982 hollowVolume = 0; 1390 hollowVolume = 0;
983 break; 1391 break;
984 }
985 volume *= (1.0f - hollowVolume);
986 } 1392 }
1393 volume *= (1.0f - hollowVolume);
987 } 1394 }
1395 }
988 1396
989 break; 1397 break;
990 1398
991 case ProfileShape.Circle: 1399 case ProfileShape.Circle:
992 1400
993 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1401 if (_pbs.PathCurve == (byte)Extrusion.Straight)
994 { 1402 {
995 volume *= 0.78539816339f; // elipse base 1403 volume *= 0.78539816339f; // elipse base
996 1404
997 if (hollowAmount > 0.0) 1405 if (hollowAmount > 0.0)
998 { 1406 {
999 switch (_pbs.HollowShape) 1407 switch (_pbs.HollowShape)
1000 { 1408 {
1001 case HollowShape.Same: 1409 case HollowShape.Same:
1002 case HollowShape.Circle: 1410 case HollowShape.Circle:
1003 break; 1411 break;
@@ -1013,25 +1421,25 @@ namespace OpenSim.Region.Physics.OdePlugin
1013 default: 1421 default:
1014 hollowVolume = 0; 1422 hollowVolume = 0;
1015 break; 1423 break;
1016 }
1017 volume *= (1.0f - hollowVolume);
1018 } 1424 }
1425 volume *= (1.0f - hollowVolume);
1019 } 1426 }
1427 }
1020 1428
1021 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1429 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1022 { 1430 {
1023 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); 1431 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
1024 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1432 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1025 volume *= (1.0f - tmp * tmp); 1433 volume *= (1.0f - tmp * tmp);
1026 1434
1027 if (hollowAmount > 0.0) 1435 if (hollowAmount > 0.0)
1028 { 1436 {
1029 1437
1030 // calculate the hollow volume by it's shape compared to the prim shape 1438 // calculate the hollow volume by it's shape compared to the prim shape
1031 hollowVolume *= hollowAmount; 1439 hollowVolume *= hollowAmount;
1032 1440
1033 switch (_pbs.HollowShape) 1441 switch (_pbs.HollowShape)
1034 { 1442 {
1035 case HollowShape.Same: 1443 case HollowShape.Same:
1036 case HollowShape.Circle: 1444 case HollowShape.Circle:
1037 break; 1445 break;
@@ -1047,31 +1455,31 @@ namespace OpenSim.Region.Physics.OdePlugin
1047 default: 1455 default:
1048 hollowVolume = 0; 1456 hollowVolume = 0;
1049 break; 1457 break;
1050 }
1051 volume *= (1.0f - hollowVolume);
1052 } 1458 }
1459 volume *= (1.0f - hollowVolume);
1053 } 1460 }
1461 }
1054 break; 1462 break;
1055 1463
1056 case ProfileShape.HalfCircle: 1464 case ProfileShape.HalfCircle:
1057 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1465 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1058 { 1466 {
1059 volume *= 0.52359877559829887307710723054658f; 1467 volume *= 0.52359877559829887307710723054658f;
1060 } 1468 }
1061 break; 1469 break;
1062 1470
1063 case ProfileShape.EquilateralTriangle: 1471 case ProfileShape.EquilateralTriangle:
1064 1472
1065 if (_pbs.PathCurve == (byte)Extrusion.Straight) 1473 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1066 { 1474 {
1067 volume *= 0.32475953f; 1475 volume *= 0.32475953f;
1068 1476
1069 if (hollowAmount > 0.0) 1477 if (hollowAmount > 0.0)
1070 { 1478 {
1071 1479
1072 // calculate the hollow volume by it's shape compared to the prim shape 1480 // calculate the hollow volume by it's shape compared to the prim shape
1073 switch (_pbs.HollowShape) 1481 switch (_pbs.HollowShape)
1074 { 1482 {
1075 case HollowShape.Same: 1483 case HollowShape.Same:
1076 case HollowShape.Triangle: 1484 case HollowShape.Triangle:
1077 hollowVolume *= .25f; 1485 hollowVolume *= .25f;
@@ -1091,24 +1499,24 @@ namespace OpenSim.Region.Physics.OdePlugin
1091 default: 1499 default:
1092 hollowVolume = 0; 1500 hollowVolume = 0;
1093 break; 1501 break;
1094 }
1095 volume *= (1.0f - hollowVolume);
1096 } 1502 }
1503 volume *= (1.0f - hollowVolume);
1097 } 1504 }
1505 }
1098 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 1506 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1099 { 1507 {
1100 volume *= 0.32475953f; 1508 volume *= 0.32475953f;
1101 volume *= 0.01f * (float)(200 - _pbs.PathScaleX); 1509 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
1102 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 1510 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
1103 volume *= (1.0f - tmp * tmp); 1511 volume *= (1.0f - tmp * tmp);
1104 1512
1105 if (hollowAmount > 0.0) 1513 if (hollowAmount > 0.0)
1106 { 1514 {
1107 1515
1108 hollowVolume *= hollowAmount; 1516 hollowVolume *= hollowAmount;
1109 1517
1110 switch (_pbs.HollowShape) 1518 switch (_pbs.HollowShape)
1111 { 1519 {
1112 case HollowShape.Same: 1520 case HollowShape.Same:
1113 case HollowShape.Triangle: 1521 case HollowShape.Triangle:
1114 hollowVolume *= .25f; 1522 hollowVolume *= .25f;
@@ -1126,15 +1534,15 @@ namespace OpenSim.Region.Physics.OdePlugin
1126 default: 1534 default:
1127 hollowVolume = 0; 1535 hollowVolume = 0;
1128 break; 1536 break;
1129 }
1130 volume *= (1.0f - hollowVolume);
1131 } 1537 }
1538 volume *= (1.0f - hollowVolume);
1132 } 1539 }
1133 break; 1540 }
1541 break;
1134 1542
1135 default: 1543 default:
1136 break; 1544 break;
1137 } 1545 }
1138 1546
1139 1547
1140 1548
@@ -1148,7 +1556,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1148 float profileEnd; 1556 float profileEnd;
1149 1557
1150 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) 1558 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
1151 { 1559 {
1152 taperX1 = _pbs.PathScaleX * 0.01f; 1560 taperX1 = _pbs.PathScaleX * 0.01f;
1153 if (taperX1 > 1.0f) 1561 if (taperX1 > 1.0f)
1154 taperX1 = 2.0f - taperX1; 1562 taperX1 = 2.0f - taperX1;
@@ -1158,9 +1566,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1158 if (taperY1 > 1.0f) 1566 if (taperY1 > 1.0f)
1159 taperY1 = 2.0f - taperY1; 1567 taperY1 = 2.0f - taperY1;
1160 taperY = 1.0f - taperY1; 1568 taperY = 1.0f - taperY1;
1161 } 1569 }
1162 else 1570 else
1163 { 1571 {
1164 taperX = _pbs.PathTaperX * 0.01f; 1572 taperX = _pbs.PathTaperX * 0.01f;
1165 if (taperX < 0.0f) 1573 if (taperX < 0.0f)
1166 taperX = -taperX; 1574 taperX = -taperX;
@@ -1168,10 +1576,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1168 1576
1169 taperY = _pbs.PathTaperY * 0.01f; 1577 taperY = _pbs.PathTaperY * 0.01f;
1170 if (taperY < 0.0f) 1578 if (taperY < 0.0f)
1171 taperY = -taperY; 1579 taperY = -taperY;
1172 taperY1 = 1.0f - taperY; 1580 taperY1 = 1.0f - taperY;
1173 1581
1174 } 1582 }
1175 1583
1176 1584
1177 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); 1585 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
@@ -1180,7 +1588,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1180 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; 1588 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
1181 volume *= (pathEnd - pathBegin); 1589 volume *= (pathEnd - pathBegin);
1182 1590
1183// this is crude aproximation 1591 // this is crude aproximation
1184 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; 1592 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
1185 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; 1593 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
1186 volume *= (profileEnd - profileBegin); 1594 volume *= (profileEnd - profileBegin);
@@ -1189,8 +1597,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1189 1597
1190 if (returnMass <= 0) 1598 if (returnMass <= 0)
1191 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. 1599 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
1192// else if (returnMass > _parent_scene.maximumMassObject) 1600 // else if (returnMass > _parent_scene.maximumMassObject)
1193// returnMass = _parent_scene.maximumMassObject; 1601 // returnMass = _parent_scene.maximumMassObject;
1194 1602
1195 1603
1196 1604
@@ -1230,7 +1638,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1230 1638
1231 public void setMass() 1639 public void setMass()
1232 { 1640 {
1233 if (Body != (IntPtr) 0) 1641 if (Body != (IntPtr)0)
1234 { 1642 {
1235 float newmass = CalculateMass(); 1643 float newmass = CalculateMass();
1236 1644
@@ -1260,7 +1668,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1260 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1668 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1261 } 1669 }
1262 1670
1263 1671
1264 d.BodyDestroy(Body); 1672 d.BodyDestroy(Body);
1265 lock (childrenPrim) 1673 lock (childrenPrim)
1266 { 1674 {
@@ -1279,7 +1687,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1279 else 1687 else
1280 { 1688 {
1281 _parent_scene.remActivePrim(this); 1689 _parent_scene.remActivePrim(this);
1282 1690
1283 m_collisionCategories &= ~CollisionCategories.Body; 1691 m_collisionCategories &= ~CollisionCategories.Body;
1284 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 1692 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1285 1693
@@ -1289,7 +1697,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1289 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1697 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1290 } 1698 }
1291 1699
1292 1700
1293 Body = IntPtr.Zero; 1701 Body = IntPtr.Zero;
1294 } 1702 }
1295 } 1703 }
@@ -1345,10 +1753,10 @@ namespace OpenSim.Region.Physics.OdePlugin
1345 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1753 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1346 try 1754 try
1347 { 1755 {
1348 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer 1756 // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer
1349 // { 1757 // {
1350 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 1758 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
1351 // } 1759 // }
1352 } 1760 }
1353 catch (AccessViolationException) 1761 catch (AccessViolationException)
1354 { 1762 {
@@ -1357,14 +1765,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1357 } 1765 }
1358 1766
1359 1767
1360 // if (IsPhysical && Body == (IntPtr) 0) 1768 // if (IsPhysical && Body == (IntPtr) 0)
1361 // { 1769 // {
1362 // Recreate the body 1770 // Recreate the body
1363 // m_interpenetrationcount = 0; 1771 // m_interpenetrationcount = 0;
1364 // m_collisionscore = 0; 1772 // m_collisionscore = 0;
1365 1773
1366 // enableBody(); 1774 // enableBody();
1367 // } 1775 // }
1368 } 1776 }
1369 1777
1370 public void ProcessTaints(float timestep) //============================================================================= 1778 public void ProcessTaints(float timestep) //=============================================================================
@@ -1373,37 +1781,37 @@ namespace OpenSim.Region.Physics.OdePlugin
1373 { 1781 {
1374 changeadd(timestep); 1782 changeadd(timestep);
1375 } 1783 }
1376 1784
1377 if (prim_geom != IntPtr.Zero) 1785 if (prim_geom != IntPtr.Zero)
1378 { 1786 {
1379 if (!_position.ApproxEquals(m_taintposition, 0f)) 1787 if (!_position.ApproxEquals(m_taintposition, 0f))
1380 { 1788 {
1381 changemove(timestep); 1789 changemove(timestep);
1382 } 1790 }
1383 if (m_taintrot != _orientation) 1791 if (m_taintrot != _orientation)
1384 { 1792 {
1385 if(childPrim && IsPhysical) // For physical child prim... 1793 if (childPrim && IsPhysical) // For physical child prim...
1386 { 1794 {
1387 rotate(timestep); 1795 rotate(timestep);
1388 // KF: ODE will also rotate the parent prim! 1796 // KF: ODE will also rotate the parent prim!
1389 // so rotate the root back to where it was 1797 // so rotate the root back to where it was
1390 OdePrim parent = (OdePrim)_parent; 1798 OdePrim parent = (OdePrim)_parent;
1391 parent.rotate(timestep); 1799 parent.rotate(timestep);
1392 } 1800 }
1393 else 1801 else
1394 { 1802 {
1395 //Just rotate the prim 1803 //Just rotate the prim
1396 rotate(timestep); 1804 rotate(timestep);
1397 } 1805 }
1398 } 1806 }
1399 // 1807 //
1400 1808
1401 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 1809 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1402 { 1810 {
1403 changePhysicsStatus(timestep); 1811 changePhysicsStatus(timestep);
1404 }// 1812 }//
1405 1813
1406 if (!_size.ApproxEquals(m_taintsize,0f)) 1814 if (!_size.ApproxEquals(m_taintsize, 0f))
1407 changesize(timestep); 1815 changesize(timestep);
1408 // 1816 //
1409 1817
@@ -1434,10 +1842,14 @@ namespace OpenSim.Region.Physics.OdePlugin
1434 1842
1435 if (m_taintCollidesWater != m_collidesWater) 1843 if (m_taintCollidesWater != m_collidesWater)
1436 changefloatonwater(timestep); 1844 changefloatonwater(timestep);
1437/* obsolete 1845
1438 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) 1846 if (m_taintserial != null)
1439 changeAngularLock(timestep); 1847 DoSerialize(m_taintserial);
1440 */ 1848
1849 /* obsolete
1850 if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f))
1851 changeAngularLock(timestep);
1852 */
1441 } 1853 }
1442 else 1854 else
1443 { 1855 {
@@ -1445,16 +1857,16 @@ namespace OpenSim.Region.Physics.OdePlugin
1445 } 1857 }
1446 } 1858 }
1447 1859
1448/* obsolete 1860 /* obsolete
1449 private void changeAngularLock(float timestep) 1861 private void changeAngularLock(float timestep)
1450 { 1862 {
1451 if (_parent == null) 1863 if (_parent == null)
1452 { 1864 {
1453 m_angularLock = m_taintAngularLock; 1865 m_angularLock = m_taintAngularLock;
1454 m_angularLockSet = true; 1866 m_angularLockSet = true;
1455 } 1867 }
1456 } 1868 }
1457 */ 1869 */
1458 private void changelink(float timestep) 1870 private void changelink(float timestep)
1459 { 1871 {
1460 // If the newly set parent is not null 1872 // If the newly set parent is not null
@@ -1489,7 +1901,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1489 childPrim = false; 1901 childPrim = false;
1490 //_parent = null; 1902 //_parent = null;
1491 } 1903 }
1492 1904
1493 /* 1905 /*
1494 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) 1906 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
1495 d.JointGroupDestroy(_linkJointGroup); 1907 d.JointGroupDestroy(_linkJointGroup);
@@ -1498,7 +1910,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1498 m_linkJoint = (IntPtr)0; 1910 m_linkJoint = (IntPtr)0;
1499 */ 1911 */
1500 } 1912 }
1501 1913
1502 _parent = m_taintparent; 1914 _parent = m_taintparent;
1503 m_taintPhysics = m_isphysical; 1915 m_taintPhysics = m_isphysical;
1504 } 1916 }
@@ -1512,8 +1924,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1512 if (Body == IntPtr.Zero) 1924 if (Body == IntPtr.Zero)
1513 { 1925 {
1514 Body = d.BodyCreate(_parent_scene.world); 1926 Body = d.BodyCreate(_parent_scene.world);
1515 // disconnect from world gravity so we can apply buoyancy 1927 // disconnect from world gravity so we can apply buoyancy
1516 d.BodySetGravityMode (Body, false); 1928 d.BodySetGravityMode(Body, false);
1517 1929
1518 setMass(); 1930 setMass();
1519 } 1931 }
@@ -1524,7 +1936,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1524 if (!childrenPrim.Contains(prim)) 1936 if (!childrenPrim.Contains(prim))
1525 { 1937 {
1526 childrenPrim.Add(prim); 1938 childrenPrim.Add(prim);
1527 1939
1528 foreach (OdePrim prm in childrenPrim) 1940 foreach (OdePrim prm in childrenPrim)
1529 { 1941 {
1530 d.Mass m2; 1942 d.Mass m2;
@@ -1546,7 +1958,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1546 } 1958 }
1547 foreach (OdePrim prm in childrenPrim) 1959 foreach (OdePrim prm in childrenPrim)
1548 { 1960 {
1549 1961
1550 prm.m_collisionCategories |= CollisionCategories.Body; 1962 prm.m_collisionCategories |= CollisionCategories.Body;
1551 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1963 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1552 1964
@@ -1571,7 +1983,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1571 { 1983 {
1572 d.GeomSetBody(prm.prim_geom, Body); 1984 d.GeomSetBody(prm.prim_geom, Body);
1573 prm.childPrim = true; 1985 prm.childPrim = true;
1574 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); 1986 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z);
1575 //d.GeomSetOffsetPosition(prim.prim_geom, 1987 //d.GeomSetOffsetPosition(prim.prim_geom,
1576 // (Position.X - prm.Position.X) - pMass.c.X, 1988 // (Position.X - prm.Position.X) - pMass.c.X,
1577 // (Position.Y - prm.Position.Y) - pMass.c.Y, 1989 // (Position.Y - prm.Position.Y) - pMass.c.Y,
@@ -1668,7 +2080,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1668 ParentPrim(prm); 2080 ParentPrim(prm);
1669 } 2081 }
1670 } 2082 }
1671 2083
1672 } 2084 }
1673 2085
1674 private void ChildDelink(OdePrim odePrim) 2086 private void ChildDelink(OdePrim odePrim)
@@ -1719,7 +2131,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1719 // in between the disabling and the collision properties setting 2131 // in between the disabling and the collision properties setting
1720 // which would wake the physical body up from a soft disabling and potentially cause it to fall 2132 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1721 // through the ground. 2133 // through the ground.
1722 2134
1723 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select 2135 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1724 // just one part of the assembly, the rest of the assembly is non-selected and still simulating, 2136 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1725 // so that causes the selected part to wake up and continue moving. 2137 // so that causes the selected part to wake up and continue moving.
@@ -1752,12 +2164,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1752 { 2164 {
1753 disableBodySoft(); 2165 disableBodySoft();
1754 } 2166 }
1755 if (Body != IntPtr.Zero) 2167 if (Body != IntPtr.Zero)
1756 { 2168 {
1757 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2169 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1758 d.BodySetForce(Body, 0f, 0f, 0f); 2170 d.BodySetForce(Body, 0f, 0f, 0f);
1759 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2171 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1760 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2172 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1761 } 2173 }
1762 2174
1763 } 2175 }
@@ -1780,12 +2192,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1780 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2192 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1781 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2193 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1782 } 2194 }
1783 if (Body != IntPtr.Zero) 2195 if (Body != IntPtr.Zero)
1784 { 2196 {
1785 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2197 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1786 d.BodySetForce(Body, 0f, 0f, 0f); 2198 d.BodySetForce(Body, 0f, 0f, 0f);
1787 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 2199 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
1788 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); 2200 d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f);
1789 } 2201 }
1790 2202
1791 if (m_isphysical) 2203 if (m_isphysical)
@@ -1913,7 +2325,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1913 m_meshfailed = true; 2325 m_meshfailed = true;
1914 } 2326 }
1915 // createmesh returns null when it's a shape that isn't a cube. 2327 // createmesh returns null when it's a shape that isn't a cube.
1916 // m_log.Debug(m_localID); 2328 // m_log.Debug(m_localID);
1917 } 2329 }
1918 } 2330 }
1919 2331
@@ -1948,7 +2360,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1948 { 2360 {
1949 if (m_isphysical) 2361 if (m_isphysical)
1950 { 2362 {
1951// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! 2363 // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1952 if (!m_taintremove && !childPrim) 2364 if (!m_taintremove && !childPrim)
1953 { 2365 {
1954 if (Body == IntPtr.Zero) 2366 if (Body == IntPtr.Zero)
@@ -1972,8 +2384,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1972 OdePrim odParent = (OdePrim)_parent; 2384 OdePrim odParent = (OdePrim)_parent;
1973 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 2385 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1974 { 2386 {
1975// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 2387 // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1976Console.WriteLine("ODEPrim JointCreateFixed !!!"); 2388 Console.WriteLine("ODEPrim JointCreateFixed !!!");
1977 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 2389 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1978 d.JointAttach(m_linkJoint, Body, odParent.Body); 2390 d.JointAttach(m_linkJoint, Body, odParent.Body);
1979 d.JointSetFixed(m_linkJoint); 2391 d.JointSetFixed(m_linkJoint);
@@ -1991,8 +2403,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
1991 } 2403 }
1992 } 2404 }
1993 //else 2405 //else
1994 // { 2406 // {
1995 //m_log.Debug("[BUG]: race!"); 2407 //m_log.Debug("[BUG]: race!");
1996 //} 2408 //}
1997 } 2409 }
1998 else 2410 else
@@ -2031,15 +2443,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2031 myrot.W = _orientation.W; 2443 myrot.W = _orientation.W;
2032 if (Body != IntPtr.Zero) 2444 if (Body != IntPtr.Zero)
2033 { 2445 {
2034 // KF: If this is a root prim do BodySet 2446 // KF: If this is a root prim do BodySet
2035 d.BodySetQuaternion(Body, ref myrot); 2447 d.BodySetQuaternion(Body, ref myrot);
2036 }
2037 else
2038 {
2039 // daughter prim, do Geom set
2040 d.GeomSetQuaternion(prim_geom, ref myrot);
2041 } 2448 }
2042 2449 else
2450 {
2451 // daughter prim, do Geom set
2452 d.GeomSetQuaternion(prim_geom, ref myrot);
2453 }
2454
2043 resetCollisionAccounting(); 2455 resetCollisionAccounting();
2044 m_taintrot = _orientation; 2456 m_taintrot = _orientation;
2045 } 2457 }
@@ -2111,7 +2523,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2111 2523
2112 public void changesize(float timestamp) 2524 public void changesize(float timestamp)
2113 { 2525 {
2114 2526
2115 string oldname = _parent_scene.geom_name_map[prim_geom]; 2527 string oldname = _parent_scene.geom_name_map[prim_geom];
2116 2528
2117 if (_size.X <= 0) _size.X = 0.01f; 2529 if (_size.X <= 0) _size.X = 0.01f;
@@ -2170,7 +2582,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2170 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2582 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2171 CreateGeom(m_targetSpace, mesh); 2583 CreateGeom(m_targetSpace, mesh);
2172 2584
2173 2585
2174 } 2586 }
2175 else 2587 else
2176 { 2588 {
@@ -2210,7 +2622,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2210 m_taintsize = _size; 2622 m_taintsize = _size;
2211 } 2623 }
2212 2624
2213 2625
2214 2626
2215 public void changefloatonwater(float timestep) 2627 public void changefloatonwater(float timestep)
2216 { 2628 {
@@ -2398,7 +2810,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2398 } 2810 }
2399 d.BodyEnable(Body); 2811 d.BodyEnable(Body);
2400 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); 2812 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
2401 2813
2402 } 2814 }
2403 m_angularforcelist.Clear(); 2815 m_angularforcelist.Clear();
2404 } 2816 }
@@ -2420,7 +2832,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2420 if (Body != IntPtr.Zero) 2832 if (Body != IntPtr.Zero)
2421 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); 2833 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
2422 } 2834 }
2423 2835
2424 //resetCollisionAccounting(); 2836 //resetCollisionAccounting();
2425 } 2837 }
2426 m_taintVelocity = Vector3.Zero; 2838 m_taintVelocity = Vector3.Zero;
@@ -2428,9 +2840,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2428 2840
2429 public void UpdatePositionAndVelocity() 2841 public void UpdatePositionAndVelocity()
2430 { 2842 {
2431 return; // moved to the Move () method 2843 return; // moved to the Move () method
2432 } 2844 }
2433 2845
2434 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2846 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2435 { 2847 {
2436 obj.I.M00 = pMat[0, 0]; 2848 obj.I.M00 = pMat[0, 0];
@@ -2455,7 +2867,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2455 { 2867 {
2456 _parent_scene.remCollisionEventReporting(this); 2868 _parent_scene.remCollisionEventReporting(this);
2457 m_eventsubscription = 0; 2869 m_eventsubscription = 0;
2458 } 2870 }
2459 2871
2460 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 2872 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
2461 { 2873 {
@@ -2499,9 +2911,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2499 public static Matrix4 Adjoint(Matrix4 pMat) 2911 public static Matrix4 Adjoint(Matrix4 pMat)
2500 { 2912 {
2501 Matrix4 adjointMatrix = new Matrix4(); 2913 Matrix4 adjointMatrix = new Matrix4();
2502 for (int i=0; i<4; i++) 2914 for (int i = 0; i < 4; i++)
2503 { 2915 {
2504 for (int j=0; j<4; j++) 2916 for (int j = 0; j < 4; j++)
2505 { 2917 {
2506 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); 2918 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
2507 } 2919 }
@@ -2524,7 +2936,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2524 { 2936 {
2525 if (j == iCol) 2937 if (j == iCol)
2526 continue; 2938 continue;
2527 Matrix4SetValue(ref minor, m,n, matrix[i, j]); 2939 Matrix4SetValue(ref minor, m, n, matrix[i, j]);
2528 n++; 2940 n++;
2529 } 2941 }
2530 m++; 2942 m++;
@@ -2622,18 +3034,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2622 private static float determinant3x3(Matrix4 pMat) 3034 private static float determinant3x3(Matrix4 pMat)
2623 { 3035 {
2624 float det = 0; 3036 float det = 0;
2625 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; 3037 float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2];
2626 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; 3038 float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0];
2627 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; 3039 float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1];
2628 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; 3040 float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2];
2629 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; 3041 float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0];
2630 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; 3042 float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1];
2631 3043
2632 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); 3044 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
2633 return det; 3045 return det;
2634 3046
2635 } 3047 }
2636 3048
2637 private static void DMassCopy(ref d.Mass src, ref d.Mass dst) 3049 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
2638 { 3050 {
2639 dst.c.W = src.c.W; 3051 dst.c.W = src.c.W;
@@ -2690,15 +3102,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2690 // m_bankingTimescale = pValue; 3102 // m_bankingTimescale = pValue;
2691 break; 3103 break;
2692 case Vehicle.BUOYANCY: 3104 case Vehicle.BUOYANCY:
2693 if (pValue < -1f) pValue = -1f; 3105 if (pValue < -1f) pValue = -1f;
2694 if (pValue > 1f) pValue = 1f; 3106 if (pValue > 1f) pValue = 1f;
2695 m_VehicleBuoyancy = pValue; 3107 m_VehicleBuoyancy = pValue;
2696 break; 3108 break;
2697// case Vehicle.HOVER_EFFICIENCY: 3109 // case Vehicle.HOVER_EFFICIENCY:
2698// if (pValue < 0f) pValue = 0f; 3110 // if (pValue < 0f) pValue = 0f;
2699// if (pValue > 1f) pValue = 1f; 3111 // if (pValue > 1f) pValue = 1f;
2700// m_VhoverEfficiency = pValue; 3112 // m_VhoverEfficiency = pValue;
2701// break; 3113 // break;
2702 case Vehicle.HOVER_HEIGHT: 3114 case Vehicle.HOVER_HEIGHT:
2703 m_VhoverHeight = pValue; 3115 m_VhoverHeight = pValue;
2704 break; 3116 break;
@@ -2731,12 +3143,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2731 if (pValue < 0.1f) pValue = 0.1f; 3143 if (pValue < 0.1f) pValue = 0.1f;
2732 m_verticalAttractionTimescale = pValue; 3144 m_verticalAttractionTimescale = pValue;
2733 break; 3145 break;
2734 3146
2735 // These are vector properties but the engine lets you use a single float value to 3147 // These are vector properties but the engine lets you use a single float value to
2736 // set all of the components to the same value 3148 // set all of the components to the same value
2737 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3149 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2738 if (pValue > 30f) pValue = 30f; 3150 if (pValue > 30f) pValue = 30f;
2739 if (pValue < 0.1f) pValue = 0.1f; 3151 if (pValue < 0.1f) pValue = 0.1f;
2740 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); 3152 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2741 break; 3153 break;
2742 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3154 case Vehicle.ANGULAR_MOTOR_DIRECTION:
@@ -2744,7 +3156,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2744 UpdateAngDecay(); 3156 UpdateAngDecay();
2745 break; 3157 break;
2746 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3158 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2747 if (pValue < 0.1f) pValue = 0.1f; 3159 if (pValue < 0.1f) pValue = 0.1f;
2748 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); 3160 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2749 break; 3161 break;
2750 case Vehicle.LINEAR_MOTOR_DIRECTION: 3162 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2756,7 +3168,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2756 break; 3168 break;
2757 3169
2758 } 3170 }
2759 3171
2760 }//end ProcessFloatVehicleParam 3172 }//end ProcessFloatVehicleParam
2761 3173
2762 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) 3174 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
@@ -2764,29 +3176,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2764 switch (pParam) 3176 switch (pParam)
2765 { 3177 {
2766 case Vehicle.ANGULAR_FRICTION_TIMESCALE: 3178 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2767 if (pValue.X > 30f) pValue.X = 30f; 3179 if (pValue.X > 30f) pValue.X = 30f;
2768 if (pValue.X < 0.1f) pValue.X = 0.1f; 3180 if (pValue.X < 0.1f) pValue.X = 0.1f;
2769 if (pValue.Y > 30f) pValue.Y = 30f; 3181 if (pValue.Y > 30f) pValue.Y = 30f;
2770 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3182 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2771 if (pValue.Z > 30f) pValue.Z = 30f; 3183 if (pValue.Z > 30f) pValue.Z = 30f;
2772 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3184 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2773 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3185 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2774 break; 3186 break;
2775 case Vehicle.ANGULAR_MOTOR_DIRECTION: 3187 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2776 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); 3188 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2777 // Limit requested angular speed to 2 rps= 4 pi rads/sec 3189 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2778 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; 3190 if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2779 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; 3191 if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f;
2780 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; 3192 if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2781 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; 3193 if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f;
2782 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; 3194 if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2783 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; 3195 if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f;
2784 UpdateAngDecay(); 3196 UpdateAngDecay();
2785 break; 3197 break;
2786 case Vehicle.LINEAR_FRICTION_TIMESCALE: 3198 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2787 if (pValue.X < 0.1f) pValue.X = 0.1f; 3199 if (pValue.X < 0.1f) pValue.X = 0.1f;
2788 if (pValue.Y < 0.1f) pValue.Y = 0.1f; 3200 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2789 if (pValue.Z < 0.1f) pValue.Z = 0.1f; 3201 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2790 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); 3202 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2791 break; 3203 break;
2792 case Vehicle.LINEAR_MOTOR_DIRECTION: 3204 case Vehicle.LINEAR_MOTOR_DIRECTION:
@@ -2797,7 +3209,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2797 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); 3209 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2798 break; 3210 break;
2799 } 3211 }
2800 3212
2801 }//end ProcessVectorVehicleParam 3213 }//end ProcessVectorVehicleParam
2802 3214
2803 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) 3215 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
@@ -2808,31 +3220,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2808 // m_referenceFrame = pValue; 3220 // m_referenceFrame = pValue;
2809 break; 3221 break;
2810 } 3222 }
2811 3223
2812 }//end ProcessRotationVehicleParam 3224 }//end ProcessRotationVehicleParam
2813 3225
2814 internal void ProcessVehicleFlags(int pParam, bool remove) 3226 internal void ProcessVehicleFlags(int pParam, bool remove)
2815 { 3227 {
2816 if (remove) 3228 if (remove)
2817 { 3229 {
2818 m_flags &= ~((VehicleFlag)pParam); 3230 m_flags &= ~((VehicleFlag)pParam);
2819 } 3231 }
2820 else 3232 else
2821 { 3233 {
2822 m_flags |= (VehicleFlag)pParam; 3234 m_flags |= (VehicleFlag)pParam;
2823 } 3235 }
2824 } 3236 }
2825 3237
2826 internal void ProcessTypeChange(Vehicle pType) 3238 internal void ProcessTypeChange(Vehicle pType)
2827 { 3239 {
2828 // Set Defaults For Type 3240 // Set Defaults For Type
2829 m_type = pType; 3241 m_type = pType;
2830 switch (pType) 3242 switch (pType)
2831 { 3243 {
2832 case Vehicle.TYPE_SLED: 3244 case Vehicle.TYPE_SLED:
2833 m_linearFrictionTimescale = new Vector3(30, 1, 1000); 3245 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2834 m_angularFrictionTimescale = new Vector3(30, 30, 30); 3246 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2835// m_lLinMotorVel = Vector3.Zero; 3247 // m_lLinMotorVel = Vector3.Zero;
2836 m_linearMotorTimescale = 1000; 3248 m_linearMotorTimescale = 1000;
2837 m_linearMotorDecayTimescale = 120; 3249 m_linearMotorDecayTimescale = 120;
2838 m_angularMotorDirection = Vector3.Zero; 3250 m_angularMotorDirection = Vector3.Zero;
@@ -2840,7 +3252,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2840 m_angularMotorTimescale = 1000; 3252 m_angularMotorTimescale = 1000;
2841 m_angularMotorDecayTimescale = 120; 3253 m_angularMotorDecayTimescale = 120;
2842 m_VhoverHeight = 0; 3254 m_VhoverHeight = 0;
2843// m_VhoverEfficiency = 1; 3255 // m_VhoverEfficiency = 1;
2844 m_VhoverTimescale = 10; 3256 m_VhoverTimescale = 10;
2845 m_VehicleBuoyancy = 0; 3257 m_VehicleBuoyancy = 0;
2846 // m_linearDeflectionEfficiency = 1; 3258 // m_linearDeflectionEfficiency = 1;
@@ -2859,7 +3271,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2859 case Vehicle.TYPE_CAR: 3271 case Vehicle.TYPE_CAR:
2860 m_linearFrictionTimescale = new Vector3(100, 2, 1000); 3272 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2861 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. 3273 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2862// m_lLinMotorVel = Vector3.Zero; 3274 // m_lLinMotorVel = Vector3.Zero;
2863 m_linearMotorTimescale = 1; 3275 m_linearMotorTimescale = 1;
2864 m_linearMotorDecayTimescale = 60; 3276 m_linearMotorDecayTimescale = 60;
2865 m_angularMotorDirection = Vector3.Zero; 3277 m_angularMotorDirection = Vector3.Zero;
@@ -2867,7 +3279,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2867 m_angularMotorTimescale = 1; 3279 m_angularMotorTimescale = 1;
2868 m_angularMotorDecayTimescale = 0.8f; 3280 m_angularMotorDecayTimescale = 0.8f;
2869 m_VhoverHeight = 0; 3281 m_VhoverHeight = 0;
2870// m_VhoverEfficiency = 0; 3282 // m_VhoverEfficiency = 0;
2871 m_VhoverTimescale = 1000; 3283 m_VhoverTimescale = 1000;
2872 m_VehicleBuoyancy = 0; 3284 m_VehicleBuoyancy = 0;
2873 // // m_linearDeflectionEfficiency = 1; 3285 // // m_linearDeflectionEfficiency = 1;
@@ -2886,8 +3298,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2886 break; 3298 break;
2887 case Vehicle.TYPE_BOAT: 3299 case Vehicle.TYPE_BOAT:
2888 m_linearFrictionTimescale = new Vector3(10, 3, 2); 3300 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2889 m_angularFrictionTimescale = new Vector3(10,10,10); 3301 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2890// m_lLinMotorVel = Vector3.Zero; 3302 // m_lLinMotorVel = Vector3.Zero;
2891 m_linearMotorTimescale = 5; 3303 m_linearMotorTimescale = 5;
2892 m_linearMotorDecayTimescale = 60; 3304 m_linearMotorDecayTimescale = 60;
2893 m_angularMotorDirection = Vector3.Zero; 3305 m_angularMotorDirection = Vector3.Zero;
@@ -2895,7 +3307,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2895 m_angularMotorTimescale = 4; 3307 m_angularMotorTimescale = 4;
2896 m_angularMotorDecayTimescale = 4; 3308 m_angularMotorDecayTimescale = 4;
2897 m_VhoverHeight = 0; 3309 m_VhoverHeight = 0;
2898// m_VhoverEfficiency = 0.5f; 3310 // m_VhoverEfficiency = 0.5f;
2899 m_VhoverTimescale = 2; 3311 m_VhoverTimescale = 2;
2900 m_VehicleBuoyancy = 1; 3312 m_VehicleBuoyancy = 1;
2901 // m_linearDeflectionEfficiency = 0.5f; 3313 // m_linearDeflectionEfficiency = 0.5f;
@@ -2908,15 +3320,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2908 // m_bankingMix = 0.8f; 3320 // m_bankingMix = 0.8f;
2909 // m_bankingTimescale = 1; 3321 // m_bankingTimescale = 1;
2910 // m_referenceFrame = Quaternion.Identity; 3322 // m_referenceFrame = Quaternion.Identity;
2911 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | 3323 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2912 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); 3324 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2913 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | 3325 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2914 VehicleFlag.LIMIT_MOTOR_UP); 3326 VehicleFlag.LIMIT_MOTOR_UP);
2915 break; 3327 break;
2916 case Vehicle.TYPE_AIRPLANE: 3328 case Vehicle.TYPE_AIRPLANE:
2917 m_linearFrictionTimescale = new Vector3(200, 10, 5); 3329 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2918 m_angularFrictionTimescale = new Vector3(20, 20, 20); 3330 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2919// m_lLinMotorVel = Vector3.Zero; 3331 // m_lLinMotorVel = Vector3.Zero;
2920 m_linearMotorTimescale = 2; 3332 m_linearMotorTimescale = 2;
2921 m_linearMotorDecayTimescale = 60; 3333 m_linearMotorDecayTimescale = 60;
2922 m_angularMotorDirection = Vector3.Zero; 3334 m_angularMotorDirection = Vector3.Zero;
@@ -2924,7 +3336,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2924 m_angularMotorTimescale = 4; 3336 m_angularMotorTimescale = 4;
2925 m_angularMotorDecayTimescale = 4; 3337 m_angularMotorDecayTimescale = 4;
2926 m_VhoverHeight = 0; 3338 m_VhoverHeight = 0;
2927// m_VhoverEfficiency = 0.5f; 3339 // m_VhoverEfficiency = 0.5f;
2928 m_VhoverTimescale = 1000; 3340 m_VhoverTimescale = 1000;
2929 m_VehicleBuoyancy = 0; 3341 m_VehicleBuoyancy = 0;
2930 // m_linearDeflectionEfficiency = 0.5f; 3342 // m_linearDeflectionEfficiency = 0.5f;
@@ -2951,7 +3363,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2951 m_angularMotorTimescale = 6; 3363 m_angularMotorTimescale = 6;
2952 m_angularMotorDecayTimescale = 10; 3364 m_angularMotorDecayTimescale = 10;
2953 m_VhoverHeight = 5; 3365 m_VhoverHeight = 5;
2954// m_VhoverEfficiency = 0.8f; 3366 // m_VhoverEfficiency = 0.8f;
2955 m_VhoverTimescale = 10; 3367 m_VhoverTimescale = 10;
2956 m_VehicleBuoyancy = 1; 3368 m_VehicleBuoyancy = 1;
2957 // m_linearDeflectionEfficiency = 0; 3369 // m_linearDeflectionEfficiency = 0;
@@ -2981,63 +3393,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
2981 } 3393 }
2982 3394
2983 3395
2984 internal void Halt() 3396 internal void Halt()
2985 { // Kill all motions, when non-physical 3397 { // Kill all motions, when non-physical
2986 // m_linearMotorDirection = Vector3.Zero; 3398 // m_linearMotorDirection = Vector3.Zero;
2987 m_lLinMotorDVel = Vector3.Zero; 3399 m_lLinMotorDVel = Vector3.Zero;
2988 m_lLinObjectVel = Vector3.Zero; 3400 m_lLinObjectVel = Vector3.Zero;
2989 m_wLinObjectVel = Vector3.Zero; 3401 m_wLinObjectVel = Vector3.Zero;
2990 m_angularMotorDirection = Vector3.Zero; 3402 m_angularMotorDirection = Vector3.Zero;
2991 m_lastAngularVelocity = Vector3.Zero; 3403 m_lastAngularVelocity = Vector3.Zero;
2992 m_angularMotorDVel = Vector3.Zero; 3404 m_angularMotorDVel = Vector3.Zero;
2993 _acceleration = Vector3.Zero; 3405 _acceleration = Vector3.Zero;
2994 } 3406 }
2995 3407
2996 private void UpdateLinDecay() 3408 private void UpdateLinDecay()
2997 { 3409 {
2998// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; 3410 m_lLinMotorDVel.X = m_linearMotorDirection.X;
2999// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3411 m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3000// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3412 m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3001 m_lLinMotorDVel.X = m_linearMotorDirection.X; 3413 } // else let the motor decay on its own
3002 m_lLinMotorDVel.Y = m_linearMotorDirection.Y; 3414
3003 m_lLinMotorDVel.Z = m_linearMotorDirection.Z; 3415 private void UpdateAngDecay()
3004 } // else let the motor decay on its own 3416 {
3005 3417 m_angularMotorDVel.X = m_angularMotorDirection.X;
3006 private void UpdateAngDecay() 3418 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3007 { 3419 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3008// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; 3420 } // else let the motor decay on its own
3009// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; 3421
3010// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3011 m_angularMotorDVel.X = m_angularMotorDirection.X;
3012 m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3013 m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3014 } // else let the motor decay on its own
3015
3016 public void Move(float timestep) 3422 public void Move(float timestep)
3017 { 3423 {
3018 float fx = 0; 3424 float fx = 0;
3019 float fy = 0; 3425 float fy = 0;
3020 float fz = 0; 3426 float fz = 0;
3021 Vector3 linvel; // velocity applied, including any reversal 3427 Vector3 linvel; // velocity applied, including any reversal
3022 int outside = 0; 3428
3023
3024 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. 3429 // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders.
3025 // This is a temp patch until proper region crossing is developed. 3430 // This is a temp patch until proper region crossing is developed.
3026 3431
3027 int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; 3432
3028 float fence = _parent_scene.geomRegionFence;
3029
3030 frcount++; // used to limit debug comment output
3031 if (frcount > 50)
3032 frcount = 0;
3033
3034 if(revcount > 0) revcount--;
3035
3036 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. 3433 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims.
3037 { 3434 {
3038 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here 3435 // Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3039 bool lastZeroFlag = _zeroFlag; // was it stopped 3436 bool lastZeroFlag = _zeroFlag; // was it stopped
3040 3437
3041 d.Vector3 vec = d.BodyGetPosition(Body); 3438 d.Vector3 vec = d.BodyGetPosition(Body);
3042 Vector3 l_position = Vector3.Zero; 3439 Vector3 l_position = Vector3.Zero;
3043 l_position.X = vec.X; 3440 l_position.X = vec.X;
@@ -3045,95 +3442,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3045 l_position.Z = vec.Z; 3442 l_position.Z = vec.Z;
3046 m_lastposition = _position; 3443 m_lastposition = _position;
3047 _position = l_position; 3444 _position = l_position;
3048 3445
3049 d.Quaternion ori = d.BodyGetQuaternion(Body); 3446 d.Quaternion ori = d.BodyGetQuaternion(Body);
3050 // Quaternion l_orientation = Quaternion.Identity; 3447 // Quaternion l_orientation = Quaternion.Identity;
3051 _orientation.X = ori.X; 3448 _orientation.X = ori.X;
3052 _orientation.Y = ori.Y; 3449 _orientation.Y = ori.Y;
3053 _orientation.Z = ori.Z; 3450 _orientation.Z = ori.Z;
3054 _orientation.W = ori.W; 3451 _orientation.W = ori.W;
3055 m_lastorientation = _orientation; 3452 m_lastorientation = _orientation;
3056 3453
3057 d.Vector3 vel = d.BodyGetLinearVel(Body); 3454 d.Vector3 vel = d.BodyGetLinearVel(Body);
3058 m_lastVelocity = _velocity; 3455 m_lastVelocity = _velocity;
3059 _velocity.X = vel.X; 3456 _velocity.X = vel.X;
3060 _velocity.Y = vel.Y; 3457 _velocity.Y = vel.Y;
3061 _velocity.Z = vel.Z; 3458 _velocity.Z = vel.Z;
3062 _acceleration = ((_velocity - m_lastVelocity) / timestep); 3459 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3063 3460
3064 d.Vector3 torque = d.BodyGetTorque(Body); 3461 d.Vector3 torque = d.BodyGetTorque(Body);
3065 _torque = new Vector3(torque.X, torque.Y, torque.Z); 3462 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3066 3463
3067 3464
3068//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3069 /*
3070 // Check if outside region
3071 // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border!
3072 if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence))
3073 {
3074 l_position.X = ((float)_parent_scene.WorldExtents.X - fence);
3075 outside = 1;
3076 }
3077
3078 if (l_position.X < fence)
3079 {
3080 l_position.X = fence;
3081 outside = 2;
3082 }
3083 if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence))
3084 {
3085 l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence);
3086 outside = 3;
3087 }
3088
3089 if (l_position.Y < fence)
3090 {
3091 l_position.Y = fence;
3092 outside = 4;
3093 }
3094
3095 if (outside > 0)
3096 {
3097
3098//Console.WriteLine("Border {0} fence={1}", l_position, fence);
3099 if (fence > 0.0f) // bounce object off boundary
3100 {
3101 if (revcount == 0)
3102 {
3103 if (outside < 3)
3104 {
3105 _velocity.X = -_velocity.X;
3106 }
3107 else
3108 {
3109 _velocity.Y = -_velocity.Y;
3110 }
3111 if (m_type != Vehicle.TYPE_NONE) Halt();
3112 _position = l_position;
3113 m_taintposition = _position;
3114 m_lastVelocity = _velocity;
3115 _acceleration = Vector3.Zero;
3116 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3117 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
3118 base.RequestPhysicsterseUpdate();
3119
3120 revcount = 25; // wait for object to move away from border
3121 }
3122 } // else old crossing mode
3123 else if (m_crossingfailures < failureLimit)
3124 { // keep trying to cross?
3125 _position = l_position;
3126 //_parent_scene.remActivePrim(this);
3127 if (_parent == null) base.RequestPhysicsterseUpdate();
3128 return; // Dont process any other motion?
3129 }
3130 else
3131 { // Too many tries
3132 if (_parent == null) base.RaiseOutOfBounds(l_position);
3133 return; // Dont process any other motion?
3134 } // end various methods
3135 } // end outside region horizontally
3136 */
3137 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X 3465 if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X
3138 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y 3466 || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y
3139 ) 3467 )
@@ -3146,41 +3474,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3146 _position.Z = Util.Clip(l_position.Z, -100f, 50000f); 3474 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3147 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 3475 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3148 d.BodySetLinearVel(Body, 0, 0, 0); 3476 d.BodySetLinearVel(Body, 0, 0, 0);
3149 /*
3150 if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0)
3151 { // tell base code only once
3152 Interlocked.Increment(ref m_crossingfailures);
3153 base.RequestPhysicsterseUpdate();
3154 }
3155 */
3156 m_outofBounds = true; 3477 m_outofBounds = true;
3157 base.RequestPhysicsterseUpdate(); 3478 base.RequestPhysicsterseUpdate();
3158 return; 3479 return;
3159 } 3480 }
3160/*
3161 if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0)
3162 {
3163 // main simulator had a crossing failure
3164 // park it inside region
3165 _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
3166 _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
3167 _position.Z = Util.Clip(l_position.Z, -100f, 50000f);
3168 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
3169
3170 m_lastposition = _position;
3171
3172 _velocity = Vector3.Zero;
3173 m_lastVelocity = _velocity;
3174 3481
3175
3176 if (m_type != Vehicle.TYPE_NONE)
3177 Halt();
3178
3179 d.BodySetLinearVel(Body, 0, 0, 0);
3180 base.RequestPhysicsterseUpdate();
3181 return;
3182 }
3183*/
3184 base.RequestPhysicsterseUpdate(); 3482 base.RequestPhysicsterseUpdate();
3185 3483
3186 if (l_position.Z < 0) 3484 if (l_position.Z < 0)
@@ -3193,8 +3491,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3193 3491
3194 //IsPhysical = false; 3492 //IsPhysical = false;
3195 if (_parent == null) base.RaiseOutOfBounds(_position); 3493 if (_parent == null) base.RaiseOutOfBounds(_position);
3196 3494
3197 3495
3198 _acceleration.X = 0; // This stuff may stop client display but it has no 3496 _acceleration.X = 0; // This stuff may stop client display but it has no
3199 _acceleration.Y = 0; // effect on the object in phys engine! 3497 _acceleration.Y = 0; // effect on the object in phys engine!
3200 _acceleration.Z = 0; 3498 _acceleration.Z = 0;
@@ -3215,13 +3513,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3215 //outofBounds = true; 3513 //outofBounds = true;
3216 } // end neg Z check 3514 } // end neg Z check
3217 3515
3218 // Is it moving? 3516 // Is it moving?
3219 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3517 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3220 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3518 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3221 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ 3519 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3222 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly 3520 if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3223 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating 3521 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3224 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly 3522 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly
3225 { 3523 {
3226 _zeroFlag = true; 3524 _zeroFlag = true;
3227 m_throttleUpdates = false; 3525 m_throttleUpdates = false;
@@ -3238,18 +3536,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3238 { // Its stopped 3536 { // Its stopped
3239 _velocity.X = 0.0f; 3537 _velocity.X = 0.0f;
3240 _velocity.Y = 0.0f; 3538 _velocity.Y = 0.0f;
3241 // _velocity.Z = 0.0f; 3539 // _velocity.Z = 0.0f;
3242 3540
3243 _acceleration.X = 0; 3541 _acceleration.X = 0;
3244 _acceleration.Y = 0; 3542 _acceleration.Y = 0;
3245 // _acceleration.Z = 0; 3543 // _acceleration.Z = 0;
3246 3544
3247 m_rotationalVelocity.X = 0; 3545 m_rotationalVelocity.X = 0;
3248 m_rotationalVelocity.Y = 0; 3546 m_rotationalVelocity.Y = 0;
3249 m_rotationalVelocity.Z = 0; 3547 m_rotationalVelocity.Z = 0;
3250 // Stop it in the phys engine 3548 // Stop it in the phys engine
3251 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); 3549 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3252 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 3550 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3253 d.BodySetForce(Body, 0f, 0f, 0f); 3551 d.BodySetForce(Body, 0f, 0f, 0f);
3254 3552
3255 if (!m_lastUpdateSent) 3553 if (!m_lastUpdateSent)
@@ -3287,71 +3585,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3287 } 3585 }
3288 } 3586 }
3289 m_lastposition = l_position; 3587 m_lastposition = l_position;
3290 3588
3291 /// End UpdatePositionAndVelocity insert 3589 /// End UpdatePositionAndVelocity insert
3292 3590
3293 3591
3294 // Rotation lock ===================================== 3592 // Rotation lock =====================================
3295 if(m_rotateEnableUpdate) 3593 if (m_rotateEnableUpdate)
3296 { 3594 {
3297 // Snapshot current angles, set up Amotor(s) 3595 // Snapshot current angles, set up Amotor(s)
3298 m_rotateEnableUpdate = false; 3596 m_rotateEnableUpdate = false;
3299 m_rotateEnable = m_rotateEnableRequest; 3597 m_rotateEnable = m_rotateEnableRequest;
3300//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); 3598 //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3301 3599
3302 if (Amotor != IntPtr.Zero) 3600 if (Amotor != IntPtr.Zero)
3303 { 3601 {
3304 d.JointDestroy(Amotor); 3602 d.JointDestroy(Amotor);
3305 Amotor = IntPtr.Zero; 3603 Amotor = IntPtr.Zero;
3306//Console.WriteLine("Old Amotor Destroyed"); 3604 //Console.WriteLine("Old Amotor Destroyed");
3307 } 3605 }
3308 3606
3309 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) 3607 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
3310 { // not all are enabled 3608 { // not all are enabled
3311 d.Quaternion r = d.BodyGetQuaternion(Body); 3609 d.Quaternion r = d.BodyGetQuaternion(Body);
3312 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); 3610 Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W);
3313 // extract the axes vectors 3611 // extract the axes vectors
3314 Vector3 vX = new Vector3(1f,0f,0f); 3612 Vector3 vX = new Vector3(1f, 0f, 0f);
3315 Vector3 vY = new Vector3(0f,1f,0f); 3613 Vector3 vY = new Vector3(0f, 1f, 0f);
3316 Vector3 vZ = new Vector3(0f,0f,1f); 3614 Vector3 vZ = new Vector3(0f, 0f, 1f);
3317 vX = vX * locrot; 3615 vX = vX * locrot;
3318 vY = vY * locrot; 3616 vY = vY * locrot;
3319 vZ = vZ * locrot; 3617 vZ = vZ * locrot;
3320 // snapshot the current angle vectors 3618 // snapshot the current angle vectors
3321 m_lockX = vX; 3619 m_lockX = vX;
3322 m_lockY = vY; 3620 m_lockY = vY;
3323 m_lockZ = vZ; 3621 m_lockZ = vZ;
3324 // m_lockRot = locrot; 3622 // m_lockRot = locrot;
3325 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 3623 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3326 d.JointAttach(Amotor, Body, IntPtr.Zero); 3624 d.JointAttach(Amotor, Body, IntPtr.Zero);
3327 d.JointSetAMotorMode(Amotor, 0); // User mode?? 3625 d.JointSetAMotorMode(Amotor, 0); // User mode??
3328//Console.WriteLine("New Amotor Created for {0}", m_primName); 3626 //Console.WriteLine("New Amotor Created for {0}", m_primName);
3329 3627
3330 float axisnum = 3; // how many to lock 3628 float axisnum = 3; // how many to lock
3331 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); 3629 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3332 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 3630 d.JointSetAMotorNumAxes(Amotor, (int)axisnum);
3333//Console.WriteLine("AxisNum={0}",(int)axisnum); 3631 //Console.WriteLine("AxisNum={0}",(int)axisnum);
3334 3632
3335 int i = 0; 3633 int i = 0;
3336 3634
3337 if (m_rotateEnable.X == 0) 3635 if (m_rotateEnable.X == 0)
3338 { 3636 {
3339 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); 3637 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3340//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); 3638 //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3341 i++; 3639 i++;
3342 } 3640 }
3343 3641
3344 if (m_rotateEnable.Y == 0) 3642 if (m_rotateEnable.Y == 0)
3345 { 3643 {
3346 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); 3644 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3347//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); 3645 //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3348 i++; 3646 i++;
3349 } 3647 }
3350 3648
3351 if (m_rotateEnable.Z == 0) 3649 if (m_rotateEnable.Z == 0)
3352 { 3650 {
3353 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); 3651 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3354//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); 3652 //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3355 i++; 3653 i++;
3356 } 3654 }
3357 3655
@@ -3362,519 +3660,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3362 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); 3660 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3363 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); 3661 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3364 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); 3662 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3365 d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); 3663 d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f);
3366 d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); 3664 d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f);
3367 d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); 3665 d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f);
3368 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); 3666 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f);
3369 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); 3667 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f);
3370 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); 3668 d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f);
3371 } // else none are locked 3669 } // else none are locked
3372 } // end Rotation Update 3670 } // end Rotation Update
3373 3671
3374 3672
3375 // VEHICLE processing ========================================== 3673 // VEHICLE processing ==========================================
3376 if (m_type != Vehicle.TYPE_NONE) 3674 if (m_type != Vehicle.TYPE_NONE)
3377 { 3675 {
3378 // get body attitude 3676 // get body attitude
3379 d.Quaternion rot = d.BodyGetQuaternion(Body); 3677 d.Quaternion rot = d.BodyGetQuaternion(Body);
3380 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object 3678 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3381 Quaternion irotq = Quaternion.Inverse(rotq); 3679 Quaternion irotq = Quaternion.Inverse(rotq);
3382 3680
3383 // VEHICLE Linear Motion 3681 // VEHICLE Linear Motion
3384 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame 3682 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3385 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); 3683 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3386 m_lLinObjectVel = vel_now * irotq; 3684 m_lLinObjectVel = vel_now * irotq;
3387 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate 3685 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3388 { 3686 {
3389 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) 3687 if (Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3390 { 3688 {
3391 float decayfactor = m_linearMotorDecayTimescale/timestep; 3689 float decayfactor = m_linearMotorDecayTimescale / timestep;
3392 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); 3690 Vector3 decayAmount = (m_lLinMotorDVel / decayfactor);
3393 m_lLinMotorDVel -= decayAmount; 3691 m_lLinMotorDVel -= decayAmount;
3394 } 3692 }
3395 else 3693 else
3396 { 3694 {
3397 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); 3695 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3398 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; 3696 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3399 m_lLinMotorDVel -= decel; 3697 m_lLinMotorDVel -= decel;
3400 } 3698 }
3401 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3699 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3402 { 3700 {
3403 m_lLinMotorDVel = Vector3.Zero; 3701 m_lLinMotorDVel = Vector3.Zero;
3404 } 3702 }
3405 3703
3406 /* else 3704 /* else
3407 { 3705 {
3408 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; 3706 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3409 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; 3707 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3410 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; 3708 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3411 } */ 3709 } */
3412 } // end linear motor decay 3710 } // end linear motor decay
3413 3711
3414 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3712 if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3415 { 3713 {
3416 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3714 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3417 if (m_linearMotorTimescale < 300.0f) 3715 if (m_linearMotorTimescale < 300.0f)
3418 { 3716 {
3419 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; 3717 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3420 float linfactor = m_linearMotorTimescale/timestep; 3718 float linfactor = m_linearMotorTimescale / timestep;
3421 Vector3 attackAmount = (attack_error/linfactor) * 1.3f; 3719 Vector3 attackAmount = (attack_error / linfactor) * 1.3f;
3422 m_lLinObjectVel += attackAmount; 3720 m_lLinObjectVel += attackAmount;
3423 } 3721 }
3424 if (m_linearFrictionTimescale.X < 300.0f) 3722 if (m_linearFrictionTimescale.X < 300.0f)
3425 { 3723 {
3426 float fricfactor = m_linearFrictionTimescale.X / timestep; 3724 float fricfactor = m_linearFrictionTimescale.X / timestep;
3427 float fricX = m_lLinObjectVel.X / fricfactor; 3725 float fricX = m_lLinObjectVel.X / fricfactor;
3428 m_lLinObjectVel.X -= fricX; 3726 m_lLinObjectVel.X -= fricX;
3429 } 3727 }
3430 if (m_linearFrictionTimescale.Y < 300.0f) 3728 if (m_linearFrictionTimescale.Y < 300.0f)
3431 { 3729 {
3432 float fricfactor = m_linearFrictionTimescale.Y / timestep; 3730 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3433 float fricY = m_lLinObjectVel.Y / fricfactor; 3731 float fricY = m_lLinObjectVel.Y / fricfactor;
3434 m_lLinObjectVel.Y -= fricY; 3732 m_lLinObjectVel.Y -= fricY;
3435 } 3733 }
3436 if (m_linearFrictionTimescale.Z < 300.0f) 3734 if (m_linearFrictionTimescale.Z < 300.0f)
3437 { 3735 {
3438 float fricfactor = m_linearFrictionTimescale.Z / timestep; 3736 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3439 float fricZ = m_lLinObjectVel.Z / fricfactor; 3737 float fricZ = m_lLinObjectVel.Z / fricfactor;
3440 m_lLinObjectVel.Z -= fricZ; 3738 m_lLinObjectVel.Z -= fricZ;
3441 } 3739 }
3442 } 3740 }
3443 m_wLinObjectVel = m_lLinObjectVel * rotq; 3741 m_wLinObjectVel = m_lLinObjectVel * rotq;
3444 3742
3445 // Gravity and Buoyancy 3743 // Gravity and Buoyancy
3446 Vector3 grav = Vector3.Zero; 3744 Vector3 grav = Vector3.Zero;
3447 if(m_VehicleBuoyancy < 1.0f) 3745 if (m_VehicleBuoyancy < 1.0f)
3448 { 3746 {
3449 // There is some gravity, make a gravity force vector 3747 // There is some gravity, make a gravity force vector
3450 // that is applied after object velocity. 3748 // that is applied after object velocity.
3451 d.Mass objMass; 3749 d.Mass objMass;
3452 d.BodyGetMass(Body, out objMass); 3750 d.BodyGetMass(Body, out objMass);
3453 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; 3751 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3454 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force 3752 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3455 } // else its 1.0, no gravity. 3753 } // else its 1.0, no gravity.
3456 3754
3457 // Hovering 3755 // Hovering
3458 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) 3756 if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3459 { 3757 {
3460 // We should hover, get the target height 3758 // We should hover, get the target height
3461 d.Vector3 pos = d.BodyGetPosition(Body); 3759 d.Vector3 pos = d.BodyGetPosition(Body);
3462 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) 3760 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3463 { 3761 {
3464 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; 3762 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3465 } 3763 }
3466 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) 3764 else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3467 { 3765 {
3468 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; 3766 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3469 } 3767 }
3470 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) 3768 else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3471 { 3769 {
3472 m_VhoverTargetHeight = m_VhoverHeight; 3770 m_VhoverTargetHeight = m_VhoverHeight;
3473 } 3771 }
3474 3772
3475 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) 3773 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3476 { 3774 {
3477 // If body is aready heigher, use its height as target height 3775 // If body is aready heigher, use its height as target height
3478 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; 3776 if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3479 } 3777 }
3480 3778
3481// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped 3779 // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3482// m_VhoverTimescale = 0f; // time to acheive height 3780 // m_VhoverTimescale = 0f; // time to acheive height
3483// timestep is time since last frame,in secs 3781 // timestep is time since last frame,in secs
3484 float herr0 = pos.Z - m_VhoverTargetHeight; 3782 float herr0 = pos.Z - m_VhoverTargetHeight;
3485 // Replace Vertical speed with correction figure if significant 3783 // Replace Vertical speed with correction figure if significant
3486 if(Math.Abs(herr0) > 0.01f ) 3784 if (Math.Abs(herr0) > 0.01f)
3487 { 3785 {
3488 //? d.Mass objMass; 3786 //? d.Mass objMass;
3489 //? d.BodyGetMass(Body, out objMass); 3787 //? d.BodyGetMass(Body, out objMass);
3490 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); 3788 m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale);
3491 //KF: m_VhoverEfficiency is not yet implemented 3789 //KF: m_VhoverEfficiency is not yet implemented
3492 } 3790 }
3493 else 3791 else
3494 { 3792 {
3495 m_wLinObjectVel.Z = 0f; 3793 m_wLinObjectVel.Z = 0f;
3496 } 3794 }
3497 } 3795 }
3498 else 3796 else
3499 { // not hovering 3797 { // not hovering
3500 if (m_wLinObjectVel.Z == 0f) 3798 if (m_wLinObjectVel.Z == 0f)
3501 { // Gravity rules 3799 { // Gravity rules
3502 m_wLinObjectVel.Z = vel_now.Z; 3800 m_wLinObjectVel.Z = vel_now.Z;
3503 } // else the motor has it 3801 } // else the motor has it
3504 } 3802 }
3505 linvel = m_wLinObjectVel; 3803 linvel = m_wLinObjectVel;
3506 3804
3507 // Vehicle Linear Motion done ======================================= 3805 // Vehicle Linear Motion done =======================================
3508 // Apply velocity 3806 // Apply velocity
3509 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); 3807 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3510 // apply gravity force 3808 // apply gravity force
3511 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3809 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3512//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); 3810 //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3513 // end MoveLinear() 3811 // end MoveLinear()
3514 3812
3515 3813
3516 // MoveAngular 3814 // MoveAngular
3517 /* 3815 /*
3518 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor 3816 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3519
3520 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3521 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3522 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3523
3524 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3525 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3526 */
3527//if(frcount == 0) Console.WriteLine("MoveAngular ");
3528 3817
3529 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); 3818 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3530 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); 3819 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3531 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation 3820 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3532 3821
3533//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); 3822 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3534 3823 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3535 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. 3824 */
3536 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); 3825 //if(frcount == 0) Console.WriteLine("MoveAngular ");
3537 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; 3826
3538 // Decay Angular Motor 2. 3827 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3539 if (m_angularMotorDecayTimescale < 300.0f) 3828 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3540 { 3829 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3541 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) 3830
3542 { 3831 //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3543 float decayfactor = (m_angularMotorDecayTimescale)/timestep; 3832
3544 Vector3 decayAmount = (m_angularMotorDVel/decayfactor); 3833 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3545 m_angularMotorDVel -= decayAmount; 3834 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3546 } 3835 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3547 else 3836 // Decay Angular Motor 2.
3548 { 3837 if (m_angularMotorDecayTimescale < 300.0f)
3549 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; 3838 {
3550 m_angularMotorDVel -= decel; 3839 if (Vector3.Mag(m_angularMotorDVel) < 1.0f)
3551 } 3840 {
3552 3841 float decayfactor = (m_angularMotorDecayTimescale) / timestep;
3553 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) 3842 Vector3 decayAmount = (m_angularMotorDVel / decayfactor);
3554 { 3843 m_angularMotorDVel -= decayAmount;
3555 m_angularMotorDVel = Vector3.Zero; 3844 }
3556 } 3845 else
3557 else 3846 {
3558 { 3847 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3559 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; 3848 m_angularMotorDVel -= decel;
3560 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; 3849 }
3561 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; 3850
3562 } 3851 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3563 } // end decay angular motor 3852 {
3564//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3853 m_angularMotorDVel = Vector3.Zero;
3565 3854 }
3566//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); 3855 else
3567 3856 {
3568 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) 3857 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3569 { // if motor or object have motion 3858 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3570 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); 3859 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3571 3860 }
3572 if (m_angularMotorTimescale < 300.0f) 3861 } // end decay angular motor
3573 { 3862 //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3574 Vector3 attack_error = m_angularMotorDVel - angObjectVel; 3863
3575 float angfactor = m_angularMotorTimescale/timestep; 3864 //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3576 Vector3 attackAmount = (attack_error/angfactor); 3865
3577 angObjectVel += attackAmount; 3866 if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)))
3578//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); 3867 { // if motor or object have motion
3579//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); 3868 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body);
3580 } 3869
3581 3870 if (m_angularMotorTimescale < 300.0f)
3582 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); 3871 {
3583 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); 3872 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3584 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); 3873 float angfactor = m_angularMotorTimescale / timestep;
3585 } // else no signif. motion 3874 Vector3 attackAmount = (attack_error / angfactor);
3586 3875 angObjectVel += attackAmount;
3587//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); 3876 //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3588 // Bank section tba 3877 //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3589 // Deflection section tba 3878 }
3590//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); 3879
3591 3880 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3592 3881 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3593 /* // Rotation Axis Disables: 3882 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3594 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3883 } // else no signif. motion
3595 { 3884
3596 if (m_angularEnable.X == 0) 3885 //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3597 angObjectVel.X = 0f; 3886 // Bank section tba
3598 if (m_angularEnable.Y == 0) 3887 // Deflection section tba
3599 angObjectVel.Y = 0f; 3888 //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3600 if (m_angularEnable.Z == 0) 3889
3601 angObjectVel.Z = 0f; 3890
3602 } 3891 /* // Rotation Axis Disables:
3603 */ 3892 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3604 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation 3893 {
3605 3894 if (m_angularEnable.X == 0)
3606 // Vertical attractor section 3895 angObjectVel.X = 0f;
3607 Vector3 vertattr = Vector3.Zero; 3896 if (m_angularEnable.Y == 0)
3608 3897 angObjectVel.Y = 0f;
3609 if(m_verticalAttractionTimescale < 300) 3898 if (m_angularEnable.Z == 0)
3610 { 3899 angObjectVel.Z = 0f;
3611 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); 3900 }
3612 // make a vector pointing up 3901 */
3613 Vector3 verterr = Vector3.Zero; 3902 angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation
3614 verterr.Z = 1.0f; 3903
3615 // rotate it to Body Angle 3904 // Vertical attractor section
3616 verterr = verterr * rotq; 3905 Vector3 vertattr = Vector3.Zero;
3617 // 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. 3906
3618 // 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 3907 if (m_verticalAttractionTimescale < 300)
3619 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. 3908 {
3620 3909 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3621 if (verterr.Z < 0.0f) 3910 // make a vector pointing up
3622 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to 3911 Vector3 verterr = Vector3.Zero;
3623 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. 3912 verterr.Z = 1.0f;
3624//Console.WriteLine("InvertFlip"); 3913 // rotate it to Body Angle
3625 verterr.X = 2.0f - verterr.X; 3914 verterr = verterr * rotq;
3626 verterr.Y = 2.0f - verterr.Y; 3915 // 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.
3627 } 3916 // 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
3628 verterr *= 0.5f; 3917 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3629 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) 3918
3630 Vector3 xyav = angObjectVel; 3919 if (verterr.Z < 0.0f)
3631 xyav.Z = 0.0f; 3920 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3632 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) 3921 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3633 { 3922 //Console.WriteLine("InvertFlip");
3634 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 3923 verterr.X = 2.0f - verterr.X;
3635 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. 3924 verterr.Y = 2.0f - verterr.Y;
3636 vertattr.X = verterr.Y; 3925 }
3637 vertattr.Y = - verterr.X; 3926 verterr *= 0.5f;
3638 vertattr.Z = 0f; 3927 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3639//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); 3928 Vector3 xyav = angObjectVel;
3640 3929 xyav.Z = 0.0f;
3641 // scaling appears better usingsquare-law 3930 if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3642 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; 3931 {
3643 float bounce = 1.0f - damped; 3932 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3644 // 0 = crit damp, 1 = bouncy 3933 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3645 float oavz = angObjectVel.Z; // retain z velocity 3934 vertattr.X = verterr.Y;
3646 // time-scaled correction, which sums, therefore is bouncy: 3935 vertattr.Y = -verterr.X;
3647 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; 3936 vertattr.Z = 0f;
3648 // damped, good @ < 90: 3937 //if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3649 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); 3938
3650 angObjectVel.Z = oavz; 3939 // scaling appears better usingsquare-law
3651//if(frcount == 0) Console.WriteLine("VA+"); 3940 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3652//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); 3941 float bounce = 1.0f - damped;
3653 } 3942 // 0 = crit damp, 1 = bouncy
3654 else 3943 float oavz = angObjectVel.Z; // retain z velocity
3655 { 3944 // time-scaled correction, which sums, therefore is bouncy:
3656 // else error is very small 3945 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3657 angObjectVel.X = 0f; 3946 // damped, good @ < 90:
3658 angObjectVel.Y = 0f; 3947 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3659//if(frcount == 0) Console.WriteLine("VA0"); 3948 angObjectVel.Z = oavz;
3660 } 3949 //if(frcount == 0) Console.WriteLine("VA+");
3661 } // else vertical attractor is off 3950 //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3662//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); 3951 }
3663 3952 else
3664 3953 {
3665 m_lastAngularVelocity = angObjectVel; 3954 // else error is very small
3666 // apply Angular Velocity to body 3955 angObjectVel.X = 0f;
3667 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 3956 angObjectVel.Y = 0f;
3668//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); 3957 //if(frcount == 0) Console.WriteLine("VA0");
3669 3958 }
3670 } // end VEHICLES 3959 } // else vertical attractor is off
3671 else 3960 //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3672 { 3961
3673 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ 3962
3674 3963 m_lastAngularVelocity = angObjectVel;
3675 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 3964 // apply Angular Velocity to body
3676 3965 d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3677 /// Dynamics Buoyancy 3966 //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3678 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. 3967
3679 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 3968 } // end VEHICLES
3680 // NB Prims in ODE are no subject to global gravity 3969 else
3681 // This should only affect gravity operations 3970 {
3682 3971 // Dyamics (NON-'VEHICLES') are dealt with here ================================================================
3683 float m_mass = CalculateMass(); 3972
3684 // calculate z-force due togravity on object. 3973 if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3685 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass 3974
3686 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. 3975 /// Dynamics Buoyancy
3687 { 3976 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3688 fz = 0; // llMoveToTarget ignores gravity. 3977 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3689 // it also ignores mass of object, and any physical resting on it. 3978 // NB Prims in ODE are no subject to global gravity
3690 // Vector3 m_PIDTarget is where we are going 3979 // This should only affect gravity operations
3691 // float m_PIDTau is time to get there 3980
3981 float m_mass = CalculateMass();
3982 // calculate z-force due togravity on object.
3983 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3984 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3985 {
3986 fz = 0; // llMoveToTarget ignores gravity.
3987 // it also ignores mass of object, and any physical resting on it.
3988 // Vector3 m_PIDTarget is where we are going
3989 // float m_PIDTau is time to get there
3692 fx = 0; 3990 fx = 0;
3693 fy = 0; 3991 fy = 0;
3694 d.Vector3 pos = d.BodyGetPosition(Body); 3992 d.Vector3 pos = d.BodyGetPosition(Body);
3695 Vector3 error = new Vector3( 3993 Vector3 error = new Vector3(
3696 (m_PIDTarget.X - pos.X), 3994 (m_PIDTarget.X - pos.X),
3697 (m_PIDTarget.Y - pos.Y), 3995 (m_PIDTarget.Y - pos.Y),
3698 (m_PIDTarget.Z - pos.Z)); 3996 (m_PIDTarget.Z - pos.Z));
3699 if (error.ApproxEquals(Vector3.Zero,0.01f)) 3997 if (error.ApproxEquals(Vector3.Zero, 0.01f))
3700 { // Very close, Jump there and quit move 3998 { // Very close, Jump there and quit move
3701 3999
3702 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 4000 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3703 _target_velocity = Vector3.Zero; 4001 _target_velocity = Vector3.Zero;
3704 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); 4002 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3705 d.BodySetForce(Body, 0f, 0f, 0f); 4003 d.BodySetForce(Body, 0f, 0f, 0f);
3706 }
3707 else
3708 {
3709 float scale = 50.0f * timestep / m_PIDTau;
3710 if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero))
3711 {
3712 // Nearby, quit update of velocity
3713 }
3714 else
3715 { // Far, calc damped velocity
3716 _target_velocity = error * scale;
3717 }
3718 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3719 } 4004 }
3720 } // end PID MoveToTarget 4005 else
3721 4006 {
3722 4007 float scale = 50.0f * timestep / m_PIDTau;
3723 /// Dynamics Hover =================================================================================== 4008 if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero))
3724 // Hover PID Controller can only run if the PIDcontroller is not in use. 4009 {
3725 if (m_useHoverPID && !m_usePID) 4010 // Nearby, quit update of velocity
3726 { 4011 }
3727//Console.WriteLine("Hover " + m_primName); 4012 else
3728 4013 { // Far, calc damped velocity
3729 // If we're using the PID controller, then we have no gravity 4014 _target_velocity = error * scale;
3730 fz = (-1 * _parent_scene.gravityz) * m_mass; 4015 }
3731 4016 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
3732 // no lock; for now it's only called from within Simulate() 4017 }
3733 4018 } // end PID MoveToTarget
3734 // If the PID Controller isn't active then we set our force 4019
3735 // calculating base velocity to the current position 4020
3736 4021 /// Dynamics Hover ===================================================================================
3737 if ((m_PIDTau < 1)) 4022 // Hover PID Controller can only run if the PIDcontroller is not in use.
3738 { 4023 if (m_useHoverPID && !m_usePID)
3739 PID_G = PID_G / m_PIDTau; 4024 {
3740 } 4025 //Console.WriteLine("Hover " + m_primName);
3741 4026
3742 if ((PID_G - m_PIDTau) <= 0) 4027 // If we're using the PID controller, then we have no gravity
3743 { 4028 fz = (-1 * _parent_scene.gravityz) * m_mass;
3744 PID_G = m_PIDTau + 1; 4029
3745 } 4030 // no lock; for now it's only called from within Simulate()
3746 4031
3747 4032 // If the PID Controller isn't active then we set our force
3748 // Where are we, and where are we headed? 4033 // calculating base velocity to the current position
3749 d.Vector3 pos = d.BodyGetPosition(Body); 4034
3750// d.Vector3 vel = d.BodyGetLinearVel(Body); 4035 if ((m_PIDTau < 1))
3751 4036 {
3752 4037 PID_G = PID_G / m_PIDTau;
3753 // Non-Vehicles have a limited set of Hover options. 4038 }
3754 // determine what our target height really is based on HoverType 4039
3755 switch (m_PIDHoverType) 4040 if ((PID_G - m_PIDTau) <= 0)
3756 { 4041 {
3757 case PIDHoverType.Ground: 4042 PID_G = m_PIDTau + 1;
3758 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4043 }
3759 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4044
3760 break; 4045
3761 case PIDHoverType.GroundAndWater: 4046 // Where are we, and where are we headed?
3762 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 4047 d.Vector3 pos = d.BodyGetPosition(Body);
3763 m_waterHeight = _parent_scene.GetWaterLevel(); 4048 // d.Vector3 vel = d.BodyGetLinearVel(Body);
3764 if (m_groundHeight > m_waterHeight) 4049
3765 { 4050
3766 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 4051 // Non-Vehicles have a limited set of Hover options.
3767 } 4052 // determine what our target height really is based on HoverType
3768 else 4053 switch (m_PIDHoverType)
3769 { 4054 {
3770 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 4055 case PIDHoverType.Ground:
3771 } 4056 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3772 break; 4057 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3773 4058 break;
3774 } // end switch (m_PIDHoverType) 4059 case PIDHoverType.GroundAndWater:
3775 4060 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3776 4061 m_waterHeight = _parent_scene.GetWaterLevel();
3777 _target_velocity = 4062 if (m_groundHeight > m_waterHeight)
4063 {
4064 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
4065 }
4066 else
4067 {
4068 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
4069 }
4070 break;
4071
4072 } // end switch (m_PIDHoverType)
4073
4074
4075 _target_velocity =
3778 new Vector3(0.0f, 0.0f, 4076 new Vector3(0.0f, 0.0f,
3779 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 4077 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3780 ); 4078 );
3781 4079
3782 // if velocity is zero, use position control; otherwise, velocity control 4080 // if velocity is zero, use position control; otherwise, velocity control
3783 4081
3784 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 4082 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3785 { 4083 {
3786 // keep track of where we stopped. No more slippin' & slidin' 4084 // keep track of where we stopped. No more slippin' & slidin'
3787 4085
3788 // We only want to deactivate the PID Controller if we think we want to have our surrogate 4086 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3789 // react to the physics scene by moving it's position. 4087 // react to the physics scene by moving it's position.
3790 // Avatar to Avatar collisions 4088 // Avatar to Avatar collisions
3791 // Prim to avatar collisions 4089 // Prim to avatar collisions
3792 d.Vector3 dlinvel = vel; 4090 d.Vector3 dlinvel = vel;
3793 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 4091 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3794 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); 4092 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3795 d.BodyAddForce(Body, 0, 0, fz); 4093 d.BodyAddForce(Body, 0, 0, fz);
3796 //KF this prevents furthur motions return; 4094 //KF this prevents furthur motions return;
3797 } 4095 }
3798 else 4096 else
3799 { 4097 {
3800 _zeroFlag = false; 4098 _zeroFlag = false;
3801 4099
3802 // We're flying and colliding with something 4100 // We're flying and colliding with something
3803 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 4101 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3804 } 4102 }
3805 } // end m_useHoverPID && !m_usePID 4103 } // end m_useHoverPID && !m_usePID
3806 4104
3807 4105
3808 /// Dynamics Apply Forces =================================================================================== 4106 /// Dynamics Apply Forces ===================================================================================
3809 fx *= m_mass; 4107 fx *= m_mass;
3810 fy *= m_mass; 4108 fy *= m_mass;
3811 //fz *= m_mass; 4109 //fz *= m_mass;
3812 fx += m_force.X; 4110 fx += m_force.X;
3813 fy += m_force.Y; 4111 fy += m_force.Y;
3814 fz += m_force.Z; 4112 fz += m_force.Z;
3815 4113
3816 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 4114 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3817 if (fx != 0 || fy != 0 || fz != 0) 4115 if (fx != 0 || fy != 0 || fz != 0)
3818 { 4116 {
3819 //m_taintdisable = true; 4117 //m_taintdisable = true;
3820 //base.RaiseOutOfBounds(Position); 4118 //base.RaiseOutOfBounds(Position);
3821 //d.BodySetLinearVel(Body, fx, fy, 0f); 4119 //d.BodySetLinearVel(Body, fx, fy, 0f);
3822 if (!d.BodyIsEnabled(Body)) 4120 if (!d.BodyIsEnabled(Body))
3823 { 4121 {
3824 // A physical body at rest on a surface will auto-disable after a while, 4122 // A physical body at rest on a surface will auto-disable after a while,
3825 // this appears to re-enable it incase the surface it is upon vanishes, 4123 // this appears to re-enable it incase the surface it is upon vanishes,
3826 // and the body should fall again. 4124 // and the body should fall again.
3827 d.BodySetLinearVel(Body, 0f, 0f, 0f); 4125 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3828 d.BodySetForce(Body, 0f, 0f, 0f); 4126 d.BodySetForce(Body, 0f, 0f, 0f);
3829 enableBodySoft(); 4127 enableBodySoft();
3830 } 4128 }
3831 4129
3832 // 35x10 = 350n times the mass per second applied maximum. 4130 // 35x10 = 350n times the mass per second applied maximum.
3833 float nmax = 35f * m_mass; 4131 float nmax = 35f * m_mass;
3834 float nmin = -35f * m_mass; 4132 float nmin = -35f * m_mass;
3835 4133
3836 4134
3837 if (fx > nmax) 4135 if (fx > nmax)
3838 fx = nmax; 4136 fx = nmax;
3839 if (fx < nmin) 4137 if (fx < nmin)
3840 fx = nmin; 4138 fx = nmin;
3841 if (fy > nmax) 4139 if (fy > nmax)
3842 fy = nmax; 4140 fy = nmax;
3843 if (fy < nmin) 4141 if (fy < nmin)
3844 fy = nmin; 4142 fy = nmin;
3845 d.BodyAddForce(Body, fx, fy, fz); 4143 d.BodyAddForce(Body, fx, fy, fz);
3846 } // end apply forces 4144 } // end apply forces
3847 } // end Vehicle/Dynamics 4145 } // end Vehicle/Dynamics
3848 4146
3849 /// RotLookAt / LookAt ================================================================================= 4147 /// RotLookAt / LookAt =================================================================================
3850 if (m_useAPID) 4148 if (m_useAPID)
3851 { 4149 {
3852 // RotLookAt, apparently overrides all other rotation sources. Inputs: 4150 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3853 // Quaternion m_APIDTarget 4151 // Quaternion m_APIDTarget
3854 // float m_APIDStrength // From SL experiments, this is the time to get there 4152 // float m_APIDStrength // From SL experiments, this is the time to get there
3855 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly 4153 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3856 // Also in SL the mass of the object has no effect on time to get there. 4154 // Also in SL the mass of the object has no effect on time to get there.
3857 // Factors: 4155 // Factors:
3858 // get present body rotation 4156 // get present body rotation
3859 float limit = 1.0f; 4157 float limit = 1.0f;
3860 float rscaler = 50f; // adjusts rotation damping time 4158 float rscaler = 50f; // adjusts rotation damping time
3861 float lscaler = 10f; // adjusts linear damping time in llLookAt 4159 float lscaler = 10f; // adjusts linear damping time in llLookAt
3862 float RLAservo = 0f; 4160 float RLAservo = 0f;
3863 Vector3 diff_axis; 4161 Vector3 diff_axis;
3864 float diff_angle; 4162 float diff_angle;
3865 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation 4163 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3866 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 4164 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3867 Quaternion rtarget = new Quaternion(); 4165 Quaternion rtarget = new Quaternion();
3868 4166
3869 if(m_APIDTarget.W == -99.9f) 4167 if (m_APIDTarget.W == -99.9f)
3870 { 4168 {
3871 // this is really a llLookAt(), x,y,z is the target vector 4169 // this is really a llLookAt(), x,y,z is the target vector
3872 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); 4170 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
3873 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; 4171 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3874 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); 4172 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3875 float twopi = 2.0f * (float)Math.PI; 4173 float twopi = 2.0f * (float)Math.PI;
3876 Vector3 dir = target - _position; 4174 Vector3 dir = target - _position;
3877 dir.Normalize(); 4175 dir.Normalize();
3878 float tzrot = (float)Math.Atan2(dir.Y, dir.X); 4176 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3879 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); 4177 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3880 float terot = (float)Math.Atan2(dir.Z, txy); 4178 float terot = (float)Math.Atan2(dir.Z, txy);
@@ -3882,63 +4180,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!");
3882 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); 4180 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3883 float oerot = (float)Math.Atan2(ospin.Z, oxy); 4181 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3884 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); 4182 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
3885 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); 4183 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X));
3886 float roll = (float)Math.Atan2(ra, rb); 4184 float roll = (float)Math.Atan2(ra, rb);
3887 float errorz = tzrot - ozrot; 4185 float errorz = tzrot - ozrot;
3888 if(errorz > (float)Math.PI) errorz -= twopi; 4186 if (errorz > (float)Math.PI) errorz -= twopi;
3889 else if(errorz < -(float)Math.PI) errorz += twopi; 4187 else if (errorz < -(float)Math.PI) errorz += twopi;
3890 float errory = oerot - terot; 4188 float errory = oerot - terot;
3891 if(errory > (float)Math.PI) errory -= twopi; 4189 if (errory > (float)Math.PI) errory -= twopi;
3892 else if(errory < -(float)Math.PI) errory += twopi; 4190 else if (errory < -(float)Math.PI) errory += twopi;
3893 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); 4191 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3894 if(diff_angle > 0.01f * m_APIDdamper) 4192 if (diff_angle > 0.01f * m_APIDdamper)
3895 { 4193 {
3896 m_APIDdamper = 1.0f; 4194 m_APIDdamper = 1.0f;
3897 RLAservo = timestep / m_APIDStrength * rscaler; 4195 RLAservo = timestep / m_APIDStrength * rscaler;
3898 errorz *= RLAservo; 4196 errorz *= RLAservo;
3899 errory *= RLAservo; 4197 errory *= RLAservo;
3900 error.X = -roll * 8.0f; 4198 error.X = -roll * 8.0f;
3901 error.Y = errory; 4199 error.Y = errory;
3902 error.Z = errorz; 4200 error.Z = errorz;
3903 error *= rotq; 4201 error *= rotq;
3904 d.BodySetAngularVel (Body, error.X, error.Y, error.Z); 4202 d.BodySetAngularVel(Body, error.X, error.Y, error.Z);
3905 } 4203 }
3906 else 4204 else
3907 { 4205 {
3908 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4206 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3909 m_APIDdamper = 2.0f; 4207 m_APIDdamper = 2.0f;
3910 } 4208 }
3911 } 4209 }
3912 else 4210 else
3913 { 4211 {
3914 // this is a llRotLookAt() 4212 // this is a llRotLookAt()
3915 rtarget = m_APIDTarget; 4213 rtarget = m_APIDTarget;
3916 4214
3917 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot 4215 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
3918 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle 4216 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
3919//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); 4217 //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3920 4218
3921 // diff_axis.Normalize(); it already is! 4219 // diff_axis.Normalize(); it already is!
3922 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error 4220 if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3923 { 4221 {
3924 m_APIDdamper = 1.0f; 4222 m_APIDdamper = 1.0f;
3925 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); 4223 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3926 rotforce = rotforce * rotq; 4224 rotforce = rotforce * rotq;
3927 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate 4225 if (diff_angle > limit) diff_angle = limit; // cap the rotate rate
3928 RLAservo = timestep / m_APIDStrength * lscaler; 4226 RLAservo = timestep / m_APIDStrength * lscaler;
3929 rotforce = rotforce * RLAservo * diff_angle ; 4227 rotforce = rotforce * RLAservo * diff_angle;
3930 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); 4228 d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z);
3931//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); 4229 //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3932 } 4230 }
3933 else 4231 else
3934 { // close enough 4232 { // close enough
3935 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); 4233 d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f);
3936 m_APIDdamper = 2.0f; 4234 m_APIDdamper = 2.0f;
3937 } 4235 }
3938 } // end llLookAt/llRotLookAt 4236 } // end llLookAt/llRotLookAt
3939//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); 4237 //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3940 } // end m_useAPID 4238 } // end m_useAPID
3941 } // end root prims 4239 } // end root prims
3942 } // end Move() 4240 } // end Move()
3943 } // end class 4241 } // end class
3944} 4242} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
index cf7c1d7..03059f7 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin
1736 return newPrim; 1736 return newPrim;
1737 } 1737 }
1738 1738
1739 private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent,
1740 PrimitiveBaseShape pbs, uint localid, byte[] sdata)
1741 {
1742 Vector3 pos = position;
1743
1744 OdePrim newPrim;
1745 lock (OdeLock)
1746 {
1747 newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata);
1748 lock (_prims)
1749 _prims.Add(newPrim);
1750 }
1751
1752 return newPrim;
1753 }
1754
1755
1739 public void addActivePrim(OdePrim activatePrim) 1756 public void addActivePrim(OdePrim activatePrim)
1740 { 1757 {
1741 // adds active prim.. (ones that should be iterated over in collisions_optimized 1758 // adds active prim.. (ones that should be iterated over in collisions_optimized
@@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin
1762 return result; 1779 return result;
1763 } 1780 }
1764 1781
1782 public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
1783 uint localid, byte[] sdata)
1784 {
1785 PhysicsActor result;
1786
1787 result = AddPrim(primName, position, parent,
1788 pbs, localid, sdata);
1789
1790 return result;
1791 }
1792
1765 public override float TimeDilation 1793 public override float TimeDilation
1766 { 1794 {
1767 get { return m_timeDilation; } 1795 get { return m_timeDilation; }
diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
new file mode 100644
index 0000000..edd58d3
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs
@@ -0,0 +1,167 @@
1// adapted from libomv removing cpu endian adjust
2// for prims lowlevel serialization
3
4using System;
5using System.IO;
6using OpenMetaverse;
7
8namespace OpenSim.Region.Physics.OdePlugin
9{
10 public class wstreamer
11 {
12 private MemoryStream st;
13
14 public wstreamer()
15 {
16 st = new MemoryStream();
17 }
18
19 public byte[] close()
20 {
21 byte[] data = st.ToArray();
22 st.Close();
23 return data;
24 }
25
26 public void Wshort(short value)
27 {
28 st.Write(BitConverter.GetBytes(value), 0, 2);
29 }
30 public void Wushort(ushort value)
31 {
32 byte[] t = BitConverter.GetBytes(value);
33 st.Write(BitConverter.GetBytes(value), 0, 2);
34 }
35 public void Wint(int value)
36 {
37 st.Write(BitConverter.GetBytes(value), 0, 4);
38 }
39 public void Wuint(uint value)
40 {
41 st.Write(BitConverter.GetBytes(value), 0, 4);
42 }
43 public void Wlong(long value)
44 {
45 st.Write(BitConverter.GetBytes(value), 0, 8);
46 }
47 public void Wulong(ulong value)
48 {
49 st.Write(BitConverter.GetBytes(value), 0, 8);
50 }
51
52 public void Wfloat(float value)
53 {
54 st.Write(BitConverter.GetBytes(value), 0, 4);
55 }
56
57 public void Wdouble(double value)
58 {
59 st.Write(BitConverter.GetBytes(value), 0, 8);
60 }
61
62 public void Wvector3(Vector3 value)
63 {
64 st.Write(BitConverter.GetBytes(value.X), 0, 4);
65 st.Write(BitConverter.GetBytes(value.Y), 0, 4);
66 st.Write(BitConverter.GetBytes(value.Z), 0, 4);
67 }
68 public void Wquat(Quaternion value)
69 {
70 st.Write(BitConverter.GetBytes(value.X), 0, 4);
71 st.Write(BitConverter.GetBytes(value.Y), 0, 4);
72 st.Write(BitConverter.GetBytes(value.Z), 0, 4);
73 st.Write(BitConverter.GetBytes(value.W), 0, 4);
74 }
75 }
76
77 public class rstreamer
78 {
79 private byte[] rbuf;
80 private int ptr;
81
82 public rstreamer(byte[] data)
83 {
84 rbuf = data;
85 ptr = 0;
86 }
87
88 public void close()
89 {
90 }
91
92 public short Rshort()
93 {
94 short v = BitConverter.ToInt16(rbuf, ptr);
95 ptr += 2;
96 return v;
97 }
98 public ushort Rushort()
99 {
100 ushort v = BitConverter.ToUInt16(rbuf, ptr);
101 ptr += 2;
102 return v;
103 }
104 public int Rint()
105 {
106 int v = BitConverter.ToInt32(rbuf, ptr);
107 ptr += 4;
108 return v;
109 }
110 public uint Ruint()
111 {
112 uint v = BitConverter.ToUInt32(rbuf, ptr);
113 ptr += 4;
114 return v;
115 }
116 public long Rlong()
117 {
118 long v = BitConverter.ToInt64(rbuf, ptr);
119 ptr += 8;
120 return v;
121 }
122 public ulong Rulong()
123 {
124 ulong v = BitConverter.ToUInt64(rbuf, ptr);
125 ptr += 8;
126 return v;
127 }
128 public float Rfloat()
129 {
130 float v = BitConverter.ToSingle(rbuf, ptr);
131 ptr += 4;
132 return v;
133 }
134
135 public double Rdouble()
136 {
137 double v = BitConverter.ToDouble(rbuf, ptr);
138 ptr += 8;
139 return v;
140 }
141
142 public Vector3 Rvector3()
143 {
144 Vector3 v;
145 v.X = BitConverter.ToSingle(rbuf, ptr);
146 ptr += 4;
147 v.Y = BitConverter.ToSingle(rbuf, ptr);
148 ptr += 4;
149 v.Z = BitConverter.ToSingle(rbuf, ptr);
150 ptr += 4;
151 return v;
152 }
153 public Quaternion Rquat()
154 {
155 Quaternion v;
156 v.X = BitConverter.ToSingle(rbuf, ptr);
157 ptr += 4;
158 v.Y = BitConverter.ToSingle(rbuf, ptr);
159 ptr += 4;
160 v.Z = BitConverter.ToSingle(rbuf, ptr);
161 ptr += 4;
162 v.W = BitConverter.ToSingle(rbuf, ptr);
163 ptr += 4;
164 return v;
165 }
166 }
167}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index e1a68be..eb0228a 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -214,6 +214,11 @@ namespace OpenSim.Region.Physics.Manager
214 } 214 }
215 } 215 }
216 216
217 public virtual byte[] Serialize(bool PhysIsRunning)
218 {
219 return new byte[0];
220 }
221
217 public virtual void RaiseOutOfBounds(Vector3 pos) 222 public virtual void RaiseOutOfBounds(Vector3 pos)
218 { 223 {
219 // Make a temporary copy of the event to avoid possibility of 224 // Make a temporary copy of the event to avoid possibility of
@@ -573,5 +578,6 @@ namespace OpenSim.Region.Physics.Manager
573 { 578 {
574 return false; 579 return false;
575 } 580 }
581
576 } 582 }
577} 583}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
index 3db71e5..0346d4e 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs
@@ -128,6 +128,12 @@ namespace OpenSim.Region.Physics.Manager
128 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 128 public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
129 Vector3 size, Quaternion rotation, bool isPhysical, uint localid); 129 Vector3 size, Quaternion rotation, bool isPhysical, uint localid);
130 130
131 public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
132 uint localid, byte[] sdata)
133 {
134 return null;
135 }
136
131 public virtual float TimeDilation 137 public virtual float TimeDilation
132 { 138 {
133 get { return 1.0f; } 139 get { return 1.0f; }
generated by cgit v1.1 at 2024-12-29 15:33:44 +0000