llRotLookAt Pt 2
parent
8428b25939
commit
a81a16f22f
|
@ -1,16 +1,4 @@
|
||||||
/*
|
/*
|
||||||
* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
|
|
||||||
* ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
|
|
||||||
* ODEPrim.cs contains methods dealing with Prim editing, Prim
|
|
||||||
* characteristics and Kinetic motion.
|
|
||||||
* ODEDynamics.cs contains methods dealing with Prim Physical motion
|
|
||||||
* (dynamics) and the associated settings. Old Linear and angular
|
|
||||||
* motors for dynamic motion have been replace with MoveLinear()
|
|
||||||
* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
|
|
||||||
* simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
|
|
||||||
* switch between 'VEHICLE' parameter use and general dynamics
|
|
||||||
* settings use.
|
|
||||||
*
|
|
||||||
* Copyright (c) Contributors, http://opensimulator.org/
|
* Copyright (c) Contributors, http://opensimulator.org/
|
||||||
* See CONTRIBUTORS.TXT for a full list of copyright holders.
|
* See CONTRIBUTORS.TXT for a full list of copyright holders.
|
||||||
*
|
*
|
||||||
|
@ -35,6 +23,19 @@
|
||||||
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
|
||||||
|
* ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
|
||||||
|
* ODEPrim.cs contains methods dealing with Prim editing, Prim
|
||||||
|
* characteristics and Kinetic motion.
|
||||||
|
* ODEDynamics.cs contains methods dealing with Prim Physical motion
|
||||||
|
* (dynamics) and the associated settings. Old Linear and angular
|
||||||
|
* motors for dynamic motion have been replace with MoveLinear()
|
||||||
|
* and MoveAngular(); 'Physical' is used only to switch ODE dynamic
|
||||||
|
* simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
|
||||||
|
* switch between 'VEHICLE' parameter use and general dynamics
|
||||||
|
* settings use.
|
||||||
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
using System;
|
using System;
|
||||||
|
@ -66,8 +67,8 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
|
|
||||||
// private OdeScene m_parentScene = null;
|
// private OdeScene m_parentScene = null;
|
||||||
private IntPtr m_body = IntPtr.Zero;
|
private IntPtr m_body = IntPtr.Zero;
|
||||||
private IntPtr m_jointGroup = IntPtr.Zero;
|
// private IntPtr m_jointGroup = IntPtr.Zero;
|
||||||
private IntPtr m_aMotor = IntPtr.Zero;
|
// private IntPtr m_aMotor = IntPtr.Zero;
|
||||||
|
|
||||||
|
|
||||||
// Vehicle properties
|
// Vehicle properties
|
||||||
|
@ -560,6 +561,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
|
private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
|
||||||
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
|
private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
|
||||||
*/
|
*/
|
||||||
|
//if(frcount == 0) Console.WriteLine("MoveAngular ");
|
||||||
|
|
||||||
// Get what the body is doing, this includes 'external' influences
|
// Get what the body is doing, this includes 'external' influences
|
||||||
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
|
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
|
||||||
|
@ -614,7 +616,7 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
// Error is 0 (no error) to +/- 2 (max error)
|
// Error is 0 (no error) to +/- 2 (max error)
|
||||||
// scale it by VAservo
|
// scale it by VAservo
|
||||||
verterr = verterr * VAservo;
|
verterr = verterr * VAservo;
|
||||||
//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
|
if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
|
||||||
|
|
||||||
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
|
// As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
|
||||||
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
|
// 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
|
||||||
// Deflection section tba
|
// Deflection section tba
|
||||||
|
|
||||||
// Sum velocities
|
// Sum velocities
|
||||||
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
|
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // tba: + bank + deflection
|
||||||
|
|
||||||
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
|
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
|
||||||
{
|
{
|
||||||
|
|
|
@ -87,9 +87,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
private Quaternion m_APIDTarget = new Quaternion();
|
private Quaternion m_APIDTarget = new Quaternion();
|
||||||
private float m_APIDStrength = 0.5f;
|
private float m_APIDStrength = 0.5f;
|
||||||
private float m_APIDDamping = 0.5f;
|
private float m_APIDDamping = 0.5f;
|
||||||
|
|
||||||
private float APID_D = 35f;
|
|
||||||
private float APID_G = 25f;
|
|
||||||
private bool m_useAPID = false;
|
private bool m_useAPID = false;
|
||||||
|
|
||||||
// KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
|
// KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
|
||||||
|
@ -192,6 +189,9 @@ namespace OpenSim.Region.Physics.OdePlugin
|
||||||
private ODEDynamics m_vehicle;
|
private ODEDynamics m_vehicle;
|
||||||
|
|
||||||
internal int m_material = (int)Material.Wood;
|
internal int m_material = (int)Material.Wood;
|
||||||
|
|
||||||
|
private int frcount = 0; // Used to limit dynamics debug output to
|
||||||
|
|
||||||
|
|
||||||
public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
|
public OdePrim(String primName, OdeScene parent_scene, PhysicsVector pos, PhysicsVector size,
|
||||||
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
|
Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
|
||||||
|
@ -1563,9 +1563,14 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
float fy = 0;
|
float fy = 0;
|
||||||
float fz = 0;
|
float fz = 0;
|
||||||
|
|
||||||
|
frcount++; // used to limit debug comment output
|
||||||
|
if (frcount > 100)
|
||||||
|
frcount = 0;
|
||||||
|
|
||||||
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
|
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
|
||||||
{
|
{
|
||||||
|
if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
|
||||||
|
" usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
|
||||||
if (m_vehicle.Type != Vehicle.TYPE_NONE)
|
if (m_vehicle.Type != Vehicle.TYPE_NONE)
|
||||||
{
|
{
|
||||||
// 'VEHICLES' are dealt with in ODEDynamics.cs
|
// 'VEHICLES' are dealt with in ODEDynamics.cs
|
||||||
|
@ -1573,7 +1578,6 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
//Console.WriteLine("Move " + m_primName);
|
|
||||||
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
|
if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
|
||||||
// NON-'VEHICLES' are dealt with here
|
// NON-'VEHICLES' are dealt with here
|
||||||
if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
|
if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
|
||||||
|
@ -1602,7 +1606,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
|
|
||||||
if (m_usePID)
|
if (m_usePID)
|
||||||
{
|
{
|
||||||
//Console.WriteLine("PID " + m_primName);
|
//if(frcount == 0) Console.WriteLine("PID " + m_primName);
|
||||||
// KF - this is for object MoveToTarget.
|
// KF - this is for object MoveToTarget.
|
||||||
|
|
||||||
//if (!d.BodyIsEnabled(Body))
|
//if (!d.BodyIsEnabled(Body))
|
||||||
|
@ -1741,7 +1745,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
|
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
|
||||||
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
|
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
|
||||||
d.BodyAddForce(Body, 0, 0, fz);
|
d.BodyAddForce(Body, 0, 0, fz);
|
||||||
return;
|
//KF this prevents furthur motions return;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -1756,13 +1760,16 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
{
|
{
|
||||||
// RotLookAt, apparently overrides all other rotation sources. Inputs:
|
// RotLookAt, apparently overrides all other rotation sources. Inputs:
|
||||||
// Quaternion m_APIDTarget
|
// Quaternion m_APIDTarget
|
||||||
// float m_APIDStrength // perhaps ratio other forces to lookat force?
|
// float m_APIDStrength // From SL experiments, this is the time to get there
|
||||||
// float m_APIDDamping //'seconds to critically damps in'[sic]
|
// float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
|
||||||
|
// Also in SL the mass of the object has no effect on time to get there.
|
||||||
// Factors:
|
// Factors:
|
||||||
// float APID_D
|
//if(frcount == 0) Console.WriteLine("APID ");
|
||||||
// float APID_G
|
|
||||||
|
|
||||||
// get present body rotation
|
// get present body rotation
|
||||||
|
float limit = 1.0f;
|
||||||
|
float scaler = 50f; // adjusts damping time
|
||||||
|
float RLAservo = 0f;
|
||||||
|
|
||||||
d.Quaternion rot = d.BodyGetQuaternion(Body);
|
d.Quaternion rot = d.BodyGetQuaternion(Body);
|
||||||
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
|
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
|
||||||
Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
|
Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
|
||||||
|
@ -1770,15 +1777,21 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
Vector3 diff_axis;
|
Vector3 diff_axis;
|
||||||
rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
|
rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
|
||||||
diff_axis.Normalize();
|
diff_axis.Normalize();
|
||||||
PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
|
if(diff_angle > 0.01f) // diff_angle is always +ve
|
||||||
float RLAservo = timestep / m_APIDDamping;
|
{
|
||||||
rotforce = rotforce * RLAservo * m_mass;
|
// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
|
||||||
d.BodyAddTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
|
Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
|
||||||
|
rotforce = rotforce * rotq;
|
||||||
// d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
|
if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
|
||||||
|
// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
|
||||||
|
// rotforce = rotforce * RLAservo * diff_angle ;
|
||||||
|
// d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
|
||||||
|
RLAservo = timestep / m_APIDStrength * scaler;
|
||||||
|
rotforce = rotforce * RLAservo * diff_angle ;
|
||||||
|
d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
|
||||||
|
//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
|
||||||
|
}
|
||||||
|
if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
|
||||||
} // end m_useAPID
|
} // end m_useAPID
|
||||||
|
|
||||||
fx *= m_mass;
|
fx *= m_mass;
|
||||||
|
@ -2674,7 +2687,7 @@ Console.WriteLine(" JointCreateFixed");
|
||||||
//outofBounds = true;
|
//outofBounds = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
|
// float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
|
||||||
//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
|
//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
|
||||||
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
|
if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
|
||||||
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
|
&& (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
|
||||||
|
|
Loading…
Reference in New Issue