aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs216
1 files changed, 126 insertions, 90 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 16e90ff..8402082 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -85,6 +85,7 @@ namespace OpenSim.Region.Physics.OdePlugin
85 private float m_APIDStrength = 0.5f; 85 private float m_APIDStrength = 0.5f;
86 private float m_APIDDamping = 0.5f; 86 private float m_APIDDamping = 0.5f;
87 private bool m_useAPID = false; 87 private bool m_useAPID = false;
88 private float m_APIDdamper = 1.0f;
88 89
89 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 90 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
90 // do not confuse with VEHICLE HOVER 91 // do not confuse with VEHICLE HOVER
@@ -735,12 +736,7 @@ namespace OpenSim.Region.Physics.OdePlugin
735 public override float Buoyancy 736 public override float Buoyancy
736 { 737 {
737 get { return m_buoyancy; } 738 get { return m_buoyancy; }
738// set { m_buoyancy = value; } 739 set { m_buoyancy = value; }
739 set {
740 m_buoyancy = value;
741
742 Console.WriteLine("m_buoyancy={0}", m_buoyancy);
743 }
744 } 740 }
745 741
746 public override void link(PhysicsActor obj) 742 public override void link(PhysicsActor obj)
@@ -1293,7 +1289,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1293 disableBody(); 1289 disableBody();
1294 } 1290 }
1295 } 1291 }
1296
1297 IntPtr vertices, indices; 1292 IntPtr vertices, indices;
1298 int vertexCount, indexCount; 1293 int vertexCount, indexCount;
1299 int vertexStride, triStride; 1294 int vertexStride, triStride;
@@ -1349,8 +1344,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1349 if (prim_geom != IntPtr.Zero) 1344 if (prim_geom != IntPtr.Zero)
1350 { 1345 {
1351 if (!_position.ApproxEquals(m_taintposition, 0f)) 1346 if (!_position.ApproxEquals(m_taintposition, 0f))
1347 {
1352 changemove(timestep); 1348 changemove(timestep);
1353 1349 }
1354 if (m_taintrot != _orientation) 1350 if (m_taintrot != _orientation)
1355 { 1351 {
1356 if(childPrim && IsPhysical) // For physical child prim... 1352 if(childPrim && IsPhysical) // For physical child prim...
@@ -1370,8 +1366,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1370 // 1366 //
1371 1367
1372 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 1368 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
1369 {
1373 changePhysicsStatus(timestep); 1370 changePhysicsStatus(timestep);
1374 // 1371 }//
1375 1372
1376 if (!_size.ApproxEquals(m_taintsize,0f)) 1373 if (!_size.ApproxEquals(m_taintsize,0f))
1377 changesize(timestep); 1374 changesize(timestep);
@@ -1482,6 +1479,9 @@ namespace OpenSim.Region.Physics.OdePlugin
1482 if (Body == IntPtr.Zero) 1479 if (Body == IntPtr.Zero)
1483 { 1480 {
1484 Body = d.BodyCreate(_parent_scene.world); 1481 Body = d.BodyCreate(_parent_scene.world);
1482 // disconnect from world gravity so we can apply buoyancy
1483 d.BodySetGravityMode (Body, false);
1484
1485 setMass(); 1485 setMass();
1486 } 1486 }
1487 if (Body != IntPtr.Zero) 1487 if (Body != IntPtr.Zero)
@@ -1522,7 +1522,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1522 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1522 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1523 continue; 1523 continue;
1524 } 1524 }
1525//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1526 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1525 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1527 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1526 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1528 1527
@@ -1565,9 +1564,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1565 m_collisionCategories |= CollisionCategories.Body; 1564 m_collisionCategories |= CollisionCategories.Body;
1566 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1565 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1567 1566
1568//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1569 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1567 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1570//Console.WriteLine(" Post GeomSetCategoryBits 2");
1571 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1568 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1572 1569
1573 1570
@@ -1721,14 +1718,15 @@ namespace OpenSim.Region.Physics.OdePlugin
1721 if (m_isphysical) 1718 if (m_isphysical)
1722 { 1719 {
1723 disableBodySoft(); 1720 disableBodySoft();
1724
1725 if (Body != IntPtr.Zero)
1726 {
1727 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1728 d.BodySetForce(Body, 0, 0, 0);
1729 enableBodySoft();
1730 }
1731 } 1721 }
1722 if (Body != IntPtr.Zero)
1723 {
1724 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1725 d.BodySetForce(Body, 0f, 0f, 0f);
1726 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1727 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1728 }
1729
1732 } 1730 }
1733 else 1731 else
1734 { 1732 {
@@ -1749,17 +1747,21 @@ namespace OpenSim.Region.Physics.OdePlugin
1749 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1747 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1750 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1748 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1751 } 1749 }
1752/* Uhhh - stop the motion if the object is _selected_!! 1750 if (Body != IntPtr.Zero)
1751 {
1752 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1753 d.BodySetForce(Body, 0f, 0f, 0f);
1754 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
1755 d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f);
1756 }
1757
1753 if (m_isphysical) 1758 if (m_isphysical)
1754 { 1759 {
1755 if (Body != IntPtr.Zero) 1760 if (Body != IntPtr.Zero)
1756 { 1761 {
1757 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1758 d.BodySetForce(Body, 0, 0, 0);
1759 enableBodySoft(); 1762 enableBodySoft();
1760 } 1763 }
1761 } 1764 }
1762*/
1763 } 1765 }
1764 1766
1765 resetCollisionAccounting(); 1767 resetCollisionAccounting();
@@ -1781,7 +1783,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1781 1783
1782 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1784 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1783 { 1785 {
1784//Console.WriteLine("CreateGeom:");
1785 if (_mesh != null) // Special - make mesh 1786 if (_mesh != null) // Special - make mesh
1786 { 1787 {
1787 setMesh(_parent_scene, _mesh); 1788 setMesh(_parent_scene, _mesh);
@@ -1797,7 +1798,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1797 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1798 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1798 try 1799 try
1799 { 1800 {
1800//Console.WriteLine(" CreateGeom 1");
1801 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); 1801 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1802 } 1802 }
1803 catch (AccessViolationException) 1803 catch (AccessViolationException)
@@ -1812,7 +1812,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1812 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1812 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1813 try 1813 try
1814 { 1814 {
1815//Console.WriteLine(" CreateGeom 2");
1816 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1815 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1817 } 1816 }
1818 catch (AccessViolationException) 1817 catch (AccessViolationException)
@@ -1828,7 +1827,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1828 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1827 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1829 try 1828 try
1830 { 1829 {
1831//Console.WriteLine(" CreateGeom 3");
1832 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1830 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1833 } 1831 }
1834 catch (AccessViolationException) 1832 catch (AccessViolationException)
@@ -1845,7 +1843,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1845 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1843 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1846 try 1844 try
1847 { 1845 {
1848//Console.WriteLine(" CreateGeom 4");
1849 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1846 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1850 } 1847 }
1851 catch (AccessViolationException) 1848 catch (AccessViolationException)
@@ -1890,7 +1887,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1890 1887
1891 lock (_parent_scene.OdeLock) 1888 lock (_parent_scene.OdeLock)
1892 { 1889 {
1893//Console.WriteLine("changeadd 1");
1894 CreateGeom(m_targetSpace, _mesh); 1890 CreateGeom(m_targetSpace, _mesh);
1895 1891
1896 if (prim_geom != IntPtr.Zero) 1892 if (prim_geom != IntPtr.Zero)
@@ -1917,10 +1913,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1917 1913
1918 public void changemove(float timestep) 1914 public void changemove(float timestep)
1919 { 1915 {
1920//Console.WriteLine("changemove sing/root {0} to {1}", m_primName, _position );
1921 if (m_isphysical) 1916 if (m_isphysical)
1922 { 1917 {
1923//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1924// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! 1918// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1925 if (!m_taintremove && !childPrim) 1919 if (!m_taintremove && !childPrim)
1926 { 1920 {
@@ -1946,7 +1940,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1946 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1940 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1947 { 1941 {
1948// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 1942// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1949Console.WriteLine(" JointCreateFixed"); 1943Console.WriteLine("ODEPrim JointCreateFixed !!!");
1950 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1944 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1951 d.JointAttach(m_linkJoint, Body, odParent.Body); 1945 d.JointAttach(m_linkJoint, Body, odParent.Body);
1952 d.JointSetFixed(m_linkJoint); 1946 d.JointSetFixed(m_linkJoint);
@@ -2059,7 +2053,6 @@ Console.WriteLine(" JointCreateFixed");
2059 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) 2053 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
2060 { 2054 {
2061 _mesh = null; 2055 _mesh = null;
2062//Console.WriteLine("changePhysicsStatus for " + m_primName );
2063 changeadd(2f); 2056 changeadd(2f);
2064 } 2057 }
2065 if (childPrim) 2058 if (childPrim)
@@ -2142,7 +2135,6 @@ Console.WriteLine(" JointCreateFixed");
2142 } 2135 }
2143 2136
2144 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 2137 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
2145//Console.WriteLine("changesize 1");
2146 CreateGeom(m_targetSpace, mesh); 2138 CreateGeom(m_targetSpace, mesh);
2147 2139
2148 2140
@@ -2150,7 +2142,6 @@ Console.WriteLine(" JointCreateFixed");
2150 else 2142 else
2151 { 2143 {
2152 _mesh = null; 2144 _mesh = null;
2153//Console.WriteLine("changesize 2");
2154 CreateGeom(m_targetSpace, _mesh); 2145 CreateGeom(m_targetSpace, _mesh);
2155 } 2146 }
2156 2147
@@ -2255,7 +2246,6 @@ Console.WriteLine(" JointCreateFixed");
2255 else 2246 else
2256 { 2247 {
2257 _mesh = null; 2248 _mesh = null;
2258//Console.WriteLine("changeshape");
2259 CreateGeom(m_targetSpace, null); 2249 CreateGeom(m_targetSpace, null);
2260 } 2250 }
2261 2251
@@ -2330,6 +2320,7 @@ Console.WriteLine(" JointCreateFixed");
2330 return; 2320 return;
2331 } 2321 }
2332 d.BodyEnable(Body); 2322 d.BodyEnable(Body);
2323
2333 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); 2324 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2334 } 2325 }
2335 m_forcelist.Clear(); 2326 m_forcelist.Clear();
@@ -3107,8 +3098,6 @@ Console.WriteLine(" JointCreateFixed");
3107 else 3098 else
3108 { // Too many tries 3099 { // Too many tries
3109 if (_parent == null) base.RaiseOutOfBounds(l_position); 3100 if (_parent == null) base.RaiseOutOfBounds(l_position);
3110//Console.WriteLine("ROOB 2");
3111
3112 return; // Dont process any other motion? 3101 return; // Dont process any other motion?
3113 } // end various methods 3102 } // end various methods
3114 } // end outside region horizontally 3103 } // end outside region horizontally
@@ -3124,7 +3113,6 @@ Console.WriteLine(" JointCreateFixed");
3124 3113
3125 //IsPhysical = false; 3114 //IsPhysical = false;
3126 if (_parent == null) base.RaiseOutOfBounds(_position); 3115 if (_parent == null) base.RaiseOutOfBounds(_position);
3127//Console.WriteLine("ROOB 3");
3128 3116
3129 3117
3130 _acceleration.X = 0; // This stuff may stop client display but it has no 3118 _acceleration.X = 0; // This stuff may stop client display but it has no
@@ -3147,10 +3135,12 @@ Console.WriteLine(" JointCreateFixed");
3147 } // end neg Z check 3135 } // end neg Z check
3148 3136
3149 // Is it moving? 3137 // Is it moving?
3150 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 3138 /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3151 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 3139 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3152 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 3140 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */
3153 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // KF 0.01 is far to large 3141 if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly
3142 (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating
3143 (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly
3154 { 3144 {
3155 _zeroFlag = true; 3145 _zeroFlag = true;
3156 m_throttleUpdates = false; 3146 m_throttleUpdates = false;
@@ -3167,15 +3157,19 @@ Console.WriteLine(" JointCreateFixed");
3167 { // Its stopped 3157 { // Its stopped
3168 _velocity.X = 0.0f; 3158 _velocity.X = 0.0f;
3169 _velocity.Y = 0.0f; 3159 _velocity.Y = 0.0f;
3170 _velocity.Z = 0.0f; 3160 // _velocity.Z = 0.0f;
3171 3161
3172 _acceleration.X = 0; 3162 _acceleration.X = 0;
3173 _acceleration.Y = 0; 3163 _acceleration.Y = 0;
3174 _acceleration.Z = 0; 3164 // _acceleration.Z = 0;
3175 3165
3176 m_rotationalVelocity.X = 0; 3166 m_rotationalVelocity.X = 0;
3177 m_rotationalVelocity.Y = 0; 3167 m_rotationalVelocity.Y = 0;
3178 m_rotationalVelocity.Z = 0; 3168 m_rotationalVelocity.Z = 0;
3169 // Stop it in the phys engine
3170 d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z);
3171 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3172
3179 if (!m_lastUpdateSent) 3173 if (!m_lastUpdateSent)
3180 { 3174 {
3181 m_throttleUpdates = false; 3175 m_throttleUpdates = false;
@@ -3221,13 +3215,13 @@ Console.WriteLine(" JointCreateFixed");
3221 // Snapshot current angles, set up Amotor(s) 3215 // Snapshot current angles, set up Amotor(s)
3222 m_rotateEnableUpdate = false; 3216 m_rotateEnableUpdate = false;
3223 m_rotateEnable = m_rotateEnableRequest; 3217 m_rotateEnable = m_rotateEnableRequest;
3224Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); 3218//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable);
3225 3219
3226 if (Amotor != IntPtr.Zero) 3220 if (Amotor != IntPtr.Zero)
3227 { 3221 {
3228 d.JointDestroy(Amotor); 3222 d.JointDestroy(Amotor);
3229 Amotor = IntPtr.Zero; 3223 Amotor = IntPtr.Zero;
3230Console.WriteLine("Old Amotor Destroyed"); 3224//Console.WriteLine("Old Amotor Destroyed");
3231 } 3225 }
3232 3226
3233 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) 3227 if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f))
@@ -3249,33 +3243,33 @@ Console.WriteLine("Old Amotor Destroyed");
3249 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 3243 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3250 d.JointAttach(Amotor, Body, IntPtr.Zero); 3244 d.JointAttach(Amotor, Body, IntPtr.Zero);
3251 d.JointSetAMotorMode(Amotor, 0); // User mode?? 3245 d.JointSetAMotorMode(Amotor, 0); // User mode??
3252Console.WriteLine("New Amotor Created for {0}", m_primName); 3246//Console.WriteLine("New Amotor Created for {0}", m_primName);
3253 3247
3254 float axisnum = 3; // how many to lock 3248 float axisnum = 3; // how many to lock
3255 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); 3249 axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z));
3256 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 3250 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3257Console.WriteLine("AxisNum={0}",(int)axisnum); 3251//Console.WriteLine("AxisNum={0}",(int)axisnum);
3258 3252
3259 int i = 0; 3253 int i = 0;
3260 3254
3261 if (m_rotateEnable.X == 0) 3255 if (m_rotateEnable.X == 0)
3262 { 3256 {
3263 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); 3257 d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z);
3264Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); 3258//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX);
3265 i++; 3259 i++;
3266 } 3260 }
3267 3261
3268 if (m_rotateEnable.Y == 0) 3262 if (m_rotateEnable.Y == 0)
3269 { 3263 {
3270 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); 3264 d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z);
3271Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); 3265//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY);
3272 i++; 3266 i++;
3273 } 3267 }
3274 3268
3275 if (m_rotateEnable.Z == 0) 3269 if (m_rotateEnable.Z == 0)
3276 { 3270 {
3277 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); 3271 d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z);
3278Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); 3272//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3279 i++; 3273 i++;
3280 } 3274 }
3281 3275
@@ -3308,7 +3302,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3308 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame 3302 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3309 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); 3303 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3310 m_lLinObjectVel = vel_now * irotq; 3304 m_lLinObjectVel = vel_now * irotq;
3311
3312 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate 3305 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3313 { 3306 {
3314 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) 3307 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
@@ -3434,7 +3427,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3434 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); 3427 d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z);
3435 // apply gravity force 3428 // apply gravity force
3436 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); 3429 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3437//if(frcount == 0) Console.WriteLine("Grav {0}", grav); 3430//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav);
3438 // end MoveLinear() 3431 // end MoveLinear()
3439 3432
3440 3433
@@ -3608,7 +3601,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3608 float m_mass = CalculateMass(); 3601 float m_mass = CalculateMass();
3609 // calculate z-force due togravity on object. 3602 // calculate z-force due togravity on object.
3610 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass 3603 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3611
3612 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. 3604 if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget.
3613 { 3605 {
3614 fz = 0; // llMoveToTarget ignores gravity. 3606 fz = 0; // llMoveToTarget ignores gravity.
@@ -3624,6 +3616,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3624 (m_PIDTarget.Z - pos.Z)); 3616 (m_PIDTarget.Z - pos.Z));
3625 if (error.ApproxEquals(Vector3.Zero,0.01f)) 3617 if (error.ApproxEquals(Vector3.Zero,0.01f))
3626 { // Very close, Jump there and quit move 3618 { // Very close, Jump there and quit move
3619
3627 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 3620 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3628 _target_velocity = Vector3.Zero; 3621 _target_velocity = Vector3.Zero;
3629 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); 3622 d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
@@ -3714,11 +3707,10 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3714 // Avatar to Avatar collisions 3707 // Avatar to Avatar collisions
3715 // Prim to avatar collisions 3708 // Prim to avatar collisions
3716 d.Vector3 dlinvel = vel; 3709 d.Vector3 dlinvel = vel;
3717
3718 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3710 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3719 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); 3711 d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z);
3720 d.BodyAddForce(Body, 0, 0, fz); 3712 d.BodyAddForce(Body, 0, 0, fz);
3721 //KF this prevents furthur motions return; 3713 //KF this prevents furthur motions return;
3722 } 3714 }
3723 else 3715 else
3724 { 3716 {
@@ -3734,7 +3726,6 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3734 fx *= m_mass; 3726 fx *= m_mass;
3735 fy *= m_mass; 3727 fy *= m_mass;
3736 //fz *= m_mass; 3728 //fz *= m_mass;
3737
3738 fx += m_force.X; 3729 fx += m_force.X;
3739 fy += m_force.Y; 3730 fy += m_force.Y;
3740 fz += m_force.Z; 3731 fz += m_force.Z;
@@ -3751,7 +3742,7 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3751 // this appears to re-enable it incase the surface it is upon vanishes, 3742 // this appears to re-enable it incase the surface it is upon vanishes,
3752 // and the body should fall again. 3743 // and the body should fall again.
3753 d.BodySetLinearVel(Body, 0f, 0f, 0f); 3744 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3754 d.BodySetForce(Body, 0, 0, 0); 3745 d.BodySetForce(Body, 0f, 0f, 0f);
3755 enableBodySoft(); 3746 enableBodySoft();
3756 } 3747 }
3757 3748
@@ -3769,11 +3760,10 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3769 if (fy < nmin) 3760 if (fy < nmin)
3770 fy = nmin; 3761 fy = nmin;
3771 d.BodyAddForce(Body, fx, fy, fz); 3762 d.BodyAddForce(Body, fx, fy, fz);
3772//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3773 } // end apply forces 3763 } // end apply forces
3774 } // end Vehicle/Dynamics 3764 } // end Vehicle/Dynamics
3775 3765
3776 /// RotLookAt ================================================================================= 3766 /// RotLookAt / LookAt =================================================================================
3777 if (m_useAPID) 3767 if (m_useAPID)
3778 { 3768 {
3779 // RotLookAt, apparently overrides all other rotation sources. Inputs: 3769 // RotLookAt, apparently overrides all other rotation sources. Inputs:
@@ -3784,41 +3774,87 @@ Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ);
3784 // Factors: 3774 // Factors:
3785 // get present body rotation 3775 // get present body rotation
3786 float limit = 1.0f; 3776 float limit = 1.0f;
3787 float scaler = 50f; // adjusts damping time 3777 float rscaler = 50f; // adjusts rotation damping time
3778 float lscaler = 10f; // adjusts linear damping time in llLookAt
3788 float RLAservo = 0f; 3779 float RLAservo = 0f;
3789
3790 d.Quaternion rot = d.BodyGetQuaternion(Body);
3791 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3792 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3793 float diff_angle;
3794 Vector3 diff_axis; 3780 Vector3 diff_axis;
3795 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); 3781 float diff_angle;
3796 diff_axis.Normalize(); 3782 d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation
3797 if(diff_angle > 0.01f) // diff_angle is always +ve 3783 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3784 Quaternion rtarget = new Quaternion();
3785
3786 if(m_APIDTarget.W == -99.9f)
3798 { 3787 {
3799// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); 3788 // this is really a llLookAt(), x,y,z is the target vector
3800 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); 3789 Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z);
3801 rotforce = rotforce * rotq; 3790 Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq;
3802 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate 3791 Vector3 error = new Vector3(0.0f, 0.0f, 0.0f);
3803// RLAservo = timestep / m_APIDStrength * m_mass * scaler; 3792 float twopi = 2.0f * (float)Math.PI;
3804 // rotforce = rotforce * RLAservo * diff_angle ; 3793 Vector3 dir = target - _position;
3805 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); 3794 dir.Normalize();
3806 RLAservo = timestep / m_APIDStrength * scaler; 3795 float tzrot = (float)Math.Atan2(dir.Y, dir.X);
3807 rotforce = rotforce * RLAservo * diff_angle ; 3796 float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y));
3808 /* 3797 float terot = (float)Math.Atan2(dir.Z, txy);
3809 if (m_angularEnable.X == 0) 3798 float ozrot = (float)Math.Atan2(ospin.Y, ospin.X);
3810 rotforce.X = 0; 3799 float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y));
3811 if (m_angularEnable.Y == 0) 3800 float oerot = (float)Math.Atan2(ospin.Z, oxy);
3812 rotforce.Y = 0; 3801 float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z));
3813 if (m_angularEnable.Z == 0) 3802 float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X));
3814 rotforce.Z = 0; 3803 float roll = (float)Math.Atan2(ra, rb);
3815 */ 3804 float errorz = tzrot - ozrot;
3816 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); 3805 if(errorz > (float)Math.PI) errorz -= twopi;
3817//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); 3806 else if(errorz < -(float)Math.PI) errorz += twopi;
3807 float errory = oerot - terot;
3808 if(errory > (float)Math.PI) errory -= twopi;
3809 else if(errory < -(float)Math.PI) errory += twopi;
3810 diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll);
3811 if(diff_angle > 0.01f * m_APIDdamper)
3812 {
3813 m_APIDdamper = 1.0f;
3814 RLAservo = timestep / m_APIDStrength * rscaler;
3815 errorz *= RLAservo;
3816 errory *= RLAservo;
3817 error.X = -roll * 8.0f;
3818 error.Y = errory;
3819 error.Z = errorz;
3820 error *= rotq;
3821 d.BodySetAngularVel (Body, error.X, error.Y, error.Z);
3822 }
3823 else
3824 {
3825 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3826 m_APIDdamper = 2.0f;
3827 }
3818 } 3828 }
3819//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); 3829 else
3830 {
3831 // this is a llRotLookAt()
3832 rtarget = m_APIDTarget;
3833
3834 Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot
3835 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle
3836//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle);
3837
3838 // diff_axis.Normalize(); it already is!
3839 if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error
3840 {
3841 m_APIDdamper = 1.0f;
3842 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3843 rotforce = rotforce * rotq;
3844 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3845 RLAservo = timestep / m_APIDStrength * lscaler;
3846 rotforce = rotforce * RLAservo * diff_angle ;
3847 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3848//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3849 }
3850 else
3851 { // close enough
3852 d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f);
3853 m_APIDdamper = 2.0f;
3854 }
3855 } // end llLookAt/llRotLookAt
3856//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3820 } // end m_useAPID 3857 } // end m_useAPID
3821
3822 } // end root prims 3858 } // end root prims
3823 } // end Move() 3859 } // end Move()
3824 } // end class 3860 } // end class
generated by cgit v1.1 at 2024-12-24 11:02:45 +0000