diff options
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs')
-rw-r--r-- | OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 95 |
1 files changed, 79 insertions, 16 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 3eb3b28..3765123 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | |||
@@ -21,6 +21,18 @@ | |||
21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
24 | * | ||
25 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
26 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
27 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
28 | * characteristics and Kinetic motion. | ||
29 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
30 | * (dynamics) and the associated settings. Old Linear and angular | ||
31 | * motors for dynamic motion have been replace with MoveLinear() | ||
32 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
33 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
34 | * switch between 'VEHICLE' parameter use and general dynamics | ||
35 | * settings use. | ||
24 | */ | 36 | */ |
25 | 37 | ||
26 | /* | 38 | /* |
@@ -81,7 +93,12 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
81 | private float m_PIDTau; | 93 | private float m_PIDTau; |
82 | private float PID_D = 35f; | 94 | private float PID_D = 35f; |
83 | private float PID_G = 25f; | 95 | private float PID_G = 25f; |
84 | private bool m_usePID; | 96 | private bool m_usePID = false; |
97 | |||
98 | private Quaternion m_APIDTarget = new Quaternion(); | ||
99 | private float m_APIDStrength = 0.5f; | ||
100 | private float m_APIDDamping = 0.5f; | ||
101 | private bool m_useAPID = false; | ||
85 | 102 | ||
86 | // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), | 103 | // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), |
87 | // and are for non-VEHICLES only. | 104 | // and are for non-VEHICLES only. |
@@ -182,6 +199,9 @@ namespace OpenSim.Region.Physics.OdePlugin | |||
182 | private ODEDynamics m_vehicle; | 199 | private ODEDynamics m_vehicle; |
183 | 200 | ||
184 | internal int m_material = (int)Material.Wood; | 201 | internal int m_material = (int)Material.Wood; |
202 | |||
203 | private int frcount = 0; // Used to limit dynamics debug output to | ||
204 | |||
185 | 205 | ||
186 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | 206 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, |
187 | Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) | 207 | Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) |
@@ -1561,9 +1581,14 @@ Console.WriteLine(" JointCreateFixed"); | |||
1561 | float fy = 0; | 1581 | float fy = 0; |
1562 | float fz = 0; | 1582 | float fz = 0; |
1563 | 1583 | ||
1584 | frcount++; // used to limit debug comment output | ||
1585 | if (frcount > 100) | ||
1586 | frcount = 0; | ||
1564 | 1587 | ||
1565 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. | 1588 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. |
1566 | { | 1589 | { |
1590 | //if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type + | ||
1591 | // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); | ||
1567 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | 1592 | if (m_vehicle.Type != Vehicle.TYPE_NONE) |
1568 | { | 1593 | { |
1569 | // 'VEHICLES' are dealt with in ODEDynamics.cs | 1594 | // 'VEHICLES' are dealt with in ODEDynamics.cs |
@@ -1571,7 +1596,6 @@ Console.WriteLine(" JointCreateFixed"); | |||
1571 | } | 1596 | } |
1572 | else | 1597 | else |
1573 | { | 1598 | { |
1574 | //Console.WriteLine("Move " + m_primName); | ||
1575 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | 1599 | if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 |
1576 | // NON-'VEHICLES' are dealt with here | 1600 | // NON-'VEHICLES' are dealt with here |
1577 | if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) | 1601 | if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) |
@@ -1593,21 +1617,18 @@ Console.WriteLine(" JointCreateFixed"); | |||
1593 | //m_log.Info(m_collisionFlags.ToString()); | 1617 | //m_log.Info(m_collisionFlags.ToString()); |
1594 | 1618 | ||
1595 | 1619 | ||
1596 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | 1620 | //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. |
1597 | // 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 | 1621 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up |
1599 | // gravityz multiplier = 1 - m_buoyancy | 1622 | // NB Prims in ODE are no subject to global gravity |
1600 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; | 1623 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass |
1601 | 1624 | ||
1602 | if (m_usePID) | 1625 | if (m_usePID) |
1603 | { | 1626 | { |
1604 | //Console.WriteLine("PID " + m_primName); | 1627 | //if(frcount == 0) Console.WriteLine("PID " + m_primName); |
1605 | // KF - this is for object move? eg. llSetPos() ? | 1628 | // KF - this is for object MoveToTarget. |
1629 | |||
1606 | //if (!d.BodyIsEnabled(Body)) | 1630 | //if (!d.BodyIsEnabled(Body)) |
1607 | //d.BodySetForce(Body, 0f, 0f, 0f); | 1631 | //d.BodySetForce(Body, 0f, 0f, 0f); |
1608 | // 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... | ||
1610 | fz = 0f; | ||
1611 | 1632 | ||
1612 | // no lock; for now it's only called from within Simulate() | 1633 | // no lock; for now it's only called from within Simulate() |
1613 | 1634 | ||
@@ -1742,7 +1763,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
1742 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | 1763 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); |
1743 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | 1764 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); |
1744 | d.BodyAddForce(Body, 0, 0, fz); | 1765 | d.BodyAddForce(Body, 0, 0, fz); |
1745 | return; | 1766 | //KF this prevents furthur motions return; |
1746 | } | 1767 | } |
1747 | else | 1768 | else |
1748 | { | 1769 | { |
@@ -1751,8 +1772,46 @@ Console.WriteLine(" JointCreateFixed"); | |||
1751 | // We're flying and colliding with something | 1772 | // We're flying and colliding with something |
1752 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | 1773 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); |
1753 | } | 1774 | } |
1754 | } | 1775 | } // end m_useHoverPID && !m_usePID |
1755 | 1776 | ||
1777 | if (m_useAPID) | ||
1778 | { | ||
1779 | // RotLookAt, apparently overrides all other rotation sources. Inputs: | ||
1780 | // Quaternion m_APIDTarget | ||
1781 | // float m_APIDStrength // From SL experiments, this is the time to get there | ||
1782 | // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly | ||
1783 | // Also in SL the mass of the object has no effect on time to get there. | ||
1784 | // Factors: | ||
1785 | //if(frcount == 0) Console.WriteLine("APID "); | ||
1786 | // get present body rotation | ||
1787 | float limit = 1.0f; | ||
1788 | float scaler = 50f; // adjusts damping time | ||
1789 | float RLAservo = 0f; | ||
1790 | |||
1791 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
1792 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); | ||
1793 | Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; | ||
1794 | float diff_angle; | ||
1795 | Vector3 diff_axis; | ||
1796 | rot_diff.GetAxisAngle(out diff_axis, out diff_angle); | ||
1797 | diff_axis.Normalize(); | ||
1798 | if(diff_angle > 0.01f) // diff_angle is always +ve | ||
1799 | { | ||
1800 | // PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); | ||
1801 | Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); | ||
1802 | rotforce = rotforce * rotq; | ||
1803 | if(diff_angle > limit) diff_angle = limit; // cap the rotate rate | ||
1804 | // RLAservo = timestep / m_APIDStrength * m_mass * scaler; | ||
1805 | // rotforce = rotforce * RLAservo * diff_angle ; | ||
1806 | // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); | ||
1807 | RLAservo = timestep / m_APIDStrength * scaler; | ||
1808 | rotforce = rotforce * RLAservo * diff_angle ; | ||
1809 | d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); | ||
1810 | //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); | ||
1811 | } | ||
1812 | //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); | ||
1813 | } // end m_useAPID | ||
1814 | |||
1756 | fx *= m_mass; | 1815 | fx *= m_mass; |
1757 | fy *= m_mass; | 1816 | fy *= m_mass; |
1758 | //fz *= m_mass; | 1817 | //fz *= m_mass; |
@@ -2614,7 +2673,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2614 | 2673 | ||
2615 | m_lastposition = _position; | 2674 | m_lastposition = _position; |
2616 | m_lastorientation = _orientation; | 2675 | m_lastorientation = _orientation; |
2617 | 2676 | ||
2618 | l_position.X = vec.X; | 2677 | l_position.X = vec.X; |
2619 | l_position.Y = vec.Y; | 2678 | l_position.Y = vec.Y; |
2620 | l_position.Z = vec.Z; | 2679 | l_position.Z = vec.Z; |
@@ -2622,6 +2681,10 @@ Console.WriteLine(" JointCreateFixed"); | |||
2622 | l_orientation.Y = ori.Y; | 2681 | l_orientation.Y = ori.Y; |
2623 | l_orientation.Z = ori.Z; | 2682 | l_orientation.Z = ori.Z; |
2624 | l_orientation.W = ori.W; | 2683 | l_orientation.W = ori.W; |
2684 | |||
2685 | // if(l_position.Y != m_lastposition.Y){ | ||
2686 | // Console.WriteLine("UP&V {0} {1}", m_primName, l_position); | ||
2687 | // } | ||
2625 | 2688 | ||
2626 | if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) | 2689 | if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) |
2627 | { | 2690 | { |
@@ -2822,7 +2885,7 @@ Console.WriteLine(" JointCreateFixed"); | |||
2822 | } | 2885 | } |
2823 | public override bool PIDActive { set { m_usePID = value; } } | 2886 | public override bool PIDActive { set { m_usePID = value; } } |
2824 | public override float PIDTau { set { m_PIDTau = value; } } | 2887 | public override float PIDTau { set { m_PIDTau = value; } } |
2825 | 2888 | ||
2826 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | 2889 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } |
2827 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | 2890 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } |
2828 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | 2891 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } |