aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs101
1 files changed, 100 insertions, 1 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index ae0bb11..f164048 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -69,11 +69,20 @@ namespace OpenSim.Region.Physics.OdePlugin
69 private float m_PIDTau = 0f; 69 private float m_PIDTau = 0f;
70 private float PID_D = 35f; 70 private float PID_D = 35f;
71 private float PID_G = 25f; 71 private float PID_G = 25f;
72 private bool m_usePID = false;
73
74 private float m_PIDHoverHeight = 0f;
75 private float m_PIDHoverTau = 0f;
76 private bool m_useHoverPID = false;
77 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
78 private float m_targetHoverHeight = 0f;
79 private float m_groundHeight = 0f;
80 private float m_waterHeight = 0f;
81
72 private float m_tensor = 5f; 82 private float m_tensor = 5f;
73 private int body_autodisable_frames = 20; 83 private int body_autodisable_frames = 20;
74 private IMesh primMesh = null; 84 private IMesh primMesh = null;
75 85
76 private bool m_usePID = false;
77 86
78 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom 87 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
79 | CollisionCategories.Space 88 | CollisionCategories.Space
@@ -1554,6 +1563,91 @@ namespace OpenSim.Region.Physics.OdePlugin
1554 } 1563 }
1555 } 1564 }
1556 1565
1566 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1567 if (m_useHoverPID && !m_usePID)
1568 {
1569 // If we're using the PID controller, then we have no gravity
1570 fz = (-1 * _parent_scene.gravityz) * m_mass;
1571
1572 // no lock; for now it's only called from within Simulate()
1573
1574 // If the PID Controller isn't active then we set our force
1575 // calculating base velocity to the current position
1576
1577 if ((m_PIDTau < 1))
1578 {
1579 PID_G = PID_G / m_PIDTau;
1580 }
1581
1582 if ((PID_G - m_PIDTau) <= 0)
1583 {
1584 PID_G = m_PIDTau + 1;
1585 }
1586
1587
1588 // Where are we, and where are we headed?
1589 d.Vector3 pos = d.BodyGetPosition(Body);
1590 d.Vector3 vel = d.BodyGetLinearVel(Body);
1591
1592 // determine what our target height really is based on HoverType
1593 switch (m_PIDHoverType)
1594 {
1595 case PIDHoverType.Absolute:
1596 m_targetHoverHeight = m_PIDHoverHeight;
1597 break;
1598 case PIDHoverType.Ground:
1599 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1600 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1601 break;
1602 case PIDHoverType.GroundAndWater:
1603 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1604 m_waterHeight = _parent_scene.GetWaterLevel();
1605 if (m_groundHeight > m_waterHeight)
1606 {
1607 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1608 }
1609 else
1610 {
1611 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1612 }
1613 break;
1614 case PIDHoverType.Water:
1615 m_waterHeight = _parent_scene.GetWaterLevel();
1616 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1617 break;
1618 }
1619
1620
1621 _target_velocity =
1622 new PhysicsVector(0.0f, 0.0f,
1623 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1624 );
1625
1626 // if velocity is zero, use position control; otherwise, velocity control
1627
1628 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
1629 {
1630 // keep track of where we stopped. No more slippin' & slidin'
1631
1632 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1633 // react to the physics scene by moving it's position.
1634 // Avatar to Avatar collisions
1635 // Prim to avatar collisions
1636
1637 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1638 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1639 d.BodyAddForce(Body, 0, 0, fz);
1640 return;
1641 }
1642 else
1643 {
1644 _zeroFlag = false;
1645
1646 // We're flying and colliding with something
1647 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1648 }
1649 }
1650
1557 fx *= m_mass; 1651 fx *= m_mass;
1558 fy *= m_mass; 1652 fy *= m_mass;
1559 //fz *= m_mass; 1653 //fz *= m_mass;
@@ -2622,6 +2716,11 @@ namespace OpenSim.Region.Physics.OdePlugin
2622 public override bool PIDActive { set { m_usePID = value; } } 2716 public override bool PIDActive { set { m_usePID = value; } }
2623 public override float PIDTau { set { m_PIDTau = value; } } 2717 public override float PIDTau { set { m_PIDTau = value; } }
2624 2718
2719 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2720 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2721 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2722 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2723
2625 private void createAMotor(PhysicsVector axis) 2724 private void createAMotor(PhysicsVector axis)
2626 { 2725 {
2627 if (Body == IntPtr.Zero) 2726 if (Body == IntPtr.Zero)