aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim
diff options
context:
space:
mode:
authorKittoFlora2009-10-27 19:56:39 +0100
committerKittoFlora2009-10-27 19:56:39 +0100
commita81a16f22f7af6d102f365f69a50c5d201ae8362 (patch)
treecd69aee7a8519db2af8bcc5e53698fba63cc860a /OpenSim
parentAdd llRotLookat pt1. (diff)
downloadopensim-SC-a81a16f22f7af6d102f365f69a50c5d201ae8362.zip
opensim-SC-a81a16f22f7af6d102f365f69a50c5d201ae8362.tar.gz
opensim-SC-a81a16f22f7af6d102f365f69a50c5d201ae8362.tar.bz2
opensim-SC-a81a16f22f7af6d102f365f69a50c5d201ae8362.tar.xz
llRotLookAt Pt 2
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs34
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs55
2 files changed, 52 insertions, 37 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
index 8b57f06..0c168c6 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs
@@ -1,16 +1,4 @@
1/* 1/*
2 * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
3 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
4 * ODEPrim.cs contains methods dealing with Prim editing, Prim
5 * characteristics and Kinetic motion.
6 * ODEDynamics.cs contains methods dealing with Prim Physical motion
7 * (dynamics) and the associated settings. Old Linear and angular
8 * motors for dynamic motion have been replace with MoveLinear()
9 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
10 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
11 * switch between 'VEHICLE' parameter use and general dynamics
12 * settings use.
13 *
14 * Copyright (c) Contributors, http://opensimulator.org/ 2 * Copyright (c) Contributors, http://opensimulator.org/
15 * See CONTRIBUTORS.TXT for a full list of copyright holders. 3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
16 * 4 *
@@ -35,6 +23,19 @@
35 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
37 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 *
27 * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
28 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
29 * ODEPrim.cs contains methods dealing with Prim editing, Prim
30 * characteristics and Kinetic motion.
31 * ODEDynamics.cs contains methods dealing with Prim Physical motion
32 * (dynamics) and the associated settings. Old Linear and angular
33 * motors for dynamic motion have been replace with MoveLinear()
34 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
35 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
36 * switch between 'VEHICLE' parameter use and general dynamics
37 * settings use.
38 *
38 */ 39 */
39 40
40using System; 41using System;
@@ -66,8 +67,8 @@ namespace OpenSim.Region.Physics.OdePlugin
66 67
67 // private OdeScene m_parentScene = null; 68 // private OdeScene m_parentScene = null;
68 private IntPtr m_body = IntPtr.Zero; 69 private IntPtr m_body = IntPtr.Zero;
69 private IntPtr m_jointGroup = IntPtr.Zero; 70// private IntPtr m_jointGroup = IntPtr.Zero;
70 private IntPtr m_aMotor = IntPtr.Zero; 71// private IntPtr m_aMotor = IntPtr.Zero;
71 72
72 73
73 // Vehicle properties 74 // Vehicle properties
@@ -560,6 +561,7 @@ namespace OpenSim.Region.Physics.OdePlugin
560 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate 561 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
561 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body 562 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
562 */ 563 */
564//if(frcount == 0) Console.WriteLine("MoveAngular ");
563 565
564 // Get what the body is doing, this includes 'external' influences 566 // Get what the body is doing, this includes 'external' influences
565 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); 567 d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
@@ -614,7 +616,7 @@ namespace OpenSim.Region.Physics.OdePlugin
614 // Error is 0 (no error) to +/- 2 (max error) 616 // Error is 0 (no error) to +/- 2 (max error)
615 // scale it by VAservo 617 // scale it by VAservo
616 verterr = verterr * VAservo; 618 verterr = verterr * VAservo;
617//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); 619if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
618 620
619 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so 621 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
620 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. 622 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
@@ -635,7 +637,7 @@ namespace OpenSim.Region.Physics.OdePlugin
635 // Deflection section tba 637 // Deflection section tba
636 638
637 // Sum velocities 639 // Sum velocities
638 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection 640 m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection
639 641
640 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) 642 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
641 { 643 {
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index e1bf996..93c9a44 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -87,9 +87,6 @@ namespace OpenSim.Region.Physics.OdePlugin
87 private Quaternion m_APIDTarget = new Quaternion(); 87 private Quaternion m_APIDTarget = new Quaternion();
88 private float m_APIDStrength = 0.5f; 88 private float m_APIDStrength = 0.5f;
89 private float m_APIDDamping = 0.5f; 89 private float m_APIDDamping = 0.5f;
90
91 private float APID_D = 35f;
92 private float APID_G = 25f;
93 private bool m_useAPID = false; 90 private bool m_useAPID = false;
94 91
95 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 92 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
@@ -192,6 +189,9 @@ namespace OpenSim.Region.Physics.OdePlugin
192 private ODEDynamics m_vehicle; 189 private ODEDynamics m_vehicle;
193 190
194 internal int m_material = (int)Material.Wood; 191 internal int m_material = (int)Material.Wood;
192
193 private int frcount = 0; // Used to limit dynamics debug output to
194
195 195
196 public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size, 196 public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
197 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 197 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
@@ -1563,9 +1563,14 @@ Console.WriteLine(" JointCreateFixed");
1563 float fy = 0; 1563 float fy = 0;
1564 float fz = 0; 1564 float fz = 0;
1565 1565
1566 frcount++; // used to limit debug comment output
1567 if (frcount > 100)
1568 frcount = 0;
1566 1569
1567 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. 1570 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1568 { 1571 {
1572if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
1573 " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
1569 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1574 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1570 { 1575 {
1571 // 'VEHICLES' are dealt with in ODEDynamics.cs 1576 // 'VEHICLES' are dealt with in ODEDynamics.cs
@@ -1573,7 +1578,6 @@ Console.WriteLine(" JointCreateFixed");
1573 } 1578 }
1574 else 1579 else
1575 { 1580 {
1576//Console.WriteLine("Move " + m_primName);
1577 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1581 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1578 // NON-'VEHICLES' are dealt with here 1582 // NON-'VEHICLES' are dealt with here
1579 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) 1583 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
@@ -1602,7 +1606,7 @@ Console.WriteLine(" JointCreateFixed");
1602 1606
1603 if (m_usePID) 1607 if (m_usePID)
1604 { 1608 {
1605//Console.WriteLine("PID " + m_primName); 1609//if(frcount == 0) Console.WriteLine("PID " + m_primName);
1606 // KF - this is for object MoveToTarget. 1610 // KF - this is for object MoveToTarget.
1607 1611
1608 //if (!d.BodyIsEnabled(Body)) 1612 //if (!d.BodyIsEnabled(Body))
@@ -1741,7 +1745,7 @@ Console.WriteLine(" JointCreateFixed");
1741 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 1745 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1742 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 1746 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1743 d.BodyAddForce(Body, 0, 0, fz); 1747 d.BodyAddForce(Body, 0, 0, fz);
1744 return; 1748 //KF this prevents furthur motions return;
1745 } 1749 }
1746 else 1750 else
1747 { 1751 {
@@ -1756,13 +1760,16 @@ Console.WriteLine(" JointCreateFixed");
1756 { 1760 {
1757 // RotLookAt, apparently overrides all other rotation sources. Inputs: 1761 // RotLookAt, apparently overrides all other rotation sources. Inputs:
1758 // Quaternion m_APIDTarget 1762 // Quaternion m_APIDTarget
1759 // float m_APIDStrength // perhaps ratio other forces to lookat force? 1763 // float m_APIDStrength // From SL experiments, this is the time to get there
1760 // float m_APIDDamping //'seconds to critically damps in'[sic] 1764 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
1765 // Also in SL the mass of the object has no effect on time to get there.
1761 // Factors: 1766 // Factors:
1762 // float APID_D 1767//if(frcount == 0) Console.WriteLine("APID ");
1763 // float APID_G
1764
1765 // get present body rotation 1768 // get present body rotation
1769 float limit = 1.0f;
1770 float scaler = 50f; // adjusts damping time
1771 float RLAservo = 0f;
1772
1766 d.Quaternion rot = d.BodyGetQuaternion(Body); 1773 d.Quaternion rot = d.BodyGetQuaternion(Body);
1767 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); 1774 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
1768 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget; 1775 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
@@ -1770,15 +1777,21 @@ Console.WriteLine(" JointCreateFixed");
1770 Vector3 diff_axis; 1777 Vector3 diff_axis;
1771 rot_diff.GetAxisAngle(out diff_axis, out diff_angle); 1778 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
1772 diff_axis.Normalize(); 1779 diff_axis.Normalize();
1773 PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z); 1780 if(diff_angle > 0.01f) // diff_angle is always +ve
1774 float RLAservo = timestep / m_APIDDamping; 1781 {
1775 rotforce = rotforce * RLAservo * m_mass; 1782// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
1776 d.BodyAddTorque(Body, rotforce.X, rotforce.Y, rotforce.Z); 1783 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
1777 1784 rotforce = rotforce * rotq;
1778 // d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 1785 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
1779 1786// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
1780 1787 // rotforce = rotforce * RLAservo * diff_angle ;
1781 1788 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1789 RLAservo = timestep / m_APIDStrength * scaler;
1790 rotforce = rotforce * RLAservo * diff_angle ;
1791 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1792//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1793 }
1794if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
1782 } // end m_useAPID 1795 } // end m_useAPID
1783 1796
1784 fx *= m_mass; 1797 fx *= m_mass;
@@ -2674,7 +2687,7 @@ Console.WriteLine(" JointCreateFixed");
2674 //outofBounds = true; 2687 //outofBounds = true;
2675 } 2688 }
2676 2689
2677 float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); 2690// float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2678//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); 2691//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2679 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 2692 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2680 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 2693 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)