diff options
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 562 |
1 files changed, 282 insertions, 280 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index dd7902a..567fd0e 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | |||
@@ -1,5 +1,7 @@ | |||
1 | /* Copyright (c) Contributors, http://opensimulator.org/ | 1 | /* |
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
2 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | 3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. |
4 | * | ||
3 | * Redistribution and use in source and binary forms, with or without | 5 | * Redistribution and use in source and binary forms, with or without |
4 | * modification, are permitted provided that the following conditions are met: | 6 | * modification, are permitted provided that the following conditions are met: |
5 | * * Redistributions of source code must retain the above copyright | 7 | * * Redistributions of source code must retain the above copyright |
@@ -93,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
93 | private float m_targetHoverHeight; | 95 | private float m_targetHoverHeight; |
94 | private float m_groundHeight; | 96 | private float m_groundHeight; |
95 | private float m_waterHeight; | 97 | private float m_waterHeight; |
96 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 98 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. |
97 | 99 | ||
98 | // private float m_tensor = 5f; | 100 | // private float m_tensor = 5f; |
99 | private int body_autodisable_frames = 20; | 101 | private int body_autodisable_frames = 20; |
@@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
294 | m_taintselected = value; | 296 | m_taintselected = value; |
295 | m_isSelected = value; | 297 | m_isSelected = value; |
296 | } | 298 | } |
297 | if(m_isSelected) disableBodySoft(); | 299 | if (m_isSelected) disableBodySoft(); |
298 | } | 300 | } |
299 | } | 301 | } |
300 | 302 | ||
@@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
302 | { | 304 | { |
303 | prev_geom = prim_geom; | 305 | prev_geom = prim_geom; |
304 | prim_geom = geom; | 306 | prim_geom = geom; |
305 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); | 307 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); |
306 | if (prim_geom != IntPtr.Zero) | 308 | if (prim_geom != IntPtr.Zero) |
307 | { | 309 | { |
308 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 310 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); |
@@ -314,7 +316,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
314 | if (_parent != null && _parent is OdePrim) | 316 | if (_parent != null && _parent is OdePrim) |
315 | { | 317 | { |
316 | OdePrim parent = (OdePrim)_parent; | 318 | OdePrim parent = (OdePrim)_parent; |
317 | //Console.WriteLine("SetGeom calls ChildSetGeom"); | 319 | //Console.WriteLine("SetGeom calls ChildSetGeom"); |
318 | parent.ChildSetGeom(this); | 320 | parent.ChildSetGeom(this); |
319 | } | 321 | } |
320 | } | 322 | } |
@@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
331 | { | 333 | { |
332 | d.BodyEnable(Body); | 334 | d.BodyEnable(Body); |
333 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 335 | if (m_vehicle.Type != Vehicle.TYPE_NONE) |
334 | m_vehicle.Enable(Body, _parent_scene); | 336 | m_vehicle.Enable(Body, _parent_scene); |
335 | } | 337 | } |
336 | 338 | ||
337 | m_disabled = false; | 339 | m_disabled = false; |
@@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
376 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | 378 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); |
377 | 379 | ||
378 | // disconnect from world gravity so we can apply buoyancy | 380 | // disconnect from world gravity so we can apply buoyancy |
379 | d.BodySetGravityMode (Body, false); | 381 | d.BodySetGravityMode (Body, false); |
380 | 382 | ||
381 | m_interpenetrationcount = 0; | 383 | m_interpenetrationcount = 0; |
382 | m_collisionscore = 0; | 384 | m_collisionscore = 0; |
@@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
872 | 874 | ||
873 | public void ProcessTaints(float timestep) | 875 | public void ProcessTaints(float timestep) |
874 | { | 876 | { |
875 | //Console.WriteLine("ProcessTaints for " + m_primName ); | 877 | //Console.WriteLine("ProcessTaints for " + m_primName); |
876 | if (m_taintadd) | 878 | if (m_taintadd) |
877 | { | 879 | { |
878 | changeadd(timestep); | 880 | changeadd(timestep); |
@@ -880,24 +882,24 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
880 | 882 | ||
881 | if (prim_geom != IntPtr.Zero) | 883 | if (prim_geom != IntPtr.Zero) |
882 | { | 884 | { |
883 | if (!_position.ApproxEquals(m_taintposition, 0f)) | 885 | if (!_position.ApproxEquals(m_taintposition, 0f)) |
884 | changemove(timestep); | 886 | changemove(timestep); |
885 | 887 | ||
886 | if (m_taintrot != _orientation) | 888 | if (m_taintrot != _orientation) |
887 | { | 889 | { |
888 | if(childPrim && IsPhysical) // For physical child prim... | 890 | if (childPrim && IsPhysical) // For physical child prim... |
889 | { | 891 | { |
890 | rotate(timestep); | 892 | rotate(timestep); |
891 | // KF: ODE will also rotate the parent prim! | 893 | // KF: ODE will also rotate the parent prim! |
892 | // so rotate the root back to where it was | 894 | // so rotate the root back to where it was |
893 | OdePrim parent = (OdePrim)_parent; | 895 | OdePrim parent = (OdePrim)_parent; |
894 | parent.rotate(timestep); | 896 | parent.rotate(timestep); |
895 | } | 897 | } |
896 | else | 898 | else |
897 | { | 899 | { |
898 | //Just rotate the prim | 900 | //Just rotate the prim |
899 | rotate(timestep); | 901 | rotate(timestep); |
900 | } | 902 | } |
901 | } | 903 | } |
902 | // | 904 | // |
903 | 905 | ||
@@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1006 | // destroy link | 1008 | // destroy link |
1007 | else if (_parent != null && m_taintparent == null) | 1009 | else if (_parent != null && m_taintparent == null) |
1008 | { | 1010 | { |
1009 | //Console.WriteLine(" changelink B"); | 1011 | //Console.WriteLine(" changelink B"); |
1010 | 1012 | ||
1011 | if (_parent is OdePrim) | 1013 | if (_parent is OdePrim) |
1012 | { | 1014 | { |
@@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1033 | // prim is the child | 1035 | // prim is the child |
1034 | public void ParentPrim(OdePrim prim) | 1036 | public void ParentPrim(OdePrim prim) |
1035 | { | 1037 | { |
1036 | //Console.WriteLine("ParentPrim " + m_primName); | 1038 | //Console.WriteLine("ParentPrim " + m_primName); |
1037 | if (this.m_localID != prim.m_localID) | 1039 | if (this.m_localID != prim.m_localID) |
1038 | { | 1040 | { |
1039 | if (Body == IntPtr.Zero) | 1041 | if (Body == IntPtr.Zero) |
@@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1047 | { | 1049 | { |
1048 | if (!childrenPrim.Contains(prim)) | 1050 | if (!childrenPrim.Contains(prim)) |
1049 | { | 1051 | { |
1050 | //Console.WriteLine("childrenPrim.Add " + prim); | 1052 | //Console.WriteLine("childrenPrim.Add " + prim); |
1051 | childrenPrim.Add(prim); | 1053 | childrenPrim.Add(prim); |
1052 | 1054 | ||
1053 | foreach (OdePrim prm in childrenPrim) | 1055 | foreach (OdePrim prm in childrenPrim) |
@@ -1080,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1080 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); | 1082 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); |
1081 | continue; | 1083 | continue; |
1082 | } | 1084 | } |
1083 | //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); | 1085 | //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); |
1084 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | 1086 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); |
1085 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | 1087 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); |
1086 | 1088 | ||
@@ -1128,7 +1130,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1128 | m_collisionCategories |= CollisionCategories.Body; | 1130 | m_collisionCategories |= CollisionCategories.Body; |
1129 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | 1131 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); |
1130 | 1132 | ||
1131 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); | 1133 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); |
1132 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | 1134 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); |
1133 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); | 1135 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); |
1134 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | 1136 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); |
@@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1203 | { | 1205 | { |
1204 | foreach (OdePrim prm in childrenPrim) | 1206 | foreach (OdePrim prm in childrenPrim) |
1205 | { | 1207 | { |
1206 | //Console.WriteLine("ChildSetGeom calls ParentPrim"); | 1208 | //Console.WriteLine("ChildSetGeom calls ParentPrim"); |
1207 | ParentPrim(prm); | 1209 | ParentPrim(prm); |
1208 | } | 1210 | } |
1209 | } | 1211 | } |
@@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1230 | 1232 | ||
1231 | lock (childrenPrim) | 1233 | lock (childrenPrim) |
1232 | { | 1234 | { |
1233 | //Console.WriteLine("childrenPrim.Remove " + odePrim); | 1235 | //Console.WriteLine("childrenPrim.Remove " + odePrim); |
1234 | childrenPrim.Remove(odePrim); | 1236 | childrenPrim.Remove(odePrim); |
1235 | } | 1237 | } |
1236 | 1238 | ||
@@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1248 | { | 1250 | { |
1249 | foreach (OdePrim prm in childrenPrim) | 1251 | foreach (OdePrim prm in childrenPrim) |
1250 | { | 1252 | { |
1251 | //Console.WriteLine("ChildDelink calls ParentPrim"); | 1253 | //Console.WriteLine("ChildDelink calls ParentPrim"); |
1252 | ParentPrim(prm); | 1254 | ParentPrim(prm); |
1253 | } | 1255 | } |
1254 | } | 1256 | } |
@@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1350 | 1352 | ||
1351 | public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) | 1353 | public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) |
1352 | { | 1354 | { |
1353 | //Console.WriteLine("CreateGeom:"); | 1355 | //Console.WriteLine("CreateGeom:"); |
1354 | if (_mesh != null) | 1356 | if (_mesh != null) |
1355 | { | 1357 | { |
1356 | setMesh(_parent_scene, _mesh); | 1358 | setMesh(_parent_scene, _mesh); |
@@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1381 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1383 | _parent_scene.waitForSpaceUnlock(m_targetSpace); |
1382 | try | 1384 | try |
1383 | { | 1385 | { |
1384 | //Console.WriteLine(" CreateGeom 2"); | 1386 | //Console.WriteLine(" CreateGeom 2"); |
1385 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | 1387 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); |
1386 | } | 1388 | } |
1387 | catch (AccessViolationException) | 1389 | catch (AccessViolationException) |
@@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1397 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1399 | _parent_scene.waitForSpaceUnlock(m_targetSpace); |
1398 | try | 1400 | try |
1399 | { | 1401 | { |
1400 | //Console.WriteLine(" CreateGeom 3"); | 1402 | //Console.WriteLine(" CreateGeom 3"); |
1401 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | 1403 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); |
1402 | } | 1404 | } |
1403 | catch (AccessViolationException) | 1405 | catch (AccessViolationException) |
@@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1414 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | 1416 | _parent_scene.waitForSpaceUnlock(m_targetSpace); |
1415 | try | 1417 | try |
1416 | { | 1418 | { |
1417 | //Console.WriteLine(" CreateGeom 4"); | 1419 | //Console.WriteLine(" CreateGeom 4"); |
1418 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | 1420 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); |
1419 | } | 1421 | } |
1420 | catch (AccessViolationException) | 1422 | catch (AccessViolationException) |
@@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1451 | 1453 | ||
1452 | lock (_parent_scene.OdeLock) | 1454 | lock (_parent_scene.OdeLock) |
1453 | { | 1455 | { |
1454 | //Console.WriteLine("changeadd 1"); | 1456 | //Console.WriteLine("changeadd 1"); |
1455 | CreateGeom(m_targetSpace, _mesh); | 1457 | CreateGeom(m_targetSpace, _mesh); |
1456 | 1458 | ||
1457 | if (prim_geom != IntPtr.Zero) | 1459 | if (prim_geom != IntPtr.Zero) |
@@ -1508,7 +1510,7 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
1508 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) | 1510 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) |
1509 | { | 1511 | { |
1510 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? | 1512 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? |
1511 | Console.WriteLine(" JointCreateFixed"); | 1513 | Console.WriteLine(" JointCreateFixed"); |
1512 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | 1514 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); |
1513 | d.JointAttach(m_linkJoint, Body, odParent.Body); | 1515 | d.JointAttach(m_linkJoint, Body, odParent.Body); |
1514 | d.JointSetFixed(m_linkJoint); | 1516 | d.JointSetFixed(m_linkJoint); |
@@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed"); | |||
1562 | float fz = 0; | 1564 | float fz = 0; |
1563 | 1565 | ||
1564 | 1566 | ||
1565 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. | 1567 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. |
1566 | { | 1568 | { |
1567 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1569 | if (m_vehicle.Type != Vehicle.TYPE_NONE) |
1568 | { | 1570 | { |
1569 | // 'VEHICLES' are dealt with in ODEDynamics.cs | 1571 | // 'VEHICLES' are dealt with in ODEDynamics.cs |
1570 | m_vehicle.Step(timestep, _parent_scene); | 1572 | m_vehicle.Step(timestep, _parent_scene); |
1571 | } | 1573 | } |
1572 | else | 1574 | else |
1573 | { | 1575 | { |
1574 | //Console.WriteLine("Move " + m_primName); | 1576 | //Console.WriteLine("Move " + m_primName); |
1575 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | 1577 | if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 |
1576 | // NON-'VEHICLES' are dealt with here | 1578 | // NON-'VEHICLES' are dealt with here |
1577 | if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) | 1579 | if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) |
1578 | { | 1580 | { |
1579 | d.Vector3 avel2 = d.BodyGetAngularVel(Body); | 1581 | d.Vector3 avel2 = d.BodyGetAngularVel(Body); |
1580 | if (m_angularlock.X == 1) | 1582 | if (m_angularlock.X == 1) |
1581 | avel2.X = 0; | 1583 | avel2.X = 0; |
1582 | if (m_angularlock.Y == 1) | 1584 | if (m_angularlock.Y == 1) |
1583 | avel2.Y = 0; | 1585 | avel2.Y = 0; |
1584 | if (m_angularlock.Z == 1) | 1586 | if (m_angularlock.Z == 1) |
1585 | avel2.Z = 0; | 1587 | avel2.Z = 0; |
1586 | d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); | 1588 | d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); |
1587 | } | 1589 | } |
1588 | //float PID_P = 900.0f; | 1590 | //float PID_P = 900.0f; |
1589 | 1591 | ||
1590 | float m_mass = CalculateMass(); | 1592 | float m_mass = CalculateMass(); |
1591 | 1593 | ||
1592 | // fz = 0f; | 1594 | // fz = 0f; |
1593 | //m_log.Info(m_collisionFlags.ToString()); | 1595 | //m_log.Info(m_collisionFlags.ToString()); |
1594 | 1596 | ||
1595 | 1597 | ||
1596 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 1598 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. |
1597 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? | 1599 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? |
1598 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | 1600 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up |
1599 | // gravityz multiplier = 1 - m_buoyancy | 1601 | // gravityz multiplier = 1 - m_buoyancy |
1600 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; | 1602 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; |
1601 | 1603 | ||
1602 | if (m_usePID) | 1604 | if (m_usePID) |
1603 | { | 1605 | { |
1604 | //Console.WriteLine("PID " + m_primName); | 1606 | //Console.WriteLine("PID " + m_primName); |
1605 | // KF - this is for object move? eg. llSetPos() ? | 1607 | // KF - this is for object move? eg. llSetPos() ? |
1606 | //if (!d.BodyIsEnabled(Body)) | 1608 | //if (!d.BodyIsEnabled(Body)) |
1607 | //d.BodySetForce(Body, 0f, 0f, 0f); | 1609 | //d.BodySetForce(Body, 0f, 0f, 0f); |
1608 | // If we're using the PID controller, then we have no gravity | 1610 | // If we're using the PID controller, then we have no gravity |
1609 | //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... | 1611 | //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... |
1610 | fz = 0f; | 1612 | fz = 0f; |
1611 | 1613 | ||
1612 | // no lock; for now it's only called from within Simulate() | 1614 | // no lock; for now it's only called from within Simulate() |
1613 | 1615 | ||
1614 | // If the PID Controller isn't active then we set our force | 1616 | // If the PID Controller isn't active then we set our force |
1615 | // calculating base velocity to the current position | 1617 | // calculating base velocity to the current position |
1616 | 1618 | ||
1617 | if ((m_PIDTau < 1) && (m_PIDTau != 0)) | 1619 | if ((m_PIDTau < 1) && (m_PIDTau != 0)) |
1618 | { | 1620 | { |
1619 | //PID_G = PID_G / m_PIDTau; | 1621 | //PID_G = PID_G / m_PIDTau; |
1620 | m_PIDTau = 1; | 1622 | m_PIDTau = 1; |
1621 | } | 1623 | } |
1622 | 1624 | ||
1623 | if ((PID_G - m_PIDTau) <= 0) | 1625 | if ((PID_G - m_PIDTau) <= 0) |
1624 | { | 1626 | { |
1625 | PID_G = m_PIDTau + 1; | 1627 | PID_G = m_PIDTau + 1; |
1626 | } | 1628 | } |
1627 | //PidStatus = true; | 1629 | //PidStatus = true; |
1628 | 1630 | ||
1629 | // PhysicsVector vec = new PhysicsVector(); | 1631 | // PhysicsVector vec = new PhysicsVector(); |
1630 | d.Vector3 vel = d.BodyGetLinearVel(Body); | 1632 | d.Vector3 vel = d.BodyGetLinearVel(Body); |
1631 | 1633 | ||
1632 | d.Vector3 pos = d.BodyGetPosition(Body); | 1634 | d.Vector3 pos = d.BodyGetPosition(Body); |
1633 | _target_velocity = | 1635 | _target_velocity = |
1634 | new Vector3( | 1636 | new Vector3( |
1635 | (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), | 1637 | (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), |
1636 | (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), | 1638 | (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), |
1637 | (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) | 1639 | (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) |
1638 | ); | 1640 | ); |
1639 | 1641 | ||
1640 | // if velocity is zero, use position control; otherwise, velocity control | 1642 | // if velocity is zero, use position control; otherwise, velocity control |
1641 | 1643 | ||
1642 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) | 1644 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) |
1643 | { | 1645 | { |
1644 | // keep track of where we stopped. No more slippin' & slidin' | 1646 | // keep track of where we stopped. No more slippin' & slidin' |
1645 | 1647 | ||
1646 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | 1648 | // We only want to deactivate the PID Controller if we think we want to have our surrogate |
1647 | // react to the physics scene by moving it's position. | 1649 | // react to the physics scene by moving it's position. |
1648 | // Avatar to Avatar collisions | 1650 | // Avatar to Avatar collisions |
1649 | // Prim to avatar collisions | 1651 | // Prim to avatar collisions |
1650 | 1652 | ||
1651 | //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); | 1653 | //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); |
1652 | //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); | 1654 | //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); |
1653 | //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; | 1655 | //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; |
1654 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | 1656 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); |
1655 | d.BodySetLinearVel(Body, 0, 0, 0); | 1657 | d.BodySetLinearVel(Body, 0, 0, 0); |
1656 | d.BodyAddForce(Body, 0, 0, fz); | 1658 | d.BodyAddForce(Body, 0, 0, fz); |
1657 | return; | 1659 | return; |
1658 | } | 1660 | } |
1659 | else | 1661 | else |
1660 | { | 1662 | { |
1661 | _zeroFlag = false; | 1663 | _zeroFlag = false; |
1662 | 1664 | ||
1663 | // We're flying and colliding with something | 1665 | // We're flying and colliding with something |
1664 | fx = ((_target_velocity.X) - vel.X) * (PID_D); | 1666 | fx = ((_target_velocity.X) - vel.X) * (PID_D); |
1665 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); | 1667 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); |
1666 | 1668 | ||
1667 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; | 1669 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; |
1668 | 1670 | ||
1669 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1671 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); |
1670 | } | 1672 | } |
1671 | } // end if (m_usePID) | 1673 | } // end if (m_usePID) |
1672 | 1674 | ||
1673 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | 1675 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller |
1674 | if (m_useHoverPID && !m_usePID) | 1676 | if (m_useHoverPID && !m_usePID) |
1675 | { | 1677 | { |
1676 | //Console.WriteLine("Hover " + m_primName); | 1678 | //Console.WriteLine("Hover " + m_primName); |
1677 | |||
1678 | // If we're using the PID controller, then we have no gravity | ||
1679 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
1680 | |||
1681 | // no lock; for now it's only called from within Simulate() | ||
1682 | |||
1683 | // If the PID Controller isn't active then we set our force | ||
1684 | // calculating base velocity to the current position | ||
1685 | |||
1686 | if ((m_PIDTau < 1)) | ||
1687 | { | ||
1688 | PID_G = PID_G / m_PIDTau; | ||
1689 | } | ||
1690 | |||
1691 | if ((PID_G - m_PIDTau) <= 0) | ||
1692 | { | ||
1693 | PID_G = m_PIDTau + 1; | ||
1694 | } | ||
1695 | 1679 | ||
1680 | // If we're using the PID controller, then we have no gravity | ||
1681 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
1682 | |||
1683 | // no lock; for now it's only called from within Simulate() | ||
1696 | 1684 | ||
1697 | // Where are we, and where are we headed? | 1685 | // If the PID Controller isn't active then we set our force |
1698 | d.Vector3 pos = d.BodyGetPosition(Body); | 1686 | // calculating base velocity to the current position |
1699 | d.Vector3 vel = d.BodyGetLinearVel(Body); | 1687 | |
1700 | 1688 | if ((m_PIDTau < 1)) | |
1701 | 1689 | { | |
1702 | // Non-Vehicles have a limited set of Hover options. | 1690 | PID_G = PID_G / m_PIDTau; |
1703 | // determine what our target height really is based on HoverType | 1691 | } |
1704 | switch (m_PIDHoverType) | 1692 | |
1705 | { | 1693 | if ((PID_G - m_PIDTau) <= 0) |
1706 | case PIDHoverType.Ground: | 1694 | { |
1707 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | 1695 | PID_G = m_PIDTau + 1; |
1708 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | 1696 | } |
1709 | break; | 1697 | |
1710 | case PIDHoverType.GroundAndWater: | 1698 | |
1711 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | 1699 | // Where are we, and where are we headed? |
1712 | m_waterHeight = _parent_scene.GetWaterLevel(); | 1700 | d.Vector3 pos = d.BodyGetPosition(Body); |
1713 | if (m_groundHeight > m_waterHeight) | 1701 | d.Vector3 vel = d.BodyGetLinearVel(Body); |
1714 | { | 1702 | |
1715 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | 1703 | |
1716 | } | 1704 | // Non-Vehicles have a limited set of Hover options. |
1717 | else | 1705 | // determine what our target height really is based on HoverType |
1718 | { | 1706 | switch (m_PIDHoverType) |
1719 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | 1707 | { |
1720 | } | 1708 | case PIDHoverType.Ground: |
1721 | break; | 1709 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); |
1722 | 1710 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | |
1723 | } // end switch (m_PIDHoverType) | 1711 | break; |
1724 | 1712 | case PIDHoverType.GroundAndWater: | |
1725 | 1713 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | |
1726 | _target_velocity = | 1714 | m_waterHeight = _parent_scene.GetWaterLevel(); |
1715 | if (m_groundHeight > m_waterHeight) | ||
1716 | { | ||
1717 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
1718 | } | ||
1719 | else | ||
1720 | { | ||
1721 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
1722 | } | ||
1723 | break; | ||
1724 | |||
1725 | } // end switch (m_PIDHoverType) | ||
1726 | |||
1727 | |||
1728 | _target_velocity = | ||
1727 | new Vector3(0.0f, 0.0f, | 1729 | new Vector3(0.0f, 0.0f, |
1728 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) | 1730 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) |
1729 | ); | 1731 | ); |
1730 | 1732 | ||
1731 | // if velocity is zero, use position control; otherwise, velocity control | 1733 | // if velocity is zero, use position control; otherwise, velocity control |
1732 | 1734 | ||
1733 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | 1735 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) |
1734 | { | 1736 | { |
1735 | // keep track of where we stopped. No more slippin' & slidin' | 1737 | // keep track of where we stopped. No more slippin' & slidin' |
1736 | 1738 | ||
1737 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | 1739 | // We only want to deactivate the PID Controller if we think we want to have our surrogate |
1738 | // react to the physics scene by moving it's position. | 1740 | // react to the physics scene by moving it's position. |
1739 | // Avatar to Avatar collisions | 1741 | // Avatar to Avatar collisions |
1740 | // Prim to avatar collisions | 1742 | // Prim to avatar collisions |
1741 | 1743 | ||
1742 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | 1744 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); |
1743 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | 1745 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); |
1744 | d.BodyAddForce(Body, 0, 0, fz); | 1746 | d.BodyAddForce(Body, 0, 0, fz); |
1745 | return; | 1747 | return; |
1746 | } | 1748 | } |
1747 | else | 1749 | else |
1748 | { | 1750 | { |
1749 | _zeroFlag = false; | 1751 | _zeroFlag = false; |
1750 | 1752 | ||
1751 | // We're flying and colliding with something | 1753 | // We're flying and colliding with something |
1752 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1754 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); |
1753 | } | 1755 | } |
1754 | } | 1756 | } |
1755 | 1757 | ||
1756 | fx *= m_mass; | 1758 | fx *= m_mass; |
1757 | fy *= m_mass; | 1759 | fy *= m_mass; |
1758 | //fz *= m_mass; | 1760 | //fz *= m_mass; |
1759 | 1761 | ||
1760 | fx += m_force.X; | 1762 | fx += m_force.X; |
1761 | fy += m_force.Y; | 1763 | fy += m_force.Y; |
1762 | fz += m_force.Z; | 1764 | fz += m_force.Z; |
1763 | 1765 | ||
1764 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | 1766 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); |
1765 | if (fx != 0 || fy != 0 || fz != 0) | 1767 | if (fx != 0 || fy != 0 || fz != 0) |
1766 | { | 1768 | { |
1767 | //m_taintdisable = true; | 1769 | //m_taintdisable = true; |
1768 | //base.RaiseOutOfBounds(Position); | 1770 | //base.RaiseOutOfBounds(Position); |
1769 | //d.BodySetLinearVel(Body, fx, fy, 0f); | 1771 | //d.BodySetLinearVel(Body, fx, fy, 0f); |
1770 | if (!d.BodyIsEnabled(Body)) | 1772 | if (!d.BodyIsEnabled(Body)) |
1771 | { | 1773 | { |
1772 | // A physical body at rest on a surface will auto-disable after a while, | 1774 | // A physical body at rest on a surface will auto-disable after a while, |
1773 | // this appears to re-enable it incase the surface it is upon vanishes, | 1775 | // this appears to re-enable it incase the surface it is upon vanishes, |
1774 | // and the body should fall again. | 1776 | // and the body should fall again. |
1775 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | 1777 | d.BodySetLinearVel(Body, 0f, 0f, 0f); |
1776 | d.BodySetForce(Body, 0, 0, 0); | 1778 | d.BodySetForce(Body, 0, 0, 0); |
1777 | enableBodySoft(); | 1779 | enableBodySoft(); |
1778 | } | 1780 | } |
1779 | 1781 | ||
1780 | // 35x10 = 350n times the mass per second applied maximum. | 1782 | // 35x10 = 350n times the mass per second applied maximum. |
1781 | float nmax = 35f * m_mass; | 1783 | float nmax = 35f * m_mass; |
1782 | float nmin = -35f * m_mass; | 1784 | float nmin = -35f * m_mass; |
1783 | 1785 | ||
1784 | 1786 | ||
1785 | if (fx > nmax) | 1787 | if (fx > nmax) |
1786 | fx = nmax; | 1788 | fx = nmax; |
1787 | if (fx < nmin) | 1789 | if (fx < nmin) |
1788 | fx = nmin; | 1790 | fx = nmin; |
1789 | if (fy > nmax) | 1791 | if (fy > nmax) |
1790 | fy = nmax; | 1792 | fy = nmax; |
1791 | if (fy < nmin) | 1793 | if (fy < nmin) |
1792 | fy = nmin; | 1794 | fy = nmin; |
1793 | d.BodyAddForce(Body, fx, fy, fz); | 1795 | d.BodyAddForce(Body, fx, fy, fz); |
1794 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | 1796 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); |
1795 | } | 1797 | } |
1796 | } | 1798 | } |
1797 | } | 1799 | } |
1798 | else | 1800 | else |
1799 | { // is not physical, or is not a body or is selected | 1801 | { // is not physical, or is not a body or is selected |
1800 | // _zeroPosition = d.BodyGetPosition(Body); | 1802 | // _zeroPosition = d.BodyGetPosition(Body); |
1801 | return; | 1803 | return; |
1802 | //Console.WriteLine("Nothing " + m_primName); | 1804 | //Console.WriteLine("Nothing " + m_primName); |
1803 | 1805 | ||
1804 | } | 1806 | } |
1805 | } | 1807 | } |
@@ -1815,18 +1817,18 @@ Console.WriteLine(" JointCreateFixed"); | |||
1815 | myrot.W = _orientation.W; | 1817 | myrot.W = _orientation.W; |
1816 | if (Body != IntPtr.Zero) | 1818 | if (Body != IntPtr.Zero) |
1817 | { | 1819 | { |
1818 | // KF: If this is a root prim do BodySet | 1820 | // KF: If this is a root prim do BodySet |
1819 | d.BodySetQuaternion(Body, ref myrot); | 1821 | d.BodySetQuaternion(Body, ref myrot); |
1820 | if (m_isphysical) | 1822 | if (m_isphysical) |
1821 | { | 1823 | { |
1822 | if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) | 1824 | if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) |
1823 | createAMotor(m_angularlock); | 1825 | createAMotor(m_angularlock); |
1824 | } | 1826 | } |
1825 | } | 1827 | } |
1826 | else | 1828 | else |
1827 | { | 1829 | { |
1828 | // daughter prim, do Geom set | 1830 | // daughter prim, do Geom set |
1829 | d.GeomSetQuaternion(prim_geom, ref myrot); | 1831 | d.GeomSetQuaternion(prim_geom, ref myrot); |
1830 | } | 1832 | } |
1831 | 1833 | ||
1832 | resetCollisionAccounting(); | 1834 | resetCollisionAccounting(); |
@@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1890 | m_log.Error("[PHYSICS]: PrimGeom dead"); | 1892 | m_log.Error("[PHYSICS]: PrimGeom dead"); |
1891 | } | 1893 | } |
1892 | } | 1894 | } |
1893 | //Console.WriteLine("changePhysicsStatus for " + m_primName ); | 1895 | //Console.WriteLine("changePhysicsStatus for " + m_primName); |
1894 | changeadd(2f); | 1896 | changeadd(2f); |
1895 | } | 1897 | } |
1896 | if (childPrim) | 1898 | if (childPrim) |
@@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1976 | else | 1978 | else |
1977 | { | 1979 | { |
1978 | _mesh = null; | 1980 | _mesh = null; |
1979 | //Console.WriteLine("changesize 2"); | 1981 | //Console.WriteLine("changesize 2"); |
1980 | CreateGeom(m_targetSpace, _mesh); | 1982 | CreateGeom(m_targetSpace, _mesh); |
1981 | } | 1983 | } |
1982 | 1984 | ||
@@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2083 | else | 2085 | else |
2084 | { | 2086 | { |
2085 | _mesh = null; | 2087 | _mesh = null; |
2086 | //Console.WriteLine("changeshape"); | 2088 | //Console.WriteLine("changeshape"); |
2087 | CreateGeom(m_targetSpace, null); | 2089 | CreateGeom(m_targetSpace, null); |
2088 | } | 2090 | } |
2089 | 2091 | ||
@@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2454 | if (QuaternionIsFinite(value)) | 2456 | if (QuaternionIsFinite(value)) |
2455 | { | 2457 | { |
2456 | _orientation = value; | 2458 | _orientation = value; |
2457 | } | 2459 | } |
2458 | else | 2460 | else |
2459 | m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); | 2461 | m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); |
2460 | 2462 | ||
@@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed"); | |||
2675 | //outofBounds = true; | 2677 | //outofBounds = true; |
2676 | } | 2678 | } |
2677 | 2679 | ||
2678 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); | 2680 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); |
2679 | //Console.WriteLine("Adiff " + m_primName + " = " + Adiff); | 2681 | //Console.WriteLine("Adiff " + m_primName + " = " + Adiff); |
2680 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | 2682 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) |
2681 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | 2683 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) |
2682 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) | 2684 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) |
@@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2684 | && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large | 2686 | && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large |
2685 | { | 2687 | { |
2686 | _zeroFlag = true; | 2688 | _zeroFlag = true; |
2687 | //Console.WriteLine("ZFT 2"); | 2689 | //Console.WriteLine("ZFT 2"); |
2688 | m_throttleUpdates = false; | 2690 | m_throttleUpdates = false; |
2689 | } | 2691 | } |
2690 | else | 2692 | else |