aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs715
1 files changed, 360 insertions, 355 deletions
diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
index e080b18..4e18522 100644
--- a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
+++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs
@@ -44,11 +44,8 @@
44using System; 44using System;
45using System.Collections.Generic; 45using System.Collections.Generic;
46using System.Reflection; 46using System.Reflection;
47using System.Runtime.InteropServices;
48using System.Threading;
49using log4net; 47using log4net;
50using OpenMetaverse; 48using OpenMetaverse;
51using OdeAPI;
52using OpenSim.Framework; 49using OpenSim.Framework;
53using OpenSim.Region.PhysicsModules.SharedBase; 50using OpenSim.Region.PhysicsModules.SharedBase;
54 51
@@ -182,7 +179,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
182 private float m_physCost; 179 private float m_physCost;
183 private float m_streamCost; 180 private float m_streamCost;
184 181
185 public d.Mass primdMass; // prim inertia information on it's own referencial 182 internal SafeNativeMethods.Mass primdMass; // prim inertia information on it's own referencial
186 private PhysicsInertiaData m_InertiaOverride; 183 private PhysicsInertiaData m_InertiaOverride;
187 float primMass; // prim own mass 184 float primMass; // prim own mass
188 float primVolume; // prim own volume; 185 float primVolume; // prim own volume;
@@ -489,7 +486,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
489 // double buffering 486 // double buffering
490 if(m_fakeInertiaOverride != null) 487 if(m_fakeInertiaOverride != null)
491 { 488 {
492 d.Mass objdmass = new d.Mass(); 489 SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass();
493 objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X; 490 objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X;
494 objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y; 491 objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y;
495 objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z; 492 objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z;
@@ -498,15 +495,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
498 495
499 if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999) 496 if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999)
500 { 497 {
501 d.Matrix3 inertiarotmat = new d.Matrix3(); 498 SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3();
502 d.Quaternion inertiarot = new d.Quaternion(); 499 SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion();
503 500
504 inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X; 501 inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X;
505 inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y; 502 inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y;
506 inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z; 503 inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z;
507 inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W; 504 inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W;
508 d.RfromQ(out inertiarotmat, ref inertiarot); 505 SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot);
509 d.MassRotate(ref objdmass, ref inertiarotmat); 506 SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat);
510 } 507 }
511 508
512 inertia.TotalMass = m_fakeInertiaOverride.TotalMass; 509 inertia.TotalMass = m_fakeInertiaOverride.TotalMass;
@@ -530,13 +527,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
530 return inertia; 527 return inertia;
531 } 528 }
532 529
533 d.Vector3 dtmp; 530 SafeNativeMethods.Vector3 dtmp;
534 d.Mass m = new d.Mass(); 531 SafeNativeMethods.Mass m = new SafeNativeMethods.Mass();
535 lock(_parent_scene.OdeLock) 532 lock(_parent_scene.OdeLock)
536 { 533 {
537 d.AllocateODEDataForThread(0); 534 SafeNativeMethods.AllocateODEDataForThread(0);
538 dtmp = d.GeomGetOffsetPosition(prim_geom); 535 dtmp = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
539 d.BodyGetMass(Body, out m); 536 SafeNativeMethods.BodyGetMass(Body, out m);
540 } 537 }
541 538
542 Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z); 539 Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z);
@@ -572,18 +569,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde
572 { 569 {
573 lock (_parent_scene.OdeLock) 570 lock (_parent_scene.OdeLock)
574 { 571 {
575 d.AllocateODEDataForThread(0); 572 SafeNativeMethods.AllocateODEDataForThread(0);
576 573
577 d.Vector3 dtmp; 574 SafeNativeMethods.Vector3 dtmp;
578 if (!childPrim && Body != IntPtr.Zero) 575 if (!childPrim && Body != IntPtr.Zero)
579 { 576 {
580 dtmp = d.BodyGetPosition(Body); 577 dtmp = SafeNativeMethods.BodyGetPosition(Body);
581 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); 578 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
582 } 579 }
583 else if (prim_geom != IntPtr.Zero) 580 else if (prim_geom != IntPtr.Zero)
584 { 581 {
585 d.Quaternion dq; 582 SafeNativeMethods.Quaternion dq;
586 d.GeomCopyQuaternion(prim_geom, out dq); 583 SafeNativeMethods.GeomCopyQuaternion(prim_geom, out dq);
587 Quaternion q; 584 Quaternion q;
588 q.X = dq.X; 585 q.X = dq.X;
589 q.Y = dq.Y; 586 q.Y = dq.Y;
@@ -591,7 +588,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
591 q.W = dq.W; 588 q.W = dq.W;
592 589
593 Vector3 Ptot = m_OBBOffset * q; 590 Vector3 Ptot = m_OBBOffset * q;
594 dtmp = d.GeomGetPosition(prim_geom); 591 dtmp = SafeNativeMethods.GeomGetPosition(prim_geom);
595 Ptot.X += dtmp.X; 592 Ptot.X += dtmp.X;
596 Ptot.Y += dtmp.Y; 593 Ptot.Y += dtmp.Y;
597 Ptot.Z += dtmp.Z; 594 Ptot.Z += dtmp.Z;
@@ -997,16 +994,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
997 _velocity.Y = 0; 994 _velocity.Y = 0;
998 _velocity.Z = 0; 995 _velocity.Z = 0;
999 996
1000 d.AllocateODEDataForThread(0); 997 SafeNativeMethods.AllocateODEDataForThread(0);
1001 998
1002 m_lastVelocity = _velocity; 999 m_lastVelocity = _velocity;
1003 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 1000 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
1004 m_vehicle.Stop(); 1001 m_vehicle.Stop();
1005 1002
1006 if(Body != IntPtr.Zero) 1003 if(Body != IntPtr.Zero)
1007 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 1004 SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
1008 if (prim_geom != IntPtr.Zero) 1005 if (prim_geom != IntPtr.Zero)
1009 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 1006 SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1010 1007
1011 m_outbounds = false; 1008 m_outbounds = false;
1012 changeDisable(false); 1009 changeDisable(false);
@@ -1027,24 +1024,24 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1027 m_lastposition = _position; 1024 m_lastposition = _position;
1028 m_lastorientation = _orientation; 1025 m_lastorientation = _orientation;
1029 1026
1030 d.AllocateODEDataForThread(0); 1027 SafeNativeMethods.AllocateODEDataForThread(0);
1031 if(Body != IntPtr.Zero) 1028 if(Body != IntPtr.Zero)
1032 { 1029 {
1033 d.Vector3 dtmp = d.BodyGetAngularVel(Body); 1030 SafeNativeMethods.Vector3 dtmp = SafeNativeMethods.BodyGetAngularVel(Body);
1034 m_rotationalVelocity.X = dtmp.X; 1031 m_rotationalVelocity.X = dtmp.X;
1035 m_rotationalVelocity.Y = dtmp.Y; 1032 m_rotationalVelocity.Y = dtmp.Y;
1036 m_rotationalVelocity.Z = dtmp.Z; 1033 m_rotationalVelocity.Z = dtmp.Z;
1037 1034
1038 dtmp = d.BodyGetLinearVel(Body); 1035 dtmp = SafeNativeMethods.BodyGetLinearVel(Body);
1039 _velocity.X = dtmp.X; 1036 _velocity.X = dtmp.X;
1040 _velocity.Y = dtmp.Y; 1037 _velocity.Y = dtmp.Y;
1041 _velocity.Z = dtmp.Z; 1038 _velocity.Z = dtmp.Z;
1042 1039
1043 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 1040 SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
1044 d.BodySetAngularVel(Body, 0, 0, 0); 1041 SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
1045 } 1042 }
1046 if(prim_geom != IntPtr.Zero) 1043 if(prim_geom != IntPtr.Zero)
1047 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 1044 SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
1048 disableBodySoft(); // stop collisions 1045 disableBodySoft(); // stop collisions
1049 UnSubscribeEvents(); 1046 UnSubscribeEvents();
1050 } 1047 }
@@ -1241,7 +1238,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1241 SentEmptyCollisionsEvent = true; 1238 SentEmptyCollisionsEvent = true;
1242// _parent_scene.RemoveCollisionEventReporting(this); 1239// _parent_scene.RemoveCollisionEventReporting(this);
1243 } 1240 }
1244 else if(Body == IntPtr.Zero || (d.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 )) 1241 else if(Body == IntPtr.Zero || (SafeNativeMethods.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 ))
1245 { 1242 {
1246 SentEmptyCollisionsEvent = false; 1243 SentEmptyCollisionsEvent = false;
1247 CollisionEvents.Clear(); 1244 CollisionEvents.Clear();
@@ -1450,16 +1447,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1450 { 1447 {
1451 if (prm.m_NoColide) 1448 if (prm.m_NoColide)
1452 { 1449 {
1453 d.GeomSetCategoryBits(prm.prim_geom, 0); 1450 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
1454 if (m_isphysical) 1451 if (m_isphysical)
1455 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); 1452 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1456 else 1453 else
1457 d.GeomSetCollideBits(prm.prim_geom, 0); 1454 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
1458 } 1455 }
1459 else 1456 else
1460 { 1457 {
1461 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); 1458 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1462 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); 1459 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1463 } 1460 }
1464 } 1461 }
1465 } 1462 }
@@ -1467,22 +1464,22 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1467 1464
1468 if (m_NoColide) 1465 if (m_NoColide)
1469 { 1466 {
1470 d.GeomSetCategoryBits(prim_geom, 0); 1467 SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
1471 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 1468 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1472 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1469 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1473 { 1470 {
1474 d.GeomSetCategoryBits(collide_geom, 0); 1471 SafeNativeMethods.GeomSetCategoryBits(collide_geom, 0);
1475 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); 1472 SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
1476 } 1473 }
1477 } 1474 }
1478 else 1475 else
1479 { 1476 {
1480 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1477 SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1481 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 1478 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1482 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 1479 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1483 { 1480 {
1484 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); 1481 SafeNativeMethods.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1485 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); 1482 SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1486 } 1483 }
1487 } 1484 }
1488 } 1485 }
@@ -1495,7 +1492,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1495 1492
1496 if (Amotor != IntPtr.Zero) 1493 if (Amotor != IntPtr.Zero)
1497 { 1494 {
1498 d.JointDestroy(Amotor); 1495 SafeNativeMethods.JointDestroy(Amotor);
1499 Amotor = IntPtr.Zero; 1496 Amotor = IntPtr.Zero;
1500 } 1497 }
1501 1498
@@ -1522,19 +1519,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1522 if(axisnum == 0) 1519 if(axisnum == 0)
1523 return; 1520 return;
1524 // stop it 1521 // stop it
1525 d.BodySetTorque(Body, 0, 0, 0); 1522 SafeNativeMethods.BodySetTorque(Body, 0, 0, 0);
1526 d.BodySetAngularVel(Body, 0, 0, 0); 1523 SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
1527 1524
1528 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 1525 Amotor = SafeNativeMethods.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1529 d.JointAttach(Amotor, Body, IntPtr.Zero); 1526 SafeNativeMethods.JointAttach(Amotor, Body, IntPtr.Zero);
1530 1527
1531 d.JointSetAMotorMode(Amotor, 0); 1528 SafeNativeMethods.JointSetAMotorMode(Amotor, 0);
1532 1529
1533 d.JointSetAMotorNumAxes(Amotor, axisnum); 1530 SafeNativeMethods.JointSetAMotorNumAxes(Amotor, axisnum);
1534 1531
1535 // get current orientation to lock 1532 // get current orientation to lock
1536 1533
1537 d.Quaternion dcur = d.BodyGetQuaternion(Body); 1534 SafeNativeMethods.Quaternion dcur = SafeNativeMethods.BodyGetQuaternion(Body);
1538 Quaternion curr; // crap convertion between identical things 1535 Quaternion curr; // crap convertion between identical things
1539 curr.X = dcur.X; 1536 curr.X = dcur.X;
1540 curr.Y = dcur.Y; 1537 curr.Y = dcur.Y;
@@ -1547,17 +1544,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1547 if (axisX) 1544 if (axisX)
1548 { 1545 {
1549 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X 1546 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1550 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); 1547 SafeNativeMethods.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1551 d.JointSetAMotorAngle(Amotor, 0, 0); 1548 SafeNativeMethods.JointSetAMotorAngle(Amotor, 0, 0);
1552 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); 1549 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.LoStop, 0f);
1553 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); 1550 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.HiStop, 0f);
1554 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); 1551 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.Vel, 0);
1555 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); 1552 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
1556 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); 1553 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.Bounce, 0f);
1557 d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f); 1554 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.CFM, 0f);
1558 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); 1555 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.FMax, 5e8f);
1559 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); 1556 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.StopCFM, 0f);
1560 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); 1557 SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
1561 i++; 1558 i++;
1562 j = 256; // move to next axis set 1559 j = 256; // move to next axis set
1563 } 1560 }
@@ -1565,17 +1562,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1565 if (axisY) 1562 if (axisY)
1566 { 1563 {
1567 ax = (new Vector3(0, 1, 0)) * curr; 1564 ax = (new Vector3(0, 1, 0)) * curr;
1568 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1565 SafeNativeMethods.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1569 d.JointSetAMotorAngle(Amotor, i, 0); 1566 SafeNativeMethods.JointSetAMotorAngle(Amotor, i, 0);
1570 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); 1567 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.LoStop, 0f);
1571 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); 1568 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.HiStop, 0f);
1572 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); 1569 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Vel, 0);
1573 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); 1570 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
1574 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); 1571 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Bounce, 0f);
1575 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); 1572 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.CFM, 0f);
1576 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); 1573 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FMax, 5e8f);
1577 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); 1574 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopCFM, 0f);
1578 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); 1575 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
1579 i++; 1576 i++;
1580 j += 256; 1577 j += 256;
1581 } 1578 }
@@ -1583,17 +1580,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1583 if (axisZ) 1580 if (axisZ)
1584 { 1581 {
1585 ax = (new Vector3(0, 0, 1)) * curr; 1582 ax = (new Vector3(0, 0, 1)) * curr;
1586 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1583 SafeNativeMethods.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1587 d.JointSetAMotorAngle(Amotor, i, 0); 1584 SafeNativeMethods.JointSetAMotorAngle(Amotor, i, 0);
1588 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f); 1585 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.LoStop, 0f);
1589 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); 1586 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.HiStop, 0f);
1590 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); 1587 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Vel, 0);
1591 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); 1588 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
1592 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); 1589 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Bounce, 0f);
1593 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f); 1590 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.CFM, 0f);
1594 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); 1591 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FMax, 5e8f);
1595 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); 1592 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopCFM, 0f);
1596 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); 1593 SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
1597 } 1594 }
1598 } 1595 }
1599 1596
@@ -1607,21 +1604,21 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1607 1604
1608 if (m_NoColide) 1605 if (m_NoColide)
1609 { 1606 {
1610 d.GeomSetCategoryBits(prim_geom, 0); 1607 SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
1611 if (m_isphysical) 1608 if (m_isphysical)
1612 { 1609 {
1613 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); 1610 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1614 } 1611 }
1615 else 1612 else
1616 { 1613 {
1617 d.GeomSetCollideBits(prim_geom, 0); 1614 SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
1618 d.GeomDisable(prim_geom); 1615 SafeNativeMethods.GeomDisable(prim_geom);
1619 } 1616 }
1620 } 1617 }
1621 else 1618 else
1622 { 1619 {
1623 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 1620 SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1624 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 1621 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1625 } 1622 }
1626 1623
1627 UpdatePrimBodyData(); 1624 UpdatePrimBodyData();
@@ -1689,12 +1686,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1689 1686
1690 try 1687 try
1691 { 1688 {
1692 _triMeshData = d.GeomTriMeshDataCreate(); 1689 _triMeshData = SafeNativeMethods.GeomTriMeshDataCreate();
1693 1690
1694 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); 1691 SafeNativeMethods.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1695 d.GeomTriMeshDataPreprocess(_triMeshData); 1692 SafeNativeMethods.GeomTriMeshDataPreprocess(_triMeshData);
1696 1693
1697 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); 1694 geo = SafeNativeMethods.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1698 } 1695 }
1699 1696
1700 catch (Exception e) 1697 catch (Exception e)
@@ -1704,7 +1701,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1704 { 1701 {
1705 try 1702 try
1706 { 1703 {
1707 d.GeomTriMeshDataDestroy(_triMeshData); 1704 SafeNativeMethods.GeomTriMeshDataDestroy(_triMeshData);
1708 } 1705 }
1709 catch 1706 catch
1710 { 1707 {
@@ -1760,7 +1757,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1760 { // it's a sphere 1757 { // it's a sphere
1761 try 1758 try
1762 { 1759 {
1763 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); 1760 geo = SafeNativeMethods.CreateSphere(m_targetSpace, _size.X * 0.5f);
1764 } 1761 }
1765 catch (Exception e) 1762 catch (Exception e)
1766 { 1763 {
@@ -1772,7 +1769,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1772 {// do it as a box 1769 {// do it as a box
1773 try 1770 try
1774 { 1771 {
1775 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); 1772 geo = SafeNativeMethods.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
1776 } 1773 }
1777 catch (Exception e) 1774 catch (Exception e)
1778 { 1775 {
@@ -1794,10 +1791,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1794 1791
1795 try 1792 try
1796 { 1793 {
1797 d.GeomDestroy(prim_geom); 1794 SafeNativeMethods.GeomDestroy(prim_geom);
1798 if (_triMeshData != IntPtr.Zero) 1795 if (_triMeshData != IntPtr.Zero)
1799 { 1796 {
1800 d.GeomTriMeshDataDestroy(_triMeshData); 1797 SafeNativeMethods.GeomTriMeshDataDestroy(_triMeshData);
1801 _triMeshData = IntPtr.Zero; 1798 _triMeshData = IntPtr.Zero;
1802 } 1799 }
1803 } 1800 }
@@ -1848,8 +1845,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1848 ApplyCollisionCatFlags(); 1845 ApplyCollisionCatFlags();
1849 1846
1850 _zeroFlag = true; 1847 _zeroFlag = true;
1851 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 1848 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
1852 d.BodyEnable(Body); 1849 SafeNativeMethods.BodyEnable(Body);
1853 } 1850 }
1854 } 1851 }
1855 resetCollisionAccounting(); 1852 resetCollisionAccounting();
@@ -1868,7 +1865,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1868 m_collisionCategories = 0; 1865 m_collisionCategories = 0;
1869 m_collisionFlags = 0; 1866 m_collisionFlags = 0;
1870 ApplyCollisionCatFlags(); 1867 ApplyCollisionCatFlags();
1871 d.BodyDisable(Body); 1868 SafeNativeMethods.BodyDisable(Body);
1872 } 1869 }
1873 } 1870 }
1874 } 1871 }
@@ -1896,41 +1893,41 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1896 m_log.Warn("[PHYSICS]: MakeBody called having a body"); 1893 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1897 } 1894 }
1898 1895
1899 if (d.GeomGetBody(prim_geom) != IntPtr.Zero) 1896 if (SafeNativeMethods.GeomGetBody(prim_geom) != IntPtr.Zero)
1900 { 1897 {
1901 d.GeomSetBody(prim_geom, IntPtr.Zero); 1898 SafeNativeMethods.GeomSetBody(prim_geom, IntPtr.Zero);
1902 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); 1899 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1903 } 1900 }
1904 1901
1905 bool noInertiaOverride = (m_InertiaOverride == null); 1902 bool noInertiaOverride = (m_InertiaOverride == null);
1906 1903
1907 Body = d.BodyCreate(_parent_scene.world); 1904 Body = SafeNativeMethods.BodyCreate(_parent_scene.world);
1908 1905
1909 d.Matrix3 mymat = new d.Matrix3(); 1906 SafeNativeMethods.Matrix3 mymat = new SafeNativeMethods.Matrix3();
1910 d.Quaternion myrot = new d.Quaternion(); 1907 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
1911 d.Mass objdmass = new d.Mass { }; 1908 SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
1912 1909
1913 myrot.X = _orientation.X; 1910 myrot.X = _orientation.X;
1914 myrot.Y = _orientation.Y; 1911 myrot.Y = _orientation.Y;
1915 myrot.Z = _orientation.Z; 1912 myrot.Z = _orientation.Z;
1916 myrot.W = _orientation.W; 1913 myrot.W = _orientation.W;
1917 d.RfromQ(out mymat, ref myrot); 1914 SafeNativeMethods.RfromQ(out mymat, ref myrot);
1918 1915
1919 // set the body rotation 1916 // set the body rotation
1920 d.BodySetRotation(Body, ref mymat); 1917 SafeNativeMethods.BodySetRotation(Body, ref mymat);
1921 1918
1922 if(noInertiaOverride) 1919 if(noInertiaOverride)
1923 { 1920 {
1924 objdmass = primdMass; 1921 objdmass = primdMass;
1925 d.MassRotate(ref objdmass, ref mymat); 1922 SafeNativeMethods.MassRotate(ref objdmass, ref mymat);
1926 } 1923 }
1927 1924
1928 // recompute full object inertia if needed 1925 // recompute full object inertia if needed
1929 if (childrenPrim.Count > 0) 1926 if (childrenPrim.Count > 0)
1930 { 1927 {
1931 d.Matrix3 mat = new d.Matrix3(); 1928 SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
1932 d.Quaternion quat = new d.Quaternion(); 1929 SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
1933 d.Mass tmpdmass = new d.Mass { }; 1930 SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
1934 Vector3 rcm; 1931 Vector3 rcm;
1935 1932
1936 rcm.X = _position.X; 1933 rcm.X = _position.X;
@@ -1951,70 +1948,70 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1951 quat.Y = prm._orientation.Y; 1948 quat.Y = prm._orientation.Y;
1952 quat.Z = prm._orientation.Z; 1949 quat.Z = prm._orientation.Z;
1953 quat.W = prm._orientation.W; 1950 quat.W = prm._orientation.W;
1954 d.RfromQ(out mat, ref quat); 1951 SafeNativeMethods.RfromQ(out mat, ref quat);
1955 1952
1956 // fix prim colision cats 1953 // fix prim colision cats
1957 1954
1958 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) 1955 if (SafeNativeMethods.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1959 { 1956 {
1960 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 1957 SafeNativeMethods.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1961 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); 1958 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1962 } 1959 }
1963 1960
1964 d.GeomClearOffset(prm.prim_geom); 1961 SafeNativeMethods.GeomClearOffset(prm.prim_geom);
1965 d.GeomSetBody(prm.prim_geom, Body); 1962 SafeNativeMethods.GeomSetBody(prm.prim_geom, Body);
1966 prm.Body = Body; 1963 prm.Body = Body;
1967 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation 1964 SafeNativeMethods.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1968 1965
1969 if(noInertiaOverride) 1966 if(noInertiaOverride)
1970 { 1967 {
1971 tmpdmass = prm.primdMass; 1968 tmpdmass = prm.primdMass;
1972 1969
1973 d.MassRotate(ref tmpdmass, ref mat); 1970 SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
1974 Vector3 ppos = prm._position; 1971 Vector3 ppos = prm._position;
1975 ppos.X -= rcm.X; 1972 ppos.X -= rcm.X;
1976 ppos.Y -= rcm.Y; 1973 ppos.Y -= rcm.Y;
1977 ppos.Z -= rcm.Z; 1974 ppos.Z -= rcm.Z;
1978 // refer inertia to root prim center of mass position 1975 // refer inertia to root prim center of mass position
1979 d.MassTranslate(ref tmpdmass, 1976 SafeNativeMethods.MassTranslate(ref tmpdmass,
1980 ppos.X, 1977 ppos.X,
1981 ppos.Y, 1978 ppos.Y,
1982 ppos.Z); 1979 ppos.Z);
1983 1980
1984 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia 1981 SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1985 } 1982 }
1986 } 1983 }
1987 } 1984 }
1988 } 1985 }
1989 1986
1990 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset 1987 SafeNativeMethods.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1991 // associate root geom with body 1988 // associate root geom with body
1992 d.GeomSetBody(prim_geom, Body); 1989 SafeNativeMethods.GeomSetBody(prim_geom, Body);
1993 1990
1994 if(noInertiaOverride) 1991 if(noInertiaOverride)
1995 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); 1992 SafeNativeMethods.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1996 else 1993 else
1997 { 1994 {
1998 Vector3 ncm = m_InertiaOverride.CenterOfMass * _orientation; 1995 Vector3 ncm = m_InertiaOverride.CenterOfMass * _orientation;
1999 d.BodySetPosition(Body, 1996 SafeNativeMethods.BodySetPosition(Body,
2000 _position.X + ncm.X, 1997 _position.X + ncm.X,
2001 _position.Y + ncm.Y, 1998 _position.Y + ncm.Y,
2002 _position.Z + ncm.Z); 1999 _position.Z + ncm.Z);
2003 } 2000 }
2004 2001
2005 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); 2002 SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
2006 2003
2007 if(noInertiaOverride) 2004 if(noInertiaOverride)
2008 { 2005 {
2009 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 2006 SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2010 myrot.X = -myrot.X; 2007 myrot.X = -myrot.X;
2011 myrot.Y = -myrot.Y; 2008 myrot.Y = -myrot.Y;
2012 myrot.Z = -myrot.Z; 2009 myrot.Z = -myrot.Z;
2013 2010
2014 d.RfromQ(out mymat, ref myrot); 2011 SafeNativeMethods.RfromQ(out mymat, ref myrot);
2015 d.MassRotate(ref objdmass, ref mymat); 2012 SafeNativeMethods.MassRotate(ref objdmass, ref mymat);
2016 2013
2017 d.BodySetMass(Body, ref objdmass); 2014 SafeNativeMethods.BodySetMass(Body, ref objdmass);
2018 m_mass = objdmass.mass; 2015 m_mass = objdmass.mass;
2019 } 2016 }
2020 else 2017 else
@@ -2031,35 +2028,35 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2031 2028
2032 if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999) 2029 if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999)
2033 { 2030 {
2034 d.Matrix3 inertiarotmat = new d.Matrix3(); 2031 SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3();
2035 d.Quaternion inertiarot = new d.Quaternion(); 2032 SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion();
2036 2033
2037 inertiarot.X = m_InertiaOverride.InertiaRotation.X; 2034 inertiarot.X = m_InertiaOverride.InertiaRotation.X;
2038 inertiarot.Y = m_InertiaOverride.InertiaRotation.Y; 2035 inertiarot.Y = m_InertiaOverride.InertiaRotation.Y;
2039 inertiarot.Z = m_InertiaOverride.InertiaRotation.Z; 2036 inertiarot.Z = m_InertiaOverride.InertiaRotation.Z;
2040 inertiarot.W = m_InertiaOverride.InertiaRotation.W; 2037 inertiarot.W = m_InertiaOverride.InertiaRotation.W;
2041 d.RfromQ(out inertiarotmat, ref inertiarot); 2038 SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot);
2042 d.MassRotate(ref objdmass, ref inertiarotmat); 2039 SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat);
2043 } 2040 }
2044 d.BodySetMass(Body, ref objdmass); 2041 SafeNativeMethods.BodySetMass(Body, ref objdmass);
2045 2042
2046 m_mass = objdmass.mass; 2043 m_mass = objdmass.mass;
2047 } 2044 }
2048 2045
2049 // disconnect from world gravity so we can apply buoyancy 2046 // disconnect from world gravity so we can apply buoyancy
2050 d.BodySetGravityMode(Body, false); 2047 SafeNativeMethods.BodySetGravityMode(Body, false);
2051 2048
2052 d.BodySetAutoDisableFlag(Body, true); 2049 SafeNativeMethods.BodySetAutoDisableFlag(Body, true);
2053 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 2050 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
2054 d.BodySetAutoDisableAngularThreshold(Body, 0.05f); 2051 SafeNativeMethods.BodySetAutoDisableAngularThreshold(Body, 0.05f);
2055 d.BodySetAutoDisableLinearThreshold(Body, 0.05f); 2052 SafeNativeMethods.BodySetAutoDisableLinearThreshold(Body, 0.05f);
2056 d.BodySetDamping(Body, .004f, .001f); 2053 SafeNativeMethods.BodySetDamping(Body, .004f, .001f);
2057 2054
2058 if (m_targetSpace != IntPtr.Zero) 2055 if (m_targetSpace != IntPtr.Zero)
2059 { 2056 {
2060 _parent_scene.waitForSpaceUnlock(m_targetSpace); 2057 _parent_scene.waitForSpaceUnlock(m_targetSpace);
2061 if (d.SpaceQuery(m_targetSpace, prim_geom)) 2058 if (SafeNativeMethods.SpaceQuery(m_targetSpace, prim_geom))
2062 d.SpaceRemove(m_targetSpace, prim_geom); 2059 SafeNativeMethods.SpaceRemove(m_targetSpace, prim_geom);
2063 } 2060 }
2064 2061
2065 if (childrenPrim.Count == 0) 2062 if (childrenPrim.Count == 0)
@@ -2069,20 +2066,23 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2069 } 2066 }
2070 else 2067 else
2071 { 2068 {
2072 m_targetSpace = d.SimpleSpaceCreate(_parent_scene.ActiveSpace); 2069 m_targetSpace = SafeNativeMethods.SimpleSpaceCreate(_parent_scene.ActiveSpace);
2073 d.SpaceSetSublevel(m_targetSpace, 3); 2070 SafeNativeMethods.SpaceSetSublevel(m_targetSpace, 3);
2074 d.SpaceSetCleanup(m_targetSpace, false); 2071 SafeNativeMethods.SpaceSetCleanup(m_targetSpace, false);
2075 2072
2076 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | 2073 SafeNativeMethods.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
2077 CollisionCategories.Geom | 2074 CollisionCategories.Geom |
2078 CollisionCategories.Phantom | 2075 CollisionCategories.Phantom |
2079 CollisionCategories.VolumeDtc 2076 CollisionCategories.VolumeDtc
2080 )); 2077 ));
2081 d.GeomSetCollideBits(m_targetSpace, 0); 2078 SafeNativeMethods.GeomSetCollideBits(m_targetSpace, 0);
2082 collide_geom = m_targetSpace; 2079 collide_geom = m_targetSpace;
2083 } 2080 }
2084 2081
2085 d.SpaceAdd(m_targetSpace, prim_geom); 2082 if (SafeNativeMethods.SpaceQuery(m_targetSpace, prim_geom))
2083 m_log.Debug("[PRIM]: parent already in target space");
2084 else
2085 SafeNativeMethods.SpaceAdd(m_targetSpace, prim_geom);
2086 2086
2087 if (m_delaySelect) 2087 if (m_delaySelect)
2088 { 2088 {
@@ -2101,22 +2101,27 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2101 { 2101 {
2102 foreach (OdePrim prm in childrenPrim) 2102 foreach (OdePrim prm in childrenPrim)
2103 { 2103 {
2104 if (prm.prim_geom == IntPtr.Zero) 2104 IntPtr prmgeom = prm.prim_geom;
2105 if (prmgeom == IntPtr.Zero)
2105 continue; 2106 continue;
2106 2107
2107 Vector3 ppos = prm._position; 2108 Vector3 ppos = prm._position;
2108 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position 2109 SafeNativeMethods.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
2109 2110
2110 if (prm.m_targetSpace != m_targetSpace) 2111 IntPtr prmspace = prm.m_targetSpace;
2112 if (prmspace != m_targetSpace)
2111 { 2113 {
2112 if (prm.m_targetSpace != IntPtr.Zero) 2114 if (prmspace != IntPtr.Zero)
2113 { 2115 {
2114 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); 2116 _parent_scene.waitForSpaceUnlock(prmspace);
2115 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 2117 if (SafeNativeMethods.SpaceQuery(prmspace, prmgeom))
2116 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); 2118 SafeNativeMethods.SpaceRemove(prmspace, prmgeom);
2117 } 2119 }
2118 prm.m_targetSpace = m_targetSpace; 2120 prm.m_targetSpace = m_targetSpace;
2119 d.SpaceAdd(m_targetSpace, prm.prim_geom); 2121 if (SafeNativeMethods.SpaceQuery(m_targetSpace, prmgeom))
2122 m_log.Debug("[PRIM]: child already in target space");
2123 else
2124 SafeNativeMethods.SpaceAdd(m_targetSpace, prmgeom);
2120 } 2125 }
2121 2126
2122 prm.m_collisionscore = 0; 2127 prm.m_collisionscore = 0;
@@ -2136,13 +2141,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2136 2141
2137 if (m_isSelected || m_disabled) 2142 if (m_isSelected || m_disabled)
2138 { 2143 {
2139 d.BodyDisable(Body); 2144 SafeNativeMethods.BodyDisable(Body);
2140 _zeroFlag = true; 2145 _zeroFlag = true;
2141 } 2146 }
2142 else 2147 else
2143 { 2148 {
2144 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); 2149 SafeNativeMethods.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
2145 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); 2150 SafeNativeMethods.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
2146 2151
2147 _zeroFlag = false; 2152 _zeroFlag = false;
2148 m_bodydisablecontrol = 0; 2153 m_bodydisablecontrol = 0;
@@ -2175,16 +2180,16 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2175 { 2180 {
2176 if (m_NoColide) 2181 if (m_NoColide)
2177 { 2182 {
2178 d.GeomSetCategoryBits(prim_geom, 0); 2183 SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
2179 d.GeomSetCollideBits(prim_geom, 0); 2184 SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
2180 } 2185 }
2181 else 2186 else
2182 { 2187 {
2183 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 2188 SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2184 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 2189 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2185 } 2190 }
2186 UpdateDataFromGeom(); 2191 UpdateDataFromGeom();
2187 d.GeomSetBody(prim_geom, IntPtr.Zero); 2192 SafeNativeMethods.GeomSetBody(prim_geom, IntPtr.Zero);
2188 SetInStaticSpace(this); 2193 SetInStaticSpace(this);
2189 } 2194 }
2190 2195
@@ -2211,13 +2216,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2211 { 2216 {
2212 if (prm.m_NoColide) 2217 if (prm.m_NoColide)
2213 { 2218 {
2214 d.GeomSetCategoryBits(prm.prim_geom, 0); 2219 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
2215 d.GeomSetCollideBits(prm.prim_geom, 0); 2220 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
2216 } 2221 }
2217 else 2222 else
2218 { 2223 {
2219 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); 2224 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
2220 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); 2225 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
2221 } 2226 }
2222 prm.UpdateDataFromGeom(); 2227 prm.UpdateDataFromGeom();
2223 SetInStaticSpace(prm); 2228 SetInStaticSpace(prm);
@@ -2229,11 +2234,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2229 } 2234 }
2230 if (Amotor != IntPtr.Zero) 2235 if (Amotor != IntPtr.Zero)
2231 { 2236 {
2232 d.JointDestroy(Amotor); 2237 SafeNativeMethods.JointDestroy(Amotor);
2233 Amotor = IntPtr.Zero; 2238 Amotor = IntPtr.Zero;
2234 } 2239 }
2235 _parent_scene.remActiveGroup(this); 2240 _parent_scene.remActiveGroup(this);
2236 d.BodyDestroy(Body); 2241 SafeNativeMethods.BodyDestroy(Body);
2237 } 2242 }
2238 Body = IntPtr.Zero; 2243 Body = IntPtr.Zero;
2239 } 2244 }
@@ -2243,30 +2248,30 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2243 2248
2244 private void FixInertia(Vector3 NewPos,Quaternion newrot) 2249 private void FixInertia(Vector3 NewPos,Quaternion newrot)
2245 { 2250 {
2246 d.Matrix3 mat = new d.Matrix3(); 2251 SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
2247 d.Quaternion quat = new d.Quaternion(); 2252 SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
2248 2253
2249 d.Mass tmpdmass = new d.Mass { }; 2254 SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
2250 d.Mass objdmass = new d.Mass { }; 2255 SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
2251 2256
2252 d.BodyGetMass(Body, out tmpdmass); 2257 SafeNativeMethods.BodyGetMass(Body, out tmpdmass);
2253 objdmass = tmpdmass; 2258 objdmass = tmpdmass;
2254 2259
2255 d.Vector3 dobjpos; 2260 SafeNativeMethods.Vector3 dobjpos;
2256 d.Vector3 thispos; 2261 SafeNativeMethods.Vector3 thispos;
2257 2262
2258 // get current object position and rotation 2263 // get current object position and rotation
2259 dobjpos = d.BodyGetPosition(Body); 2264 dobjpos = SafeNativeMethods.BodyGetPosition(Body);
2260 2265
2261 // get prim own inertia in its local frame 2266 // get prim own inertia in its local frame
2262 tmpdmass = primdMass; 2267 tmpdmass = primdMass;
2263 2268
2264 // transform to object frame 2269 // transform to object frame
2265 mat = d.GeomGetOffsetRotation(prim_geom); 2270 mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
2266 d.MassRotate(ref tmpdmass, ref mat); 2271 SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
2267 2272
2268 thispos = d.GeomGetOffsetPosition(prim_geom); 2273 thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
2269 d.MassTranslate(ref tmpdmass, 2274 SafeNativeMethods.MassTranslate(ref tmpdmass,
2270 thispos.X, 2275 thispos.X,
2271 thispos.Y, 2276 thispos.Y,
2272 thispos.Z); 2277 thispos.Z);
@@ -2279,66 +2284,66 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2279 2284
2280 // update to new position and orientation 2285 // update to new position and orientation
2281 _position = NewPos; 2286 _position = NewPos;
2282 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); 2287 SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2283 _orientation = newrot; 2288 _orientation = newrot;
2284 quat.X = newrot.X; 2289 quat.X = newrot.X;
2285 quat.Y = newrot.Y; 2290 quat.Y = newrot.Y;
2286 quat.Z = newrot.Z; 2291 quat.Z = newrot.Z;
2287 quat.W = newrot.W; 2292 quat.W = newrot.W;
2288 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); 2293 SafeNativeMethods.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2289 2294
2290 mat = d.GeomGetOffsetRotation(prim_geom); 2295 mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
2291 d.MassRotate(ref tmpdmass, ref mat); 2296 SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
2292 2297
2293 thispos = d.GeomGetOffsetPosition(prim_geom); 2298 thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
2294 d.MassTranslate(ref tmpdmass, 2299 SafeNativeMethods.MassTranslate(ref tmpdmass,
2295 thispos.X, 2300 thispos.X,
2296 thispos.Y, 2301 thispos.Y,
2297 thispos.Z); 2302 thispos.Z);
2298 2303
2299 d.MassAdd(ref objdmass, ref tmpdmass); 2304 SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass);
2300 2305
2301 // fix all positions 2306 // fix all positions
2302 IntPtr g = d.BodyGetFirstGeom(Body); 2307 IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
2303 while (g != IntPtr.Zero) 2308 while (g != IntPtr.Zero)
2304 { 2309 {
2305 thispos = d.GeomGetOffsetPosition(g); 2310 thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
2306 thispos.X -= objdmass.c.X; 2311 thispos.X -= objdmass.c.X;
2307 thispos.Y -= objdmass.c.Y; 2312 thispos.Y -= objdmass.c.Y;
2308 thispos.Z -= objdmass.c.Z; 2313 thispos.Z -= objdmass.c.Z;
2309 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 2314 SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2310 g = d.dBodyGetNextGeom(g); 2315 g = SafeNativeMethods.dBodyGetNextGeom(g);
2311 } 2316 }
2312 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); 2317 SafeNativeMethods.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
2313 2318
2314 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); 2319 SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2315 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 2320 SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2316 d.BodySetMass(Body, ref objdmass); 2321 SafeNativeMethods.BodySetMass(Body, ref objdmass);
2317 m_mass = objdmass.mass; 2322 m_mass = objdmass.mass;
2318 } 2323 }
2319 2324
2320 private void FixInertia(Vector3 NewPos) 2325 private void FixInertia(Vector3 NewPos)
2321 { 2326 {
2322 d.Matrix3 primmat = new d.Matrix3(); 2327 SafeNativeMethods.Matrix3 primmat = new SafeNativeMethods.Matrix3();
2323 d.Mass tmpdmass = new d.Mass { }; 2328 SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
2324 d.Mass objdmass = new d.Mass { }; 2329 SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
2325 d.Mass primmass = new d.Mass { }; 2330 SafeNativeMethods.Mass primmass = new SafeNativeMethods.Mass { };
2326 2331
2327 d.Vector3 dobjpos; 2332 SafeNativeMethods.Vector3 dobjpos;
2328 d.Vector3 thispos; 2333 SafeNativeMethods.Vector3 thispos;
2329 2334
2330 d.BodyGetMass(Body, out objdmass); 2335 SafeNativeMethods.BodyGetMass(Body, out objdmass);
2331 2336
2332 // get prim own inertia in its local frame 2337 // get prim own inertia in its local frame
2333 primmass = primdMass; 2338 primmass = primdMass;
2334 // transform to object frame 2339 // transform to object frame
2335 primmat = d.GeomGetOffsetRotation(prim_geom); 2340 primmat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
2336 d.MassRotate(ref primmass, ref primmat); 2341 SafeNativeMethods.MassRotate(ref primmass, ref primmat);
2337 2342
2338 tmpdmass = primmass; 2343 tmpdmass = primmass;
2339 2344
2340 thispos = d.GeomGetOffsetPosition(prim_geom); 2345 thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
2341 d.MassTranslate(ref tmpdmass, 2346 SafeNativeMethods.MassTranslate(ref tmpdmass,
2342 thispos.X, 2347 thispos.X,
2343 thispos.Y, 2348 thispos.Y,
2344 thispos.Z); 2349 thispos.Z);
@@ -2348,58 +2353,58 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2348 2353
2349 // update to new position 2354 // update to new position
2350 _position = NewPos; 2355 _position = NewPos;
2351 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); 2356 SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2352 2357
2353 thispos = d.GeomGetOffsetPosition(prim_geom); 2358 thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
2354 d.MassTranslate(ref primmass, 2359 SafeNativeMethods.MassTranslate(ref primmass,
2355 thispos.X, 2360 thispos.X,
2356 thispos.Y, 2361 thispos.Y,
2357 thispos.Z); 2362 thispos.Z);
2358 2363
2359 d.MassAdd(ref objdmass, ref primmass); 2364 SafeNativeMethods.MassAdd(ref objdmass, ref primmass);
2360 2365
2361 // fix all positions 2366 // fix all positions
2362 IntPtr g = d.BodyGetFirstGeom(Body); 2367 IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
2363 while (g != IntPtr.Zero) 2368 while (g != IntPtr.Zero)
2364 { 2369 {
2365 thispos = d.GeomGetOffsetPosition(g); 2370 thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
2366 thispos.X -= objdmass.c.X; 2371 thispos.X -= objdmass.c.X;
2367 thispos.Y -= objdmass.c.Y; 2372 thispos.Y -= objdmass.c.Y;
2368 thispos.Z -= objdmass.c.Z; 2373 thispos.Z -= objdmass.c.Z;
2369 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 2374 SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2370 g = d.dBodyGetNextGeom(g); 2375 g = SafeNativeMethods.dBodyGetNextGeom(g);
2371 } 2376 }
2372 2377
2373 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); 2378 SafeNativeMethods.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2374 2379
2375 // get current object position and rotation 2380 // get current object position and rotation
2376 dobjpos = d.BodyGetPosition(Body); 2381 dobjpos = SafeNativeMethods.BodyGetPosition(Body);
2377 2382
2378 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); 2383 SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2379 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 2384 SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2380 d.BodySetMass(Body, ref objdmass); 2385 SafeNativeMethods.BodySetMass(Body, ref objdmass);
2381 m_mass = objdmass.mass; 2386 m_mass = objdmass.mass;
2382 } 2387 }
2383 2388
2384 private void FixInertia(Quaternion newrot) 2389 private void FixInertia(Quaternion newrot)
2385 { 2390 {
2386 d.Matrix3 mat = new d.Matrix3(); 2391 SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
2387 d.Quaternion quat = new d.Quaternion(); 2392 SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
2388 2393
2389 d.Mass tmpdmass = new d.Mass { }; 2394 SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
2390 d.Mass objdmass = new d.Mass { }; 2395 SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
2391 d.Vector3 dobjpos; 2396 SafeNativeMethods.Vector3 dobjpos;
2392 d.Vector3 thispos; 2397 SafeNativeMethods.Vector3 thispos;
2393 2398
2394 d.BodyGetMass(Body, out objdmass); 2399 SafeNativeMethods.BodyGetMass(Body, out objdmass);
2395 2400
2396 // get prim own inertia in its local frame 2401 // get prim own inertia in its local frame
2397 tmpdmass = primdMass; 2402 tmpdmass = primdMass;
2398 mat = d.GeomGetOffsetRotation(prim_geom); 2403 mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
2399 d.MassRotate(ref tmpdmass, ref mat); 2404 SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
2400 // transform to object frame 2405 // transform to object frame
2401 thispos = d.GeomGetOffsetPosition(prim_geom); 2406 thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
2402 d.MassTranslate(ref tmpdmass, 2407 SafeNativeMethods.MassTranslate(ref tmpdmass,
2403 thispos.X, 2408 thispos.X,
2404 thispos.Y, 2409 thispos.Y,
2405 thispos.Z); 2410 thispos.Z);
@@ -2413,37 +2418,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2413 quat.Y = newrot.Y; 2418 quat.Y = newrot.Y;
2414 quat.Z = newrot.Z; 2419 quat.Z = newrot.Z;
2415 quat.W = newrot.W; 2420 quat.W = newrot.W;
2416 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); 2421 SafeNativeMethods.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2417 2422
2418 tmpdmass = primdMass; 2423 tmpdmass = primdMass;
2419 mat = d.GeomGetOffsetRotation(prim_geom); 2424 mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
2420 d.MassRotate(ref tmpdmass, ref mat); 2425 SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
2421 d.MassTranslate(ref tmpdmass, 2426 SafeNativeMethods.MassTranslate(ref tmpdmass,
2422 thispos.X, 2427 thispos.X,
2423 thispos.Y, 2428 thispos.Y,
2424 thispos.Z); 2429 thispos.Z);
2425 2430
2426 d.MassAdd(ref objdmass, ref tmpdmass); 2431 SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass);
2427 2432
2428 // fix all positions 2433 // fix all positions
2429 IntPtr g = d.BodyGetFirstGeom(Body); 2434 IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
2430 while (g != IntPtr.Zero) 2435 while (g != IntPtr.Zero)
2431 { 2436 {
2432 thispos = d.GeomGetOffsetPosition(g); 2437 thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
2433 thispos.X -= objdmass.c.X; 2438 thispos.X -= objdmass.c.X;
2434 thispos.Y -= objdmass.c.Y; 2439 thispos.Y -= objdmass.c.Y;
2435 thispos.Z -= objdmass.c.Z; 2440 thispos.Z -= objdmass.c.Z;
2436 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 2441 SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2437 g = d.dBodyGetNextGeom(g); 2442 g = SafeNativeMethods.dBodyGetNextGeom(g);
2438 } 2443 }
2439 2444
2440 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); 2445 SafeNativeMethods.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2441 // get current object position and rotation 2446 // get current object position and rotation
2442 dobjpos = d.BodyGetPosition(Body); 2447 dobjpos = SafeNativeMethods.BodyGetPosition(Body);
2443 2448
2444 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); 2449 SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2445 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 2450 SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2446 d.BodySetMass(Body, ref objdmass); 2451 SafeNativeMethods.BodySetMass(Body, ref objdmass);
2447 m_mass = objdmass.mass; 2452 m_mass = objdmass.mass;
2448 } 2453 }
2449 2454
@@ -2461,9 +2466,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2461 2466
2462 m_mass = primMass; // just in case 2467 m_mass = primMass; // just in case
2463 2468
2464 d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); 2469 SafeNativeMethods.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z);
2465 2470
2466 d.MassTranslate(ref primdMass, 2471 SafeNativeMethods.MassTranslate(ref primdMass,
2467 m_OBBOffset.X, 2472 m_OBBOffset.X,
2468 m_OBBOffset.Y, 2473 m_OBBOffset.Y,
2469 m_OBBOffset.Z); 2474 m_OBBOffset.Z);
@@ -2518,7 +2523,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2518 if (prm.Body != IntPtr.Zero) 2523 if (prm.Body != IntPtr.Zero)
2519 { 2524 {
2520 if (prm.prim_geom != IntPtr.Zero) 2525 if (prm.prim_geom != IntPtr.Zero)
2521 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 2526 SafeNativeMethods.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2522 if (prm.Body != prim.Body) 2527 if (prm.Body != prim.Body)
2523 prm.DestroyBody(); // don't loose bodies around 2528 prm.DestroyBody(); // don't loose bodies around
2524 prm.Body = IntPtr.Zero; 2529 prm.Body = IntPtr.Zero;
@@ -2535,7 +2540,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2535 if (prim.Body != IntPtr.Zero) 2540 if (prim.Body != IntPtr.Zero)
2536 { 2541 {
2537 if (prim.prim_geom != IntPtr.Zero) 2542 if (prim.prim_geom != IntPtr.Zero)
2538 d.GeomSetBody(prim.prim_geom, IntPtr.Zero); 2543 SafeNativeMethods.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2539 prim.DestroyBody(); // don't loose bodies around 2544 prim.DestroyBody(); // don't loose bodies around
2540 prim.Body = IntPtr.Zero; 2545 prim.Body = IntPtr.Zero;
2541 } 2546 }
@@ -2560,8 +2565,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2560 { 2565 {
2561 if (prim_geom != IntPtr.Zero) 2566 if (prim_geom != IntPtr.Zero)
2562 { 2567 {
2563 d.Quaternion qtmp; 2568 SafeNativeMethods.Quaternion qtmp;
2564 d.GeomCopyQuaternion(prim_geom, out qtmp); 2569 SafeNativeMethods.GeomCopyQuaternion(prim_geom, out qtmp);
2565 _orientation.X = qtmp.X; 2570 _orientation.X = qtmp.X;
2566 _orientation.Y = qtmp.Y; 2571 _orientation.Y = qtmp.Y;
2567 _orientation.Z = qtmp.Z; 2572 _orientation.Z = qtmp.Z;
@@ -2575,7 +2580,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2575*/ 2580*/
2576 _orientation.Normalize(); 2581 _orientation.Normalize();
2577 2582
2578 d.Vector3 lpos = d.GeomGetPosition(prim_geom); 2583 SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom);
2579 _position.X = lpos.X; 2584 _position.X = lpos.X;
2580 _position.Y = lpos.Y; 2585 _position.Y = lpos.Y;
2581 _position.Z = lpos.Z; 2586 _position.Z = lpos.Z;
@@ -2704,7 +2709,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2704 { 2709 {
2705 if (Amotor != IntPtr.Zero) 2710 if (Amotor != IntPtr.Zero)
2706 { 2711 {
2707 d.JointDestroy(Amotor); 2712 SafeNativeMethods.JointDestroy(Amotor);
2708 Amotor = IntPtr.Zero; 2713 Amotor = IntPtr.Zero;
2709 } 2714 }
2710 } 2715 }
@@ -2761,10 +2766,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2761 2766
2762 if (Body != IntPtr.Zero) 2767 if (Body != IntPtr.Zero)
2763 { 2768 {
2764 d.BodySetForce(Body, 0f, 0f, 0f); 2769 SafeNativeMethods.BodySetForce(Body, 0f, 0f, 0f);
2765 d.BodySetTorque(Body, 0f, 0f, 0f); 2770 SafeNativeMethods.BodySetTorque(Body, 0f, 0f, 0f);
2766 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2771 SafeNativeMethods.BodySetLinearVel(Body, 0f, 0f, 0f);
2767 d.BodySetAngularVel(Body, 0f, 0f, 0f); 2772 SafeNativeMethods.BodySetAngularVel(Body, 0f, 0f, 0f);
2768 } 2773 }
2769 } 2774 }
2770 2775
@@ -2826,7 +2831,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2826 if (newval) 2831 if (newval)
2827 { 2832 {
2828 if (!childPrim && Body != IntPtr.Zero) 2833 if (!childPrim && Body != IntPtr.Zero)
2829 d.BodyDisable(Body); 2834 SafeNativeMethods.BodyDisable(Body);
2830 2835
2831 if (m_delaySelect || m_isphysical) 2836 if (m_delaySelect || m_isphysical)
2832 { 2837 {
@@ -2845,13 +2850,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2845 2850
2846 if (prm.m_NoColide) 2851 if (prm.m_NoColide)
2847 { 2852 {
2848 d.GeomSetCategoryBits(prm.prim_geom, 0); 2853 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
2849 d.GeomSetCollideBits(prm.prim_geom, 0); 2854 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
2850 } 2855 }
2851 else 2856 else
2852 { 2857 {
2853 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); 2858 SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2854 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); 2859 SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2855 } 2860 }
2856 } 2861 }
2857 prm.m_delaySelect = false; 2862 prm.m_delaySelect = false;
@@ -2865,23 +2870,23 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2865 { 2870 {
2866 if (m_NoColide) 2871 if (m_NoColide)
2867 { 2872 {
2868 d.GeomSetCategoryBits(prim_geom, 0); 2873 SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
2869 d.GeomSetCollideBits(prim_geom, 0); 2874 SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
2870 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2875 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2871 { 2876 {
2872 d.GeomSetCategoryBits(collide_geom, 0); 2877 SafeNativeMethods.GeomSetCategoryBits(collide_geom, 0);
2873 d.GeomSetCollideBits(collide_geom, 0); 2878 SafeNativeMethods.GeomSetCollideBits(collide_geom, 0);
2874 } 2879 }
2875 2880
2876 } 2881 }
2877 else 2882 else
2878 { 2883 {
2879 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); 2884 SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2880 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); 2885 SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2881 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) 2886 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2882 { 2887 {
2883 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); 2888 SafeNativeMethods.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
2884 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); 2889 SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2885 } 2890 }
2886 } 2891 }
2887 } 2892 }
@@ -2900,8 +2905,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2900 if (Body != IntPtr.Zero && !m_disabled) 2905 if (Body != IntPtr.Zero && !m_disabled)
2901 { 2906 {
2902 _zeroFlag = true; 2907 _zeroFlag = true;
2903 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 2908 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
2904 d.BodyEnable(Body); 2909 SafeNativeMethods.BodyEnable(Body);
2905 } 2910 }
2906 } 2911 }
2907// else if (_parent != null) 2912// else if (_parent != null)
@@ -2931,11 +2936,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2931 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) 2936 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2932 { 2937 {
2933 FixInertia(newPos); 2938 FixInertia(newPos);
2934 if (!d.BodyIsEnabled(Body)) 2939 if (!SafeNativeMethods.BodyIsEnabled(Body))
2935 { 2940 {
2936 _zeroFlag = true; 2941 _zeroFlag = true;
2937 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 2942 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
2938 d.BodyEnable(Body); 2943 SafeNativeMethods.BodyEnable(Body);
2939 } 2944 }
2940 } 2945 }
2941 } 2946 }
@@ -2943,14 +2948,14 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2943 { 2948 {
2944 if (_position != newPos) 2949 if (_position != newPos)
2945 { 2950 {
2946 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 2951 SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2947 _position = newPos; 2952 _position = newPos;
2948 } 2953 }
2949 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 2954 if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
2950 { 2955 {
2951 _zeroFlag = true; 2956 _zeroFlag = true;
2952 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 2957 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
2953 d.BodyEnable(Body); 2958 SafeNativeMethods.BodyEnable(Body);
2954 } 2959 }
2955 } 2960 }
2956 } 2961 }
@@ -2960,7 +2965,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2960 { 2965 {
2961 if (newPos != _position) 2966 if (newPos != _position)
2962 { 2967 {
2963 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 2968 SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2964 _position = newPos; 2969 _position = newPos;
2965 2970
2966 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace); 2971 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace);
@@ -2998,12 +3003,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2998 { 3003 {
2999 if (newOri != _orientation) 3004 if (newOri != _orientation)
3000 { 3005 {
3001 d.Quaternion myrot = new d.Quaternion(); 3006 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3002 myrot.X = newOri.X; 3007 myrot.X = newOri.X;
3003 myrot.Y = newOri.Y; 3008 myrot.Y = newOri.Y;
3004 myrot.Z = newOri.Z; 3009 myrot.Z = newOri.Z;
3005 myrot.W = newOri.W; 3010 myrot.W = newOri.W;
3006 d.GeomSetQuaternion(prim_geom, ref myrot); 3011 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3007 _orientation = newOri; 3012 _orientation = newOri;
3008 3013
3009 if (Body != IntPtr.Zero) 3014 if (Body != IntPtr.Zero)
@@ -3012,11 +3017,11 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3012 createAMotor(m_angularlocks); 3017 createAMotor(m_angularlocks);
3013 } 3018 }
3014 } 3019 }
3015 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 3020 if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
3016 { 3021 {
3017 _zeroFlag = true; 3022 _zeroFlag = true;
3018 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3023 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3019 d.BodyEnable(Body); 3024 SafeNativeMethods.BodyEnable(Body);
3020 } 3025 }
3021 } 3026 }
3022 } 3027 }
@@ -3026,12 +3031,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3026 { 3031 {
3027 if (newOri != _orientation) 3032 if (newOri != _orientation)
3028 { 3033 {
3029 d.Quaternion myrot = new d.Quaternion(); 3034 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3030 myrot.X = newOri.X; 3035 myrot.X = newOri.X;
3031 myrot.Y = newOri.Y; 3036 myrot.Y = newOri.Y;
3032 myrot.Z = newOri.Z; 3037 myrot.Z = newOri.Z;
3033 myrot.W = newOri.W; 3038 myrot.W = newOri.W;
3034 d.GeomSetQuaternion(prim_geom, ref myrot); 3039 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3035 _orientation = newOri; 3040 _orientation = newOri;
3036 } 3041 }
3037 } 3042 }
@@ -3056,26 +3061,26 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3056 { 3061 {
3057 if (newOri != _orientation) 3062 if (newOri != _orientation)
3058 { 3063 {
3059 d.Quaternion myrot = new d.Quaternion(); 3064 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3060 myrot.X = newOri.X; 3065 myrot.X = newOri.X;
3061 myrot.Y = newOri.Y; 3066 myrot.Y = newOri.Y;
3062 myrot.Z = newOri.Z; 3067 myrot.Z = newOri.Z;
3063 myrot.W = newOri.W; 3068 myrot.W = newOri.W;
3064 d.GeomSetQuaternion(prim_geom, ref myrot); 3069 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3065 _orientation = newOri; 3070 _orientation = newOri;
3066 if (Body != IntPtr.Zero && m_angularlocks != 0) 3071 if (Body != IntPtr.Zero && m_angularlocks != 0)
3067 createAMotor(m_angularlocks); 3072 createAMotor(m_angularlocks);
3068 } 3073 }
3069 if (_position != newPos) 3074 if (_position != newPos)
3070 { 3075 {
3071 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 3076 SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3072 _position = newPos; 3077 _position = newPos;
3073 } 3078 }
3074 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 3079 if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
3075 { 3080 {
3076 _zeroFlag = true; 3081 _zeroFlag = true;
3077 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3082 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3078 d.BodyEnable(Body); 3083 SafeNativeMethods.BodyEnable(Body);
3079 } 3084 }
3080 } 3085 }
3081 } 3086 }
@@ -3088,18 +3093,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3088 { 3093 {
3089 if (newOri != _orientation) 3094 if (newOri != _orientation)
3090 { 3095 {
3091 d.Quaternion myrot = new d.Quaternion(); 3096 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3092 myrot.X = newOri.X; 3097 myrot.X = newOri.X;
3093 myrot.Y = newOri.Y; 3098 myrot.Y = newOri.Y;
3094 myrot.Z = newOri.Z; 3099 myrot.Z = newOri.Z;
3095 myrot.W = newOri.W; 3100 myrot.W = newOri.W;
3096 d.GeomSetQuaternion(prim_geom, ref myrot); 3101 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3097 _orientation = newOri; 3102 _orientation = newOri;
3098 } 3103 }
3099 3104
3100 if (newPos != _position) 3105 if (newPos != _position)
3101 { 3106 {
3102 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 3107 SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
3103 _position = newPos; 3108 _position = newPos;
3104 3109
3105 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace); 3110 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace);
@@ -3183,13 +3188,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3183 3188
3184 if (prim_geom != IntPtr.Zero) 3189 if (prim_geom != IntPtr.Zero)
3185 { 3190 {
3186 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 3191 SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3187 d.Quaternion myrot = new d.Quaternion(); 3192 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3188 myrot.X = _orientation.X; 3193 myrot.X = _orientation.X;
3189 myrot.Y = _orientation.Y; 3194 myrot.Y = _orientation.Y;
3190 myrot.Z = _orientation.Z; 3195 myrot.Z = _orientation.Z;
3191 myrot.W = _orientation.W; 3196 myrot.W = _orientation.W;
3192 d.GeomSetQuaternion(prim_geom, ref myrot); 3197 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3193 } 3198 }
3194 3199
3195 if (!m_isphysical) 3200 if (!m_isphysical)
@@ -3259,13 +3264,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3259 3264
3260 if (prim_geom != IntPtr.Zero) 3265 if (prim_geom != IntPtr.Zero)
3261 { 3266 {
3262 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 3267 SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3263 d.Quaternion myrot = new d.Quaternion(); 3268 SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
3264 myrot.X = _orientation.X; 3269 myrot.X = _orientation.X;
3265 myrot.Y = _orientation.Y; 3270 myrot.Y = _orientation.Y;
3266 myrot.Z = _orientation.Z; 3271 myrot.Z = _orientation.Z;
3267 myrot.W = _orientation.W; 3272 myrot.W = _orientation.W;
3268 d.GeomSetQuaternion(prim_geom, ref myrot); 3273 SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
3269 } 3274 }
3270 3275
3271 if (m_isphysical) 3276 if (m_isphysical)
@@ -3316,10 +3321,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3316 { 3321 {
3317 if (m_disabled) 3322 if (m_disabled)
3318 enableBodySoft(); 3323 enableBodySoft();
3319 else if (!d.BodyIsEnabled(Body)) 3324 else if (!SafeNativeMethods.BodyIsEnabled(Body))
3320 { 3325 {
3321 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3326 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3322 d.BodyEnable(Body); 3327 SafeNativeMethods.BodyEnable(Body);
3323 } 3328 }
3324 } 3329 }
3325 m_torque = newtorque; 3330 m_torque = newtorque;
@@ -3329,10 +3334,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3329 private void changeForce(Vector3 force) 3334 private void changeForce(Vector3 force)
3330 { 3335 {
3331 m_force = force; 3336 m_force = force;
3332 if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 3337 if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
3333 { 3338 {
3334 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3339 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3335 d.BodyEnable(Body); 3340 SafeNativeMethods.BodyEnable(Body);
3336 } 3341 }
3337 } 3342 }
3338 3343
@@ -3348,10 +3353,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3348 { 3353 {
3349 if (m_disabled) 3354 if (m_disabled)
3350 enableBodySoft(); 3355 enableBodySoft();
3351 else if (!d.BodyIsEnabled(Body)) 3356 else if (!SafeNativeMethods.BodyIsEnabled(Body))
3352 { 3357 {
3353 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3358 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3354 d.BodyEnable(Body); 3359 SafeNativeMethods.BodyEnable(Body);
3355 } 3360 }
3356 } 3361 }
3357 } 3362 }
@@ -3371,10 +3376,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3371 { 3376 {
3372 if (m_disabled) 3377 if (m_disabled)
3373 enableBodySoft(); 3378 enableBodySoft();
3374 else if (!d.BodyIsEnabled(Body)) 3379 else if (!SafeNativeMethods.BodyIsEnabled(Body))
3375 { 3380 {
3376 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3381 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3377 d.BodyEnable(Body); 3382 SafeNativeMethods.BodyEnable(Body);
3378 } 3383 }
3379 } 3384 }
3380 } 3385 }
@@ -3397,12 +3402,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3397 { 3402 {
3398 if (m_disabled) 3403 if (m_disabled)
3399 enableBodySoft(); 3404 enableBodySoft();
3400 else if (!d.BodyIsEnabled(Body)) 3405 else if (!SafeNativeMethods.BodyIsEnabled(Body))
3401 { 3406 {
3402 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3407 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3403 d.BodyEnable(Body); 3408 SafeNativeMethods.BodyEnable(Body);
3404 } 3409 }
3405 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); 3410 SafeNativeMethods.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3406 } 3411 }
3407 //resetCollisionAccounting(); 3412 //resetCollisionAccounting();
3408 } 3413 }
@@ -3424,12 +3429,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3424 { 3429 {
3425 if (m_disabled) 3430 if (m_disabled)
3426 enableBodySoft(); 3431 enableBodySoft();
3427 else if (!d.BodyIsEnabled(Body)) 3432 else if (!SafeNativeMethods.BodyIsEnabled(Body))
3428 { 3433 {
3429 d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); 3434 SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
3430 d.BodyEnable(Body); 3435 SafeNativeMethods.BodyEnable(Body);
3431 } 3436 }
3432 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); 3437 SafeNativeMethods.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
3433 } 3438 }
3434 //resetCollisionAccounting(); 3439 //resetCollisionAccounting();
3435 } 3440 }
@@ -3580,7 +3585,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3580 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 3585 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3581 !m_disabled && !m_isSelected && !m_building && !m_outbounds) 3586 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3582 { 3587 {
3583 if (!d.BodyIsEnabled(Body)) 3588 if (!SafeNativeMethods.BodyIsEnabled(Body))
3584 { 3589 {
3585 // let vehicles sleep 3590 // let vehicles sleep
3586 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3591 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
@@ -3590,18 +3595,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3590 return; 3595 return;
3591 3596
3592 // clear residuals 3597 // clear residuals
3593 d.BodySetAngularVel(Body,0f,0f,0f); 3598 SafeNativeMethods.BodySetAngularVel(Body,0f,0f,0f);
3594 d.BodySetLinearVel(Body,0f,0f,0f); 3599 SafeNativeMethods.BodySetLinearVel(Body,0f,0f,0f);
3595 _zeroFlag = true; 3600 _zeroFlag = true;
3596 d.BodySetAutoDisableSteps(Body, 1); 3601 SafeNativeMethods.BodySetAutoDisableSteps(Body, 1);
3597 d.BodyEnable(Body); 3602 SafeNativeMethods.BodyEnable(Body);
3598 m_bodydisablecontrol = -3; 3603 m_bodydisablecontrol = -3;
3599 } 3604 }
3600 3605
3601 if(m_bodydisablecontrol < 0) 3606 if(m_bodydisablecontrol < 0)
3602 m_bodydisablecontrol++; 3607 m_bodydisablecontrol++;
3603 3608
3604 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator 3609 SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3605 3610
3606 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3611 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3607 { 3612 {
@@ -3628,8 +3633,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3628 3633
3629 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) 3634 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3630 { 3635 {
3631 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 3636 SafeNativeMethods.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3632 d.BodySetLinearVel(Body, 0, 0, 0); 3637 SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0);
3633 return; 3638 return;
3634 } 3639 }
3635 else 3640 else
@@ -3652,7 +3657,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3652 _target_velocity *= tmp; 3657 _target_velocity *= tmp;
3653 } 3658 }
3654 3659
3655 d.Vector3 vel = d.BodyGetLinearVel(Body); 3660 SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
3656 fx = (_target_velocity.X - vel.X) * m_invTimeStep; 3661 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3657 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; 3662 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3658 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; 3663 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
@@ -3688,15 +3693,15 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3688 3693
3689 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) 3694 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3690 { 3695 {
3691 d.Vector3 vel = d.BodyGetLinearVel(Body); 3696 SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
3692 3697
3693 fz = (m_targetHoverHeight - lpos.Z); 3698 fz = (m_targetHoverHeight - lpos.Z);
3694 3699
3695 // if error is zero, use position control; otherwise, velocity control 3700 // if error is zero, use position control; otherwise, velocity control
3696 if (Math.Abs(fz) < 0.01f) 3701 if (Math.Abs(fz) < 0.01f)
3697 { 3702 {
3698 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); 3703 SafeNativeMethods.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3699 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 3704 SafeNativeMethods.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3700 } 3705 }
3701 else 3706 else
3702 { 3707 {
@@ -3739,7 +3744,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3739 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 3744 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3740 if (fx != 0 || fy != 0 || fz != 0) 3745 if (fx != 0 || fy != 0 || fz != 0)
3741 { 3746 {
3742 d.BodyAddForce(Body, fx, fy, fz); 3747 SafeNativeMethods.BodyAddForce(Body, fx, fy, fz);
3743 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3748 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3744 } 3749 }
3745 3750
@@ -3750,7 +3755,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3750 m_angularForceacc = Vector3.Zero; 3755 m_angularForceacc = Vector3.Zero;
3751 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) 3756 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3752 { 3757 {
3753 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); 3758 SafeNativeMethods.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3754 } 3759 }
3755 } 3760 }
3756 else 3761 else
@@ -3769,12 +3774,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3769 if(m_bodydisablecontrol < 0) 3774 if(m_bodydisablecontrol < 0)
3770 return; 3775 return;
3771 3776
3772 bool bodyenabled = d.BodyIsEnabled(Body); 3777 bool bodyenabled = SafeNativeMethods.BodyIsEnabled(Body);
3773 if (bodyenabled || !_zeroFlag) 3778 if (bodyenabled || !_zeroFlag)
3774 { 3779 {
3775 bool lastZeroFlag = _zeroFlag; 3780 bool lastZeroFlag = _zeroFlag;
3776 3781
3777 d.Vector3 lpos = d.GeomGetPosition(prim_geom); 3782 SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom);
3778 3783
3779 // check outside region 3784 // check outside region
3780 if (lpos.Z < -100 || lpos.Z > 100000f) 3785 if (lpos.Z < -100 || lpos.Z > 100000f)
@@ -3793,9 +3798,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3793 m_rotationalVelocity.Y = 0; 3798 m_rotationalVelocity.Y = 0;
3794 m_rotationalVelocity.Z = 0; 3799 m_rotationalVelocity.Z = 0;
3795 3800
3796 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 3801 SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
3797 d.BodySetAngularVel(Body, 0, 0, 0); // stop it 3802 SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0); // stop it
3798 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere 3803 SafeNativeMethods.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3799 m_lastposition = _position; 3804 m_lastposition = _position;
3800 m_lastorientation = _orientation; 3805 m_lastorientation = _orientation;
3801 3806
@@ -3835,19 +3840,19 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3835 m_lastposition = _position; 3840 m_lastposition = _position;
3836 m_lastorientation = _orientation; 3841 m_lastorientation = _orientation;
3837 3842
3838 d.Vector3 dtmp = d.BodyGetAngularVel(Body); 3843 SafeNativeMethods.Vector3 dtmp = SafeNativeMethods.BodyGetAngularVel(Body);
3839 m_rotationalVelocity.X = dtmp.X; 3844 m_rotationalVelocity.X = dtmp.X;
3840 m_rotationalVelocity.Y = dtmp.Y; 3845 m_rotationalVelocity.Y = dtmp.Y;
3841 m_rotationalVelocity.Z = dtmp.Z; 3846 m_rotationalVelocity.Z = dtmp.Z;
3842 3847
3843 dtmp = d.BodyGetLinearVel(Body); 3848 dtmp = SafeNativeMethods.BodyGetLinearVel(Body);
3844 _velocity.X = dtmp.X; 3849 _velocity.X = dtmp.X;
3845 _velocity.Y = dtmp.Y; 3850 _velocity.Y = dtmp.Y;
3846 _velocity.Z = dtmp.Z; 3851 _velocity.Z = dtmp.Z;
3847 3852
3848 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 3853 SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
3849 d.BodySetAngularVel(Body, 0, 0, 0); 3854 SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
3850 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 3855 SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3851 disableBodySoft(); // stop collisions 3856 disableBodySoft(); // stop collisions
3852 UnSubscribeEvents(); 3857 UnSubscribeEvents();
3853 3858
@@ -3855,8 +3860,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3855 return; 3860 return;
3856 } 3861 }
3857 3862
3858 d.Quaternion ori; 3863 SafeNativeMethods.Quaternion ori;
3859 d.GeomCopyQuaternion(prim_geom, out ori); 3864 SafeNativeMethods.GeomCopyQuaternion(prim_geom, out ori);
3860 3865
3861 // decide if moving 3866 // decide if moving
3862 // use positions since this are integrated quantities 3867 // use positions since this are integrated quantities
@@ -3917,7 +3922,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3917 } 3922 }
3918 else 3923 else
3919 { 3924 {
3920 d.Vector3 vel = d.BodyGetLinearVel(Body); 3925 SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
3921 3926
3922 m_acceleration = _velocity; 3927 m_acceleration = _velocity;
3923 3928
@@ -3944,7 +3949,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3944 m_acceleration = Vector3.Zero; 3949 m_acceleration = Vector3.Zero;
3945 } 3950 }
3946 3951
3947 vel = d.BodyGetAngularVel(Body); 3952 vel = SafeNativeMethods.BodyGetAngularVel(Body);
3948 if ((Math.Abs(vel.X) < 0.0001) && 3953 if ((Math.Abs(vel.X) < 0.0001) &&
3949 (Math.Abs(vel.Y) < 0.0001) && 3954 (Math.Abs(vel.Y) < 0.0001) &&
3950 (Math.Abs(vel.Z) < 0.0001) 3955 (Math.Abs(vel.Z) < 0.0001)
@@ -3990,7 +3995,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
3990 return true; 3995 return true;
3991 } 3996 }
3992 3997
3993 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) 3998 internal static void DMassSubPartFromObj(ref SafeNativeMethods.Mass part, ref SafeNativeMethods.Mass theobj)
3994 { 3999 {
3995 // assumes object center of mass is zero 4000 // assumes object center of mass is zero
3996 float smass = part.mass; 4001 float smass = part.mass;
generated by cgit v1.1 at 2024-12-23 04:57:19 +0000